├── 3rd_party ├── CMakeLists.txt ├── Graclus │ ├── metisLib │ │ ├── metis.c │ │ ├── metis.h │ │ ├── Makefile │ │ ├── bucketsort.c │ │ ├── mrefine2.c │ │ ├── timing.c │ │ ├── mcoarsen.c │ │ ├── coarsen.c │ │ ├── mutil.c │ │ └── fortran.c │ └── CMakeLists.txt └── Spectra │ ├── Util │ ├── Version.h │ ├── GEigsMode.h │ ├── CompInfo.h │ ├── TypeTraits.h │ └── SimpleRandom.h │ ├── CMakeLists.txt │ ├── MatOp │ ├── internal │ │ ├── SymGEigsRegInvOp.h │ │ ├── SymGEigsCholeskyOp.h │ │ ├── SymGEigsBucklingOp.h │ │ ├── SymGEigsShiftInvertOp.h │ │ └── SymGEigsCayleyOp.h │ ├── DenseGenRealShiftSolve.h │ ├── DenseGenMatProd.h │ ├── DenseSymMatProd.h │ ├── SparseGenMatProd.h │ └── DenseSymShiftSolve.h │ ├── DavidsonSymEigsSolver.h │ ├── LinAlg │ └── SearchSpace.h │ └── GenEigsRealShiftSolver.h ├── docs ├── pumpkin.png ├── Sri_Mariamman.png └── Alcatraz_courtyard.png ├── .gitignore ├── src ├── utils │ ├── random.inl │ └── CMakeLists.txt ├── geometry │ ├── align_point_clouds.h │ ├── CMakeLists.txt │ ├── track_builder.h │ ├── rotation.h │ ├── align_point_clouds.cc │ └── rotation_test.cc ├── math │ ├── CMakeLists.txt │ ├── matrix_square_root.h │ └── matrix_square_root.cc ├── test │ ├── CMakeLists.txt │ └── view_graph_generator_test.cc ├── translation_averaging │ ├── linear_position_estimator.h │ └── CMakeLists.txt ├── solver │ ├── CMakeLists.txt │ ├── summary.h │ ├── solver_options.h │ ├── sdp_solver.h │ └── rbr_sdp_solver.h ├── CMakeLists.txt ├── graph │ ├── CMakeLists.txt │ ├── union_find.h │ ├── view_graph_test.cc │ ├── node.h │ ├── edge.h │ ├── union_find.cc │ ├── graph_cut.h │ ├── view_graph.h │ └── graph_cut_test.cc └── rotation_averaging │ ├── CMakeLists.txt │ ├── robust_l1l2_rotation_estimator.h │ └── hybrid_rotation_estimator.h ├── .travis.yml ├── cmake ├── FindGtest.cmake ├── FindGFlags.cmake ├── CMakeConfig.cmake.in ├── CMakeHelper.cmake └── FindGlog.cmake ├── LICENSE ├── scripts └── dependencies.sh ├── examples ├── CMakeLists.txt ├── position_estimator.cc ├── view_graph_synthesizer.cc └── rotation_estimator.cc ├── applications ├── utils.h ├── run_global_sfm.sh.in ├── CMakeLists.txt └── utils.cc └── data └── synthetic ├── 20_5.g2o └── 20_2.g2o /3rd_party/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(Graclus) -------------------------------------------------------------------------------- /docs/pumpkin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AIBluefisher/GraphOptim/HEAD/docs/pumpkin.png -------------------------------------------------------------------------------- /docs/Sri_Mariamman.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AIBluefisher/GraphOptim/HEAD/docs/Sri_Mariamman.png -------------------------------------------------------------------------------- /docs/Alcatraz_courtyard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AIBluefisher/GraphOptim/HEAD/docs/Alcatraz_courtyard.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | lib 3 | bin 4 | 5 | .vscode 6 | 7 | applications/build 8 | applications/run_global_sfm.sh 9 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/metis.c: -------------------------------------------------------------------------------- 1 | int boundary_points = 0; 2 | int spectral_initialization = 0; 3 | int cutType = 0; 4 | int memory_saving = 0; 5 | -------------------------------------------------------------------------------- /src/utils/random.inl: -------------------------------------------------------------------------------- 1 | #include "utils/random.h" 2 | 3 | #include 4 | 5 | namespace gopt { 6 | 7 | template 8 | void RandomNumberGenerator::RandShuffle(std::vector* vec) { 9 | std::random_shuffle((*vec).begin(), (*vec).end()); 10 | } 11 | 12 | } // namespace gopt 13 | -------------------------------------------------------------------------------- /3rd_party/Graclus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( 2 | . 3 | ) 4 | 5 | file(GLOB GRACLUS_HEADER_FILES "metisLib/*.h") 6 | file(GLOB GRACLUS_SOURCE_FILES "metisLib/*.c") 7 | 8 | OPTIMIZER_ADD_LIBRARY(graclus 9 | ${GRACLUS_HEADER_FILES} 10 | ${GRACLUS_SOURCE_FILES} 11 | multilevelLib/wkkm.c 12 | multilevelLib/mlkkm.c 13 | ) 14 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | sudo: required 3 | compiler: 4 | - gcc 5 | - g++ 6 | dist: bionic # xenial 7 | os: 8 | - linux 9 | before_install: 10 | - echo $LANG 11 | - echo $LC_ALL 12 | - if [ $TRAVIS_OS_NAME == linux ]; then ./scripts/dependencies.sh; fi 13 | script: 14 | - if [ $TRAVIS_OS_NAME == linux ]; then ./build.sh ; fi 15 | install: skip 16 | branches: 17 | only: 18 | - main 19 | notifications: 20 | email: false -------------------------------------------------------------------------------- /3rd_party/Spectra/Util/Version.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2020-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_VERSION_H 8 | #define SPECTRA_VERSION_H 9 | 10 | #define SPECTRA_MAJOR_VERSION 1 11 | #define SPECTRA_MINOR_VERSION 0 12 | #define SPECTRA_PATCH_VERSION 1 13 | 14 | #define SPECTRA_VERSION (SPECTRA_MAJOR_VERSION * 10000 + SPECTRA_MINOR_VERSION * 100 + SPECTRA_PATCH_VERSION) 15 | 16 | #endif // SPECTRA_VERSION_H 17 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/metis.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1997, Regents of the University of Minnesota 3 | * 4 | * metis.h 5 | * 6 | * This file includes all necessary header files 7 | * 8 | * Started 8/27/94 9 | * George 10 | * 11 | * $Id: metis.h,v 1.1 1998/11/27 17:59:21 karypis Exp $ 12 | */ 13 | 14 | 15 | #include 16 | #ifdef __STDC__ 17 | #include 18 | #else 19 | #include 20 | #endif 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #ifdef DMALLOC 28 | #include 29 | #endif 30 | 31 | #include "defs.h" 32 | #include "struct.h" 33 | #include "macros.h" 34 | #include "proto.h" 35 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/Makefile: -------------------------------------------------------------------------------- 1 | include ../Makefile.in 2 | 3 | INCLUDES = -I. 4 | 5 | CFLAGS = $(COPTIONS) $(OPTFLAGS) $(INCLUDES) 6 | LD = $(CC) -L. 7 | 8 | 9 | OBJS = coarsen.o fm.o match.o ccgraph.o memory.o \ 10 | pmetis.o pqueue.o refine.o util.o timing.o debug.o \ 11 | bucketsort.o graph.o stat.o kmetis.o kwayrefine.o \ 12 | kwayfm.o balance.o ometis.o srefine.o sfm.o separator.o \ 13 | mincover.o mmd.o mesh.o meshpart.o frename.o fortran.o \ 14 | myqsort.o compress.o parmetis.o estmem.o \ 15 | mpmetis.o mcoarsen.o mmatch.o minitpart.o mbalance.o \ 16 | mrefine.o mutil.o mfm.o mkmetis.o mkwayrefine.o mkwayfmh.o \ 17 | mrefine2.o minitpart2.o mbalance2.o mfm2.o \ 18 | kvmetis.o kwayvolrefine.o kwayvolfm.o subdomains.o initpart.o 19 | 20 | .c.o: 21 | $(CC) $(CFLAGS) -c $*.c 22 | 23 | ../libmetis.a: $(OBJS) 24 | $(AR) $@ $(OBJS) 25 | $(RANLIB) $@ 26 | 27 | clean: 28 | rm -f *.o 29 | 30 | realclean: 31 | rm -f *.o ; rm -f ../libmetis.a 32 | -------------------------------------------------------------------------------- /3rd_party/Spectra/Util/GEigsMode.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_GEIGS_MODE_H 8 | #define SPECTRA_GEIGS_MODE_H 9 | 10 | namespace Spectra { 11 | 12 | /// 13 | /// \ingroup Enumerations 14 | /// 15 | /// The enumeration to specify the mode of generalized eigenvalue solver. 16 | /// 17 | enum class GEigsMode 18 | { 19 | Cholesky, ///< Using Cholesky decomposition to solve generalized eigenvalues. 20 | RegularInverse, ///< Regular inverse mode for generalized eigenvalue solver. 21 | ShiftInvert, ///< Shift-and-invert mode for generalized eigenvalue solver. 22 | Buckling, ///< Buckling mode for generalized eigenvalue solver. 23 | Cayley ///< Cayley transformation mode for generalized eigenvalue solver. 24 | }; 25 | 26 | } // namespace Spectra 27 | 28 | #endif // SPECTRA_GEIGS_MODE_H 29 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/bucketsort.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1997, Regents of the University of Minnesota 3 | * 4 | * bucketsort.c 5 | * 6 | * This file contains code that implement a variety of counting sorting 7 | * algorithms 8 | * 9 | * Started 7/25/97 10 | * George 11 | * 12 | * $Id: bucketsort.c,v 1.1 1998/11/27 17:59:11 karypis Exp $ 13 | * 14 | */ 15 | 16 | #include "metis.h" 17 | 18 | 19 | 20 | /************************************************************************* 21 | * This function uses simple counting sort to return a permutation array 22 | * corresponding to the sorted order. The keys are assumed to start from 23 | * 0 and they are positive. This sorting is used during matching. 24 | **************************************************************************/ 25 | void BucketSortKeysInc(int n, int max, idxtype *keys, idxtype *tperm, idxtype *perm) 26 | { 27 | int i, ii; 28 | idxtype *counts; 29 | 30 | counts = idxsmalloc(max+2, 0, "BucketSortKeysInc: counts"); 31 | 32 | for (i=0; i 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_COMP_INFO_H 8 | #define SPECTRA_COMP_INFO_H 9 | 10 | namespace Spectra { 11 | 12 | /// 13 | /// \ingroup Enumerations 14 | /// 15 | /// The enumeration to report the status of computation. 16 | /// 17 | enum class CompInfo 18 | { 19 | Successful, ///< Computation was successful. 20 | 21 | NotComputed, ///< Used in eigen solvers, indicating that computation 22 | ///< has not been conducted. Users should call 23 | ///< the `compute()` member function of solvers. 24 | 25 | NotConverging, ///< Used in eigen solvers, indicating that some eigenvalues 26 | ///< did not converge. The `compute()` 27 | ///< function returns the number of converged eigenvalues. 28 | 29 | NumericalIssue ///< Used in various matrix factorization classes, for example in 30 | ///< Cholesky decomposition it indicates that the 31 | ///< matrix is not positive definite. 32 | }; 33 | 34 | } // namespace Spectra 35 | 36 | #endif // SPECTRA_COMP_INFO_H 37 | -------------------------------------------------------------------------------- /src/geometry/align_point_clouds.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_ALIGN_POINT_CLOUDS_H_ 2 | #define GEOMETRY_ALIGN_POINT_CLOUDS_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace gopt { 8 | 9 | // Computes the orientation, position, and scale factor for the transformation 10 | // between two corresponding 3D point sets A and B such as they are related by: 11 | // 12 | // B = s * R * A + t 13 | // 14 | // where A is "left" and B is "right". Implementation is based on the paper by 15 | // Umeyama "Least-squares estimation of transformation parameters between two 16 | // point patterns". 17 | void AlignPointCloudsUmeyama(const std::vector& left, 18 | const std::vector& right, 19 | Eigen::Matrix3d* rotation, 20 | Eigen::Vector3d* translation, double* scale); 21 | 22 | // Adds weights for each match 23 | // The previous objective function E = Sum(|| yi - (c.R.xi + T) ||^2) becomes 24 | // Sum(wi * || yi - (c.R.xi + T) ||^2) 25 | // The weights must be positive 26 | void AlignPointCloudsUmeyamaWithWeights( 27 | const std::vector& left, 28 | const std::vector& right, 29 | const std::vector& weights, Eigen::Matrix3d* rotation, 30 | Eigen::Vector3d* translation, double* scale); 31 | 32 | } // namespace gopt 33 | 34 | #endif // GEOMETRY_ALIGN_POINT_CLOUDS_H_ 35 | -------------------------------------------------------------------------------- /cmake/FindGtest.cmake: -------------------------------------------------------------------------------- 1 | # message("-- Check for gtest") 2 | 3 | # unset(GTEST_FOUND) 4 | # unset(GTEST_INCLUDE_DIRS) 5 | # unset(GTEST_LIBRARIES) 6 | 7 | # include(FindPackageHandleStandardArgs) 8 | 9 | # list(APPEND GTEST_CHECK_INCLUDE_DIRS 10 | # /usr/local/include 11 | # /usr/local/homebrew/include 12 | # /opt/local/var/macports/software 13 | # /opt/local/include 14 | # /usr/include) 15 | # list(APPEND GTEST_CHECK_PATH_SUFFIXES 16 | # gtest/include 17 | # gtest/Include 18 | # Glog/include 19 | # Glog/Include 20 | # src/windows) 21 | 22 | # list(APPEND GTEST_CHECK_LIBRARY_DIRS 23 | # /usr/local/lib 24 | # /usr/local/homebrew/lib 25 | # /opt/local/lib 26 | # /usr/lib) 27 | # list(APPEND GTEST_CHECK_LIBRARY_SUFFIXES 28 | # gtest/lib 29 | # gtest/Lib 30 | # Glog/lib 31 | # Glog/Lib 32 | # x64/Release) 33 | 34 | # find_path(GTEST_INCLUDE_DIRS 35 | # NAMES gtest/gtest.h 36 | # PATHS ${GTEST_CHECK_INCLUDE_DIRS} 37 | # PATH_SUFFIXES ${GTEST_CHECK_PATH_SUFFIXES}) 38 | 39 | # find_library(GTEST_LIBRARIES 40 | # NAMES gtest libgtest gtest_main libgtest_main 41 | # PATHS ${GTEST_CHECK_LIBRARY_DIRS} 42 | # PATH_SUFFIXES ${GTEST_CHECK_LIBRARY_SUFFIXES}) 43 | 44 | # if (GTEST_INCLUDE_DIRS AND GTEST_LIBRARIES) 45 | # set(GTEST_FOUND TRUE) 46 | # message(STATUS "Found Gtest") 47 | # message(STATUS " Includes: ${GTEST_INCLUDE_DIRS}") 48 | # message(STATUS " Libraries: ${GTEST_LIBRARIES}") 49 | # else() 50 | # if (Gtest_FIND_REQUIRED) 51 | # message(FATAL_ERROR "Could not find Gtest") 52 | # endif() 53 | # endif() 54 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Chenyu 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /scripts/dependencies.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo apt-get update 4 | 5 | sudo apt-get install git cmake build-essential \ 6 | libgoogle-glog-dev libgflags-dev libgtest-dev libeigen3-dev 7 | 8 | mkdir tmp && cd tmp 9 | 10 | # Install Ceres-Solver 11 | sudo apt-get install libatlas-base-dev libsuitesparse-dev 12 | git clone https://ceres-solver.googlesource.com/ceres-solver 13 | cd ceres-solver 14 | # Latest release of ceres not provides FindEigen3.cmake 15 | git checkout 1.14.x 16 | mkdir build && cd build 17 | cmake .. -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF 18 | make -j8 19 | sudo make install 20 | cd ../../ 21 | 22 | # Install COLMAP 23 | sudo apt-get install \ 24 | git \ 25 | cmake \ 26 | build-essential \ 27 | libboost-program-options-dev \ 28 | libboost-filesystem-dev \ 29 | libboost-graph-dev \ 30 | libboost-system-dev \ 31 | libboost-test-dev \ 32 | libsuitesparse-dev \ 33 | libfreeimage-dev \ 34 | libmetis-dev \ 35 | libgoogle-glog-dev \ 36 | libgflags-dev \ 37 | libglew-dev \ 38 | qtbase5-dev \ 39 | libqt5opengl5-dev \ 40 | libcgal-dev 41 | 42 | sudo apt-get install libcgal-qt5-dev 43 | 44 | git clone https://github.com/AIBluefisher/colmap.git 45 | cd colmap 46 | git checkout dev 47 | mkdir build 48 | cd build 49 | cmake .. 50 | make -j 51 | sudo make install 52 | cd ../../ 53 | 54 | cd .. 55 | rm -rf tmp 56 | 57 | if [ -d "/usr/src/gtest" ]; then 58 | cd /usr/src/gtest 59 | sudo mkdir build && cd build 60 | sudo cmake .. && sudo make 61 | sudo cp libgtest* /usr/lib/ 62 | cd ../ && sudo rm -rf build 63 | else 64 | echo "Could not find gtest" 65 | fi 66 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/mrefine2.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1997, Regents of the University of Minnesota 3 | * 4 | * mrefine2.c 5 | * 6 | * This file contains the driving routines for multilevel refinement 7 | * 8 | * Started 7/24/97 9 | * George 10 | * 11 | * $Id: mrefine2.c,v 1.1 1998/11/27 17:59:26 karypis Exp $ 12 | */ 13 | 14 | #include "metis.h" 15 | 16 | 17 | /************************************************************************* 18 | * This function is the entry point of refinement 19 | **************************************************************************/ 20 | void MocRefine2Way2(CtrlType *ctrl, GraphType *orggraph, GraphType *graph, float *tpwgts, 21 | float *ubvec) 22 | { 23 | 24 | IFSET(ctrl->dbglvl, DBG_TIME, starttimer(ctrl->UncoarsenTmr)); 25 | 26 | /* Compute the parameters of the coarsest graph */ 27 | MocCompute2WayPartitionParams(ctrl, graph); 28 | 29 | for (;;) { 30 | ASSERT(CheckBnd(graph)); 31 | 32 | IFSET(ctrl->dbglvl, DBG_TIME, starttimer(ctrl->RefTmr)); 33 | switch (ctrl->RType) { 34 | case RTYPE_FM: 35 | MocBalance2Way2(ctrl, graph, tpwgts, ubvec); 36 | MocFM_2WayEdgeRefine2(ctrl, graph, tpwgts, ubvec, 8); 37 | break; 38 | default: 39 | graclus_errexit("Unknown refinement type: %d\n", ctrl->RType); 40 | } 41 | IFSET(ctrl->dbglvl, DBG_TIME, stoptimer(ctrl->RefTmr)); 42 | 43 | if (graph == orggraph) 44 | break; 45 | 46 | graph = graph->finer; 47 | IFSET(ctrl->dbglvl, DBG_TIME, starttimer(ctrl->ProjectTmr)); 48 | MocProject2WayPartition(ctrl, graph); 49 | IFSET(ctrl->dbglvl, DBG_TIME, stoptimer(ctrl->ProjectTmr)); 50 | } 51 | 52 | IFSET(ctrl->dbglvl, DBG_TIME, stoptimer(ctrl->UncoarsenTmr)); 53 | } 54 | 55 | 56 | -------------------------------------------------------------------------------- /cmake/FindGFlags.cmake: -------------------------------------------------------------------------------- 1 | set(GFLAGS_INCLUDE_DIR_HINTS "" CACHE PATH "Glog include directory") 2 | set(GFLAGS_LIBRARY_DIR_HINTS "" CACHE PATH "Glog library directory") 3 | 4 | unset(GFLAGS_FOUND) 5 | unset(GFLAGS_INCLUDE_DIRS) 6 | unset(GFLAGS_LIBRARIES) 7 | 8 | include(FindPackageHandleStandardArgs) 9 | 10 | list(APPEND GFLAGS_CHECK_INCLUDE_DIRS 11 | /usr/local/include 12 | /usr/local/homebrew/include 13 | /opt/local/var/macports/software 14 | /opt/local/include 15 | /usr/include) 16 | list(APPEND GFLAGS_CHECK_PATH_SUFFIXES 17 | gflags/include 18 | gflags/Include 19 | Gflags/include 20 | Gflags/Include 21 | src/windows) 22 | 23 | list(APPEND GFLAGS_CHECK_LIBRARY_DIRS 24 | /usr/local/lib 25 | /usr/local/homebrew/lib 26 | /opt/local/lib 27 | /usr/lib) 28 | list(APPEND GFLAGS_CHECK_LIBRARY_SUFFIXES 29 | gflags/lib 30 | gflags/Lib 31 | Gflags/lib 32 | Gflags/Lib 33 | x64/Release) 34 | 35 | find_path(GFLAGS_INCLUDE_DIRS 36 | NAMES 37 | gflags/gflags.h 38 | PATHS 39 | ${GFLAGS_INCLUDE_DIR_HINTS} 40 | ${GFLAGS_CHECK_INCLUDE_DIRS} 41 | PATH_SUFFIXES 42 | ${GFLAGS_CHECK_PATH_SUFFIXES}) 43 | find_library(GFLAGS_LIBRARIES 44 | NAMES 45 | gflags 46 | libgflags 47 | PATHS 48 | ${GFLAGS_LIBRARY_DIR_HINTS} 49 | ${GFLAGS_CHECK_LIBRARY_DIRS} 50 | PATH_SUFFIXES 51 | ${GFLAGS_CHECK_LIBRARY_SUFFIXES}) 52 | 53 | if (GFLAGS_INCLUDE_DIRS AND GFLAGS_LIBRARIES) 54 | set(GFLAGS_FOUND TRUE) 55 | message(STATUS "Found GFlags") 56 | message(STATUS " Includes : ${GFLAGS_INCLUDE_DIRS}") 57 | message(STATUS " Libraries : ${GFLAGS_LIBRARIES}") 58 | else() 59 | if(GFlags_FIND_REQUIRED) 60 | message(FATAL_ERROR "Could not find Glog") 61 | endif() 62 | endif() 63 | -------------------------------------------------------------------------------- /src/math/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | distribution.h 33 | matrix_square_root.h sparse_cholesky_llt.h) 34 | 35 | OPTIMIZER_ADD_SOURCES( 36 | matrix_square_root.cc sparse_cholesky_llt.cc) 37 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | 32 | OPTIMIZER_ADD_EXE(position_estimator position_estimator.cc) 33 | OPTIMIZER_ADD_EXE(rotation_estimator rotation_estimator.cc) 34 | OPTIMIZER_ADD_EXE(view_graph_synthesizer view_graph_synthesizer.cc) 35 | -------------------------------------------------------------------------------- /src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | alignment.h 33 | hash.h 34 | progressbar.h 35 | random.h 36 | timer.h 37 | threading.h 38 | types.h) 39 | 40 | OPTIMIZER_ADD_SOURCES( 41 | random.cc 42 | timer.cc 43 | threading.cc) 44 | -------------------------------------------------------------------------------- /src/geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | align_point_clouds.h 33 | rotation.h 34 | rotation_utils.h 35 | track_builder.h) 36 | 37 | OPTIMIZER_ADD_SOURCES( 38 | align_point_clouds.cc 39 | rotation.cc 40 | rotation_utils.cc 41 | track_builder.cc) -------------------------------------------------------------------------------- /src/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | view_graph_generator.h 33 | ) 34 | 35 | OPTIMIZER_ADD_SOURCES( 36 | view_graph_generator.cc 37 | ) 38 | 39 | if(TESTS_ENABLED) 40 | OPTIMIZER_ADD_GTEST(view_graph_generator_test view_graph_generator_test.cc) 41 | endif() -------------------------------------------------------------------------------- /src/translation_averaging/linear_position_estimator.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSLATION_AVERAGING_H_ 2 | #define TRANSLATION_AVERAGING_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "geometry/track_builder.h" 10 | #include "translation_averaging/position_estimator.h" 11 | 12 | namespace gopt { 13 | 14 | class LinearPositionEstimator : public PositionEstimator { 15 | public: 16 | LinearPositionEstimator( 17 | const std::vector& tracks, 18 | const std::unordered_map& normalized_keypoints); 19 | 20 | bool EstimatePositions( 21 | const std::unordered_map& view_pairs, 22 | const std::unordered_map& orientation, 23 | std::unordered_map* positions) override; 24 | 25 | private: 26 | void InitializeIndexMapping( 27 | const std::unordered_map& orientations); 28 | 29 | Eigen::MatrixXd SetUpLinearSystem(Eigen::MatrixXd* A_lr); 30 | 31 | void SelectLeftRightBaseViews( 32 | const TrackElements& track_elements, 33 | TrackElement* track_element1, 34 | TrackElement* track_element2); 35 | 36 | Eigen::Vector3d Theta12(const Eigen::Matrix3d& R12, 37 | const Eigen::Vector3d& X1, 38 | const Eigen::Vector3d& X2); 39 | 40 | const Eigen::Matrix A12(const Eigen::Vector3d& theta12, 41 | const Eigen::Vector3d& X2); 42 | 43 | void IdentifySign(const Eigen::MatrixXd& A_lr, 44 | Eigen::VectorXd* eigen_vectors); 45 | 46 | static const int kConstantImageIndex = -3; 47 | std::unordered_map image_id_to_index_; 48 | 49 | std::unordered_map rotations_; 50 | const std::vector tracks_; 51 | const std::unordered_map normalized_keypoints_; 52 | }; 53 | 54 | } // namespace gopt 55 | 56 | #endif // TRANSLATION_AVERAGING_H_ 57 | -------------------------------------------------------------------------------- /src/translation_averaging/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | position_estimator.h 33 | linear_position_estimator.h 34 | lud_position_estimator.h 35 | ) 36 | 37 | OPTIMIZER_ADD_SOURCES( 38 | linear_position_estimator.cc 39 | lud_position_estimator.cc 40 | ) 41 | 42 | if(TESTS_ENABLED) 43 | OPTIMIZER_ADD_GTEST(lud_position_estimator_test 44 | lud_position_estimator_test.cc) 45 | endif() 46 | -------------------------------------------------------------------------------- /src/test/view_graph_generator_test.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "test/view_graph_generator.h" 32 | 33 | #include "gtest/gtest.h" 34 | 35 | namespace gopt { 36 | 37 | TEST(VIEW_GRAPH_GENERATOR_TEST, TEST_GENERATE_SINGLE_VIEW_GRAPH) { 38 | ViewGraphGenerator::ViewGraphGeneratorOptions options; 39 | options.num_scenes = 8; 40 | ViewGraphGenerator generator(options); 41 | generator.Run(); 42 | } 43 | 44 | } // namespace gopt 45 | -------------------------------------------------------------------------------- /src/math/matrix_square_root.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef MATH_MATRIX_SQUARE_ROOT_H_ 32 | #define MATH_MATRIX_SQUARE_ROOT_H_ 33 | 34 | #include 35 | #include 36 | 37 | namespace gopt { 38 | 39 | Eigen::MatrixXd MatrixSquareRoot(const Eigen::MatrixXd& mat); 40 | 41 | Eigen::MatrixXd MatrixSquareRootForSemidefinitePositiveMat( 42 | const Eigen::MatrixXd& mat); 43 | 44 | } // namespace gopt 45 | 46 | #endif // MATH_MATRIX_SQUARE_ROOT_H_ 47 | -------------------------------------------------------------------------------- /applications/utils.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2022, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef APPLICATIONS_UTILS_H_ 32 | #define APPLICATIONS_UTILS_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace gopt { 40 | 41 | void LoadTracksFromDB( 42 | const std::string& database_path, 43 | TrackElements* track_elements, 44 | std::vector>* track_element_pairs); 45 | 46 | } 47 | 48 | #endif // APPLICATIONS_UTILS_H_ 49 | -------------------------------------------------------------------------------- /src/solver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | bcm_sdp_solver.h 33 | constrained_l1_solver.h 34 | l1_solver.h 35 | rank_restricted_sdp_solver.h 36 | rbr_sdp_solver.h 37 | riemannian_staircase.h 38 | sdp_solver.h 39 | solver_options.h 40 | summary.h) 41 | 42 | OPTIMIZER_ADD_SOURCES( 43 | constrained_l1_solver.cc 44 | rank_restricted_sdp_solver.cc 45 | rbr_sdp_solver.cc 46 | riemannian_staircase.cc) 47 | 48 | if(TESTS_ENABLED) 49 | OPTIMIZER_ADD_GTEST(l1_solver_test l1_solver_test.cc) 50 | endif() -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | add_subdirectory(geometry) 32 | add_subdirectory(graph) 33 | add_subdirectory(math) 34 | add_subdirectory(solver) 35 | add_subdirectory(rotation_averaging) 36 | add_subdirectory(utils) 37 | add_subdirectory(translation_averaging) 38 | add_subdirectory(test) 39 | 40 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 41 | 42 | OPTIMIZER_ADD_LIBRARY(${GOPT_LIB} 43 | ${GRAPH_OPTIMIZER_HEADERS} 44 | ${GRAPH_OPTIMIZER_SOURCES} 45 | ) 46 | target_link_libraries(${GOPT_LIB} 47 | ${GOPT_EXTERNAL_LIBRARIES} 48 | ${GOPT_INTERNAL_LIBRARIES} 49 | ) 50 | -------------------------------------------------------------------------------- /src/graph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | color_gradient.h 33 | edge.h 34 | graph_cut.h 35 | graph.h 36 | node.h 37 | union_find.h 38 | view_graph.h 39 | svg_drawer.h) 40 | 41 | OPTIMIZER_ADD_SOURCES( 42 | graph_cut.cc 43 | graph.inl 44 | union_find.cc 45 | view_graph.cc) 46 | 47 | if(TESTS_ENABLED) 48 | OPTIMIZER_ADD_GTEST(union_find_test union_find_test.cc) 49 | OPTIMIZER_ADD_GTEST(graph_test graph_test.cc) 50 | OPTIMIZER_ADD_GTEST(graph_cut_test graph_cut_test.cc) 51 | OPTIMIZER_ADD_GTEST(view_graph_test view_graph_test.cc) 52 | endif() 53 | -------------------------------------------------------------------------------- /src/geometry/track_builder.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_TRACK_BUILDER_H_ 2 | #define GEOMETRY_TRACK_BUILDER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "utils/types.h" 8 | 9 | namespace gopt { 10 | 11 | // Track class stores all observations of a 3D point. 12 | struct TrackElement { 13 | TrackElement() 14 | : image_id(kInvalidImageId), point2D_idx(kInvalidPoint2DIdx) {} 15 | 16 | TrackElement(const image_t image_id, const point2D_t point2D_idx) 17 | : image_id(image_id), point2D_idx(point2D_idx) {} 18 | 19 | // The image in which the track element is observed. 20 | image_t image_id; 21 | 22 | // The point in the image that the track element is observed. 23 | point2D_t point2D_idx; 24 | }; 25 | 26 | typedef std::vector TrackElements; 27 | 28 | // Build tracks from feature correspondences across multiple images. Tracks are 29 | // created with the connected components algorithm and have a maximum allowable 30 | // size. If there are multiple features from one image in a track, we do not do 31 | // any intelligent selection and just arbitrarily choose a feature to drop so 32 | // that the tracks are consistent. 33 | class TrackBuilder { 34 | public: 35 | TrackBuilder(const size_t min_track_length, const size_t max_track_length); 36 | 37 | // Build tracks for a given series of track elements. 38 | void Build(const std::vector& track_elements, 39 | const std::vector>& pair_ids); 40 | 41 | // Remove bad tracks that are too short or have ids collision. 42 | bool Filter(); 43 | 44 | // Return the number of connected set in the Union Find structure. 45 | size_t NumTracks() const; 46 | 47 | // Extract consistent tracks. 48 | const std::unordered_map& GetConsistentTracks() const; 49 | std::unordered_map& GetConsistentTracks(); 50 | 51 | double MeanTrackLength() const; 52 | 53 | bool WriteToFile(const std::string& filename); 54 | 55 | private: 56 | const size_t min_track_length_; 57 | const size_t max_track_length_; 58 | 59 | std::unordered_map consistent_tracks_; 60 | }; 61 | 62 | } // namespace gopt 63 | 64 | #endif // GEOMETRY_TRACK_BUILDER_H_ 65 | -------------------------------------------------------------------------------- /applications/run_global_sfm.sh.in: -------------------------------------------------------------------------------- 1 | DATASET_PATH=$1 2 | OUTPUT_PATH=$2 3 | VOC_TREE_PATH=$3 4 | MOST_SIMILAR_IMAGES_NUM=$4 5 | 6 | ROTATION_ESTIMATOR_TYPE="ROBUST_L1L2" 7 | POSITION_ESTIMATOR_TYPE="LUD" 8 | OPTIMIZE_RELATIVE_TRANSLATIONS="true" 9 | FILTER_GLOBAL_TRANSLATIONS="true" 10 | FINAL_GLOBAL_BUNDLE="true" 11 | RUN_INCREMENTAL="false" 12 | 13 | if [ $# -ge 5 ]; then ROTATION_ESTIMATOR_TYPE=$5; fi 14 | 15 | if [ $# -ge 6 ]; then POSITION_ESTIMATOR_TYPE=$6; fi 16 | 17 | if [ $# -ge 7 ]; then OPTIMIZE_RELATIVE_TRANSLATIONS=$7; fi 18 | 19 | if [ $# -ge 8 ]; then FILTER_GLOBAL_TRANSLATIONS=$8; fi 20 | 21 | if [ $# -ge 9 ]; then FINAL_GLOBAL_BUNDLE=$9; fi 22 | 23 | if [ $# -ge 10 ]; then RUN_INCREMENTAL=$10; fi 24 | 25 | echo "\nRunning Config:" 26 | echo "\t-Rotation Estimator: $ROTATION_ESTIMATOR_TYPE" 27 | echo "\t-Position Estimator: $POSITION_ESTIMATOR_TYPE" 28 | echo "\t-Optimize Relative Translations: $OPTIMIZE_RELATIVE_TRANSLATIONS" 29 | echo "\t-Filter Global Translations: $FILTER_GLOBAL_TRANSLATIONS" 30 | echo "\t-Final Global Bundle: $FINAL_GLOBAL_BUNDLE" 31 | echo "\t-Run Incremental Mapper: $RUN_INCREMENTAL" 32 | 33 | mkdir -p $OUTPUT_PATH 34 | 35 | colmap feature_extractor \ 36 | --database_path=$OUTPUT_PATH/database.db \ 37 | --image_path=$DATASET_PATH/images \ 38 | --SiftExtraction.num_threads=8 \ 39 | --SiftExtraction.use_gpu=1 \ 40 | --SiftExtraction.gpu_index=0 41 | 42 | colmap vocab_tree_matcher \ 43 | --database_path=$OUTPUT_PATH/database.db \ 44 | --SiftMatching.num_threads=8 \ 45 | --SiftMatching.use_gpu=1 \ 46 | --SiftMatching.gpu_index=0 \ 47 | --VocabTreeMatching.num_images=$MOST_SIMILAR_IMAGES_NUM \ 48 | --VocabTreeMatching.num_nearest_neighbors=5 \ 49 | --VocabTreeMatching.vocab_tree_path=$VOC_TREE_PATH 50 | 51 | ${CMAKE_SOURCE_DIR}/../bin/run_global_mapper \ 52 | --database_path=$OUTPUT_PATH/database.db \ 53 | --image_path=$DATASET_PATH/images \ 54 | --output_path=$OUTPUT_PATH \ 55 | --rotation_estimator_type=$ROTATION_ESTIMATOR_TYPE \ 56 | --position_estimator_type=$POSITION_ESTIMATOR_TYPE \ 57 | --optimize_relative_translations=$OPTIMIZE_RELATIVE_TRANSLATIONS \ 58 | --filter_relative_translations=$FILTER_GLOBAL_TRANSLATIONS \ 59 | --final_global_bundle=$FINAL_GLOBAL_BUNDLE \ 60 | --run_incremental=$RUN_INCREMENTAL 61 | -------------------------------------------------------------------------------- /src/rotation_averaging/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2021, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | OPTIMIZER_ADD_HEADERS( 32 | hybrid_rotation_estimator.h 33 | irls_rotation_local_refiner.h 34 | l1_rotation_global_estimator.h 35 | lagrange_dual_rotation_estimator.h 36 | robust_l1l2_rotation_estimator.h) 37 | 38 | OPTIMIZER_ADD_SOURCES( 39 | hybrid_rotation_estimator.cc 40 | irls_rotation_local_refiner.cc 41 | l1_rotation_global_estimator.cc 42 | lagrange_dual_rotation_estimator.cc 43 | robust_l1l2_rotation_estimator.cc) 44 | 45 | if(TESTS_ENABLED) 46 | OPTIMIZER_ADD_GTEST(lagrange_dual_rotation_estimator_test 47 | lagrange_dual_rotation_estimator_test.cc) 48 | OPTIMIZER_ADD_GTEST(hybrid_rotation_estimator_test 49 | hybrid_rotation_estimator_test.cc) 50 | OPTIMIZER_ADD_GTEST(robust_l1l2_rotation_estimator_test 51 | robust_l1l2_rotation_estimator_test.cc) 52 | endif() 53 | -------------------------------------------------------------------------------- /3rd_party/Spectra/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # cmake_minimum_required (VERSION 3.5 FATAL_ERROR) 2 | # project (Spectra VERSION 0.8.1 LANGUAGES CXX) 3 | 4 | # list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) # make CMake look into the ./cmake/ folder for configuration files 5 | 6 | # # Supported options 7 | # # ----------------- 8 | # option(BUILD_TESTS "Build tests" OFF) 9 | 10 | 11 | # # Look for supporting libraries 12 | # # ----------------------------- 13 | # find_package(Eigen3 REQUIRED) 14 | 15 | # # Setup library 16 | # # ------------- 17 | 18 | # add_library(Spectra INTERFACE) 19 | # target_include_directories(Spectra 20 | # INTERFACE 21 | # $ 22 | # $ 23 | # ) 24 | # target_link_libraries(Spectra INTERFACE Eigen3::Eigen) 25 | 26 | # # Parse additional options 27 | # # ------------------------ 28 | 29 | # if(BUILD_TESTS) 30 | # enable_testing() 31 | # add_subdirectory(test) 32 | # endif() 33 | 34 | # # Install the library (relative to the CMAKE_INSTALL_PREFIX) 35 | # # ---------------------------------------------------------- 36 | 37 | # include(GNUInstallDirs) 38 | 39 | # install(TARGETS Spectra 40 | # EXPORT Spectra-targets 41 | # INCLUDES DESTINATION include 42 | # ) 43 | 44 | # install(DIRECTORY ${CMAKE_SOURCE_DIR}/include/ DESTINATION include) 45 | 46 | # install(EXPORT Spectra-targets 47 | # FILE Spectra-targets.cmake 48 | # NAMESPACE Spectra:: 49 | # DESTINATION cmake 50 | # ) 51 | 52 | # Configure package 53 | # ----------------- 54 | 55 | # include(CMakePackageConfigHelpers) 56 | 57 | # configure_package_config_file( 58 | # ${CMAKE_SOURCE_DIR}/cmake/spectra-config.cmake.in 59 | # ${CMAKE_BINARY_DIR}/cmake/spectra-config.cmake 60 | # INSTALL_DESTINATION cmake 61 | # ) 62 | 63 | # write_basic_package_version_file( 64 | # ${CMAKE_BINARY_DIR}/cmake/spectra-config-version.cmake 65 | # VERSION ${Spectra_VERSION} 66 | # COMPATIBILITY AnyNewerVersion 67 | # ) 68 | 69 | # install( 70 | # FILES 71 | # ${CMAKE_BINARY_DIR}/cmake/spectra-config.cmake 72 | # ${CMAKE_BINARY_DIR}/cmake/spectra-config-version.cmake 73 | # DESTINATION cmake 74 | # ) 75 | 76 | # add_library(Spectra INTERFACE) 77 | # target_include_directories(Spectra 78 | # INTERFACE 79 | # $ 80 | # $ 81 | # ) 82 | # target_link_libraries(Spectra INTERFACE Eigen3::Eigen) 83 | # find_package(Eigen3 REQUIRED) 84 | # file(GLOB_RECURSE SPECTRA_HEADER_FILES *.h contrib/*.h LinAlg/*.h MatOp/*.h Util/*.h) 85 | # COLMAP_ADD_SOURCES(SPECTRA_HEADER_FILES) -------------------------------------------------------------------------------- /src/geometry/rotation.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021, Chenyu 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 | // 1. Redistributions of source code must retain the above copyright notice, this 8 | // list of conditions and the following disclaimer. 9 | 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // 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 ARE 21 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef SRC_GEOMETRY_ROTATION_H_ 30 | #define SRC_GEOMETRY_ROTATION_H_ 31 | 32 | #include 33 | 34 | namespace gopt { 35 | 36 | Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& quat); 37 | 38 | Eigen::Vector3d RotationMatrixToAngleAxis(const Eigen::Matrix3d& R); 39 | Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d& R); 40 | 41 | Eigen::Matrix3d AngleAxisToRotationMatrix(const Eigen::Vector3d& angle_axis); 42 | Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d& quaternion); 43 | 44 | Eigen::Vector3d QuaternionToAngleAxis(const Eigen::Vector4d& quat); 45 | Eigen::Vector4d AngleAxisToQuaternion(const Eigen::Vector3d& angle_axis); 46 | 47 | double DotProduct(const Eigen::Vector3d& x, const Eigen::Vector3d& y); 48 | Eigen::Vector3d AngleAxisRotatePoint(const Eigen::Vector3d& angle_axis, 49 | const Eigen::Vector3d& pt); 50 | 51 | } // namespace gopt 52 | 53 | #endif // SRC_GEOMETRY_ROTATION_H_ 54 | -------------------------------------------------------------------------------- /examples/position_estimator.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "graph/view_graph.h" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include "utils/types.h" 39 | 40 | DEFINE_string(g2o_filename, "", "The absolute path of g2o file"); 41 | 42 | int main(int argc, char* argv[]) { 43 | gflags::ParseCommandLineFlags(&argc, &argv, false); 44 | 45 | google::InitGoogleLogging(argv[0]); 46 | FLAGS_alsologtostderr = true; 47 | FLAGS_colorlogtostderr = true; 48 | 49 | if (argc < 2) { 50 | LOG(INFO) << "[Usage]: position_estimator --g2o_filename=g2o_filename"; 51 | return 0; 52 | } 53 | 54 | std::string g2o_filename = FLAGS_g2o_filename; 55 | gopt::graph::ViewGraph view_graph; 56 | view_graph.ReadG2OFile(g2o_filename); 57 | 58 | gopt::PositionEstimatorOptions options; 59 | options.verbose = true; 60 | 61 | std::unordered_map global_positions; 62 | view_graph.TranslationAveraging(options, &global_positions); 63 | } 64 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/internal/SymGEigsRegInvOp.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2017-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SYM_GEIGS_REG_INV_OP_H 8 | #define SPECTRA_SYM_GEIGS_REG_INV_OP_H 9 | 10 | #include 11 | 12 | #include "../SparseSymMatProd.h" 13 | #include "../SparseRegularInverse.h" 14 | 15 | namespace Spectra { 16 | 17 | /// 18 | /// \ingroup Operators 19 | /// 20 | /// This class defines the matrix operation for generalized eigen solver in the 21 | /// regular inverse mode. This class is intended for internal use. 22 | /// 23 | template , 24 | typename BOpType = SparseRegularInverse> 25 | class SymGEigsRegInvOp 26 | { 27 | public: 28 | using Scalar = typename OpType::Scalar; 29 | 30 | private: 31 | using Index = Eigen::Index; 32 | using Vector = Eigen::Matrix; 33 | 34 | const OpType& m_op; 35 | const BOpType& m_Bop; 36 | mutable Vector m_cache; // temporary working space 37 | 38 | public: 39 | /// 40 | /// Constructor to create the matrix operation object. 41 | /// 42 | /// \param op The \f$A\f$ matrix operation object. 43 | /// \param Bop The \f$B\f$ matrix operation object. 44 | /// 45 | SymGEigsRegInvOp(const OpType& op, const BOpType& Bop) : 46 | m_op(op), m_Bop(Bop), m_cache(op.rows()) 47 | {} 48 | 49 | /// 50 | /// Move constructor. 51 | /// 52 | SymGEigsRegInvOp(SymGEigsRegInvOp&& other) : 53 | m_op(other.m_op), m_Bop(other.m_Bop) 54 | { 55 | // We emulate the move constructor for Vector using Vector::swap() 56 | m_cache.swap(other.m_cache); 57 | } 58 | 59 | /// 60 | /// Return the number of rows of the underlying matrix. 61 | /// 62 | Index rows() const { return m_Bop.rows(); } 63 | /// 64 | /// Return the number of columns of the underlying matrix. 65 | /// 66 | Index cols() const { return m_Bop.rows(); } 67 | 68 | /// 69 | /// Perform the matrix operation \f$y=B^{-1}Ax\f$. 70 | /// 71 | /// \param x_in Pointer to the \f$x\f$ vector. 72 | /// \param y_out Pointer to the \f$y\f$ vector. 73 | /// 74 | // y_out = inv(B) * A * x_in 75 | void perform_op(const Scalar* x_in, Scalar* y_out) const 76 | { 77 | m_op.perform_op(x_in, m_cache.data()); 78 | m_Bop.solve(m_cache.data(), y_out); 79 | } 80 | }; 81 | 82 | } // namespace Spectra 83 | 84 | #endif // SPECTRA_SYM_GEIGS_REG_INV_OP_H 85 | -------------------------------------------------------------------------------- /examples/view_graph_synthesizer.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "test/view_graph_generator.h" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include "utils/types.h" 39 | 40 | DEFINE_int32(num_threads, 8, "Maximum number of threads."); 41 | DEFINE_int32(num_scenes, 1250, "Total number of scenes to generate."); 42 | DEFINE_string(output_dir, "", "The directory to store the synthesized view graphs."); 43 | 44 | int main(int argc, char* argv[]) { 45 | gflags::ParseCommandLineFlags(&argc, &argv, false); 46 | 47 | google::InitGoogleLogging(argv[0]); 48 | FLAGS_alsologtostderr = true; 49 | FLAGS_colorlogtostderr = true; 50 | 51 | if (argc < 2) { 52 | LOG(INFO) << "[Usage]: view_graph_synthesizer --num_scenes= --output_dir="; 53 | return 0; 54 | } 55 | 56 | gopt::ViewGraphGenerator::ViewGraphGeneratorOptions options; 57 | options.num_threads = FLAGS_num_threads; 58 | options.verbose = false; 59 | options.num_scenes = FLAGS_num_scenes; 60 | options.output_path = FLAGS_output_dir; 61 | gopt::ViewGraphGenerator generator(options); 62 | generator.Run(); 63 | } 64 | -------------------------------------------------------------------------------- /src/math/matrix_square_root.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "math/matrix_square_root.h" 32 | 33 | #include 34 | #include 35 | 36 | namespace gopt { 37 | Eigen::MatrixXd MatrixSquareRoot(const Eigen::MatrixXd& mat) { 38 | // only square matrix can apply matrix square root 39 | CHECK_EQ(mat.rows(), mat.cols()); 40 | 41 | Eigen::EigenSolver solver(mat); 42 | Eigen::MatrixXd V = solver.eigenvectors().real(); 43 | Eigen::VectorXd Dv = solver.eigenvalues().real(); 44 | Eigen::MatrixXd sqrt_D = Dv.cwiseSqrt().asDiagonal(); 45 | 46 | return V * sqrt_D * V.inverse(); 47 | } 48 | 49 | Eigen::MatrixXd MatrixSquareRootForSemidefinitePositiveMat( 50 | const Eigen::MatrixXd& mat) { 51 | // only square matrix can apply matrix square root 52 | CHECK_EQ(mat.rows(), mat.cols()); 53 | 54 | Eigen::MatrixXd result; 55 | Eigen::LDLT ldlt(mat); 56 | 57 | result = ldlt.matrixL(); 58 | result = ldlt.transpositionsP().transpose() * result; 59 | result *= ldlt.vectorD().array().sqrt().matrix().asDiagonal(); 60 | 61 | return result; 62 | } 63 | 64 | } // namespace gopt 65 | -------------------------------------------------------------------------------- /src/graph/union_find.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef GRAPH_UNION_FIND_H_ 32 | #define GRAPH_UNION_FIND_H_ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace gopt { 40 | namespace graph { 41 | 42 | class UnionFind { 43 | public: 44 | UnionFind() = delete; 45 | UnionFind(size_t n, const size_t max_num_components = 100); 46 | 47 | // union find operations. 48 | void Init(size_t n); 49 | void InitWithNodes(const std::vector& nodes); 50 | size_t FindRoot(size_t x); 51 | void Union(size_t x, size_t y); 52 | 53 | // Get functions. 54 | std::vector GetRanks() const; 55 | std::vector GetParents() const; 56 | // Components are the unique ids of parents. 57 | std::unordered_set GetConnectedComponents() const; 58 | 59 | private: 60 | std::vector ranks_; 61 | std::vector parents_; 62 | std::vector nodes_; 63 | std::vector num_components_; 64 | std::unordered_map nodes_mapper_; 65 | 66 | size_t max_num_components_; 67 | }; 68 | 69 | } // namespace graph 70 | } // namespace gopt 71 | 72 | #endif // GRAPH_UNION_FIND_H_ -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/timing.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1997, Regents of the University of Minnesota 3 | * 4 | * timing.c 5 | * 6 | * This file contains routines that deal with timing Metis 7 | * 8 | * Started 7/24/97 9 | * George 10 | * 11 | * $Id: timing.c,v 1.1 1998/11/27 17:59:32 karypis Exp $ 12 | * 13 | */ 14 | 15 | #include "metis.h" 16 | 17 | 18 | /************************************************************************* 19 | * This function clears the timers 20 | **************************************************************************/ 21 | void InitTimers(CtrlType *ctrl) 22 | { 23 | cleartimer(ctrl->TotalTmr); 24 | cleartimer(ctrl->InitPartTmr); 25 | cleartimer(ctrl->MatchTmr); 26 | cleartimer(ctrl->ContractTmr); 27 | cleartimer(ctrl->CoarsenTmr); 28 | cleartimer(ctrl->UncoarsenTmr); 29 | cleartimer(ctrl->RefTmr); 30 | cleartimer(ctrl->ProjectTmr); 31 | cleartimer(ctrl->SplitTmr); 32 | cleartimer(ctrl->SepTmr); 33 | cleartimer(ctrl->AuxTmr1); 34 | cleartimer(ctrl->AuxTmr2); 35 | cleartimer(ctrl->AuxTmr3); 36 | cleartimer(ctrl->AuxTmr4); 37 | cleartimer(ctrl->AuxTmr5); 38 | cleartimer(ctrl->AuxTmr6); 39 | } 40 | 41 | 42 | 43 | /************************************************************************* 44 | * This function prints the various timers 45 | **************************************************************************/ 46 | void PrintTimers(CtrlType *ctrl) 47 | { 48 | printf("\nTiming Information -------------------------------------------------"); 49 | printf("\n Multilevel: \t\t %7.3f", gettimer(ctrl->TotalTmr)); 50 | printf("\n Coarsening: \t\t %7.3f", gettimer(ctrl->CoarsenTmr)); 51 | printf("\n Matching: \t\t\t %7.3f", gettimer(ctrl->MatchTmr)); 52 | printf("\n Contract: \t\t\t %7.3f", gettimer(ctrl->ContractTmr)); 53 | printf("\n Initial Partition: \t %7.3f", gettimer(ctrl->InitPartTmr)); 54 | printf("\n Construct Separator: \t %7.3f", gettimer(ctrl->SepTmr)); 55 | printf("\n Uncoarsening: \t\t %7.3f", gettimer(ctrl->UncoarsenTmr)); 56 | printf("\n Refinement: \t\t\t %7.3f", gettimer(ctrl->RefTmr)); 57 | printf("\n Projection: \t\t\t %7.3f", gettimer(ctrl->ProjectTmr)); 58 | printf("\n Splitting: \t\t %7.3f", gettimer(ctrl->SplitTmr)); 59 | printf("\n AUX1: \t\t %7.3f", gettimer(ctrl->AuxTmr1)); 60 | printf("\n AUX2: \t\t %7.3f", gettimer(ctrl->AuxTmr2)); 61 | printf("\n AUX3: \t\t %7.3f", gettimer(ctrl->AuxTmr3)); 62 | printf("\n********************************************************************\n"); 63 | } 64 | 65 | 66 | /************************************************************************* 67 | * This function returns the seconds 68 | **************************************************************************/ 69 | double seconds(void) 70 | { 71 | return((double) clock()/CLOCKS_PER_SEC); 72 | } 73 | 74 | 75 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/mcoarsen.c: -------------------------------------------------------------------------------- 1 | /* 2 | * mcoarsen.c 3 | * 4 | * This file contains the driving routines for the coarsening process 5 | * 6 | * Started 7/23/97 7 | * George 8 | * 9 | * $Id: mcoarsen.c,v 1.1 1998/11/27 17:59:19 karypis Exp $ 10 | * 11 | */ 12 | 13 | #include "metis.h" 14 | 15 | 16 | /************************************************************************* 17 | * This function takes a graph and creates a sequence of coarser graphs 18 | **************************************************************************/ 19 | GraphType *MCCoarsen2Way(CtrlType *ctrl, GraphType *graph) 20 | { 21 | int i, clevel; 22 | GraphType *cgraph; 23 | 24 | IFSET(ctrl->dbglvl, DBG_TIME, starttimer(ctrl->CoarsenTmr)); 25 | 26 | cgraph = graph; 27 | 28 | clevel = 0; 29 | do { 30 | if (ctrl->dbglvl&DBG_COARSEN) { 31 | printf("%6d %7d %10d [%d] [%6.4f", cgraph->nvtxs, cgraph->nedges, 32 | idxsum(cgraph->nvtxs, cgraph->adjwgtsum), ctrl->CoarsenTo, ctrl->nmaxvwgt); 33 | for (i=0; incon; i++) 34 | printf(" %5.3f", ssum_strd(cgraph->nvtxs, cgraph->nvwgt+i, cgraph->ncon)); 35 | printf("]\n"); 36 | } 37 | 38 | switch (ctrl->CType) { 39 | case MATCH_RM: 40 | MCMatch_RM(ctrl, cgraph); 41 | break; 42 | case MATCH_HEM: 43 | if (clevel < 1) 44 | MCMatch_RM(ctrl, cgraph); 45 | else 46 | MCMatch_HEM(ctrl, cgraph); 47 | break; 48 | case MATCH_SHEM: 49 | if (clevel < 1) 50 | MCMatch_RM(ctrl, cgraph); 51 | else 52 | MCMatch_SHEM(ctrl, cgraph); 53 | break; 54 | case MATCH_SHEMKWAY: 55 | MCMatch_SHEM(ctrl, cgraph); 56 | break; 57 | case MATCH_SHEBM_ONENORM: 58 | MCMatch_SHEBM(ctrl, cgraph, 1); 59 | break; 60 | case MATCH_SHEBM_INFNORM: 61 | MCMatch_SHEBM(ctrl, cgraph, -1); 62 | break; 63 | case MATCH_SBHEM_ONENORM: 64 | MCMatch_SBHEM(ctrl, cgraph, 1); 65 | break; 66 | case MATCH_SBHEM_INFNORM: 67 | MCMatch_SBHEM(ctrl, cgraph, -1); 68 | break; 69 | default: 70 | graclus_errexit("Unknown CType: %d\n", ctrl->CType); 71 | } 72 | 73 | cgraph = cgraph->coarser; 74 | clevel++; 75 | 76 | } while (cgraph->nvtxs > ctrl->CoarsenTo && cgraph->nvtxs < COARSEN_FRACTION2*cgraph->finer->nvtxs && cgraph->nedges > cgraph->nvtxs/2); 77 | 78 | if (ctrl->dbglvl&DBG_COARSEN) { 79 | printf("%6d %7d %10d [%d] [%6.4f", cgraph->nvtxs, cgraph->nedges, 80 | idxsum(cgraph->nvtxs, cgraph->adjwgtsum), ctrl->CoarsenTo, ctrl->nmaxvwgt); 81 | for (i=0; incon; i++) 82 | printf(" %5.3f", ssum_strd(cgraph->nvtxs, cgraph->nvwgt+i, cgraph->ncon)); 83 | printf("]\n"); 84 | } 85 | 86 | 87 | IFSET(ctrl->dbglvl, DBG_TIME, stoptimer(ctrl->CoarsenTmr)); 88 | 89 | return cgraph; 90 | } 91 | 92 | -------------------------------------------------------------------------------- /3rd_party/Spectra/Util/TypeTraits.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2018-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_TYPE_TRAITS_H 8 | #define SPECTRA_TYPE_TRAITS_H 9 | 10 | #include 11 | #include 12 | 13 | /// \cond 14 | 15 | // Clang-Format will have unintended effects: 16 | // static constexpr Scalar(min)() 17 | // So we turn it off here 18 | // 19 | // clang-format off 20 | 21 | namespace Spectra { 22 | 23 | // For a real value type "Scalar", we want to know its smallest 24 | // positive value, i.e., std::numeric_limits::min(). 25 | // However, we must take non-standard value types into account, 26 | // so we rely on Eigen::NumTraits. 27 | // 28 | // Eigen::NumTraits has defined epsilon() and lowest(), but 29 | // lowest() means negative highest(), which is a very small 30 | // negative value. 31 | // 32 | // Therefore, we manually define this limit, and use eplison()^3 33 | // to mimic it for non-standard types. 34 | 35 | // Generic definition 36 | template 37 | struct TypeTraits 38 | { 39 | static constexpr Scalar epsilon() 40 | { 41 | return Eigen::numext::numeric_limits::epsilon(); 42 | } 43 | static constexpr Scalar (min)() 44 | { 45 | return epsilon() * epsilon() * epsilon(); 46 | } 47 | }; 48 | 49 | // Full specialization 50 | template <> 51 | struct TypeTraits 52 | { 53 | static constexpr float epsilon() 54 | { 55 | return std::numeric_limits::epsilon(); 56 | } 57 | static constexpr float (min)() 58 | { 59 | return (std::numeric_limits::min)(); 60 | } 61 | }; 62 | 63 | template <> 64 | struct TypeTraits 65 | { 66 | static constexpr double epsilon() 67 | { 68 | return std::numeric_limits::epsilon(); 69 | } 70 | static constexpr double (min)() 71 | { 72 | return (std::numeric_limits::min)(); 73 | } 74 | }; 75 | 76 | template <> 77 | struct TypeTraits 78 | { 79 | static constexpr long double epsilon() 80 | { 81 | return std::numeric_limits::epsilon(); 82 | } 83 | static constexpr long double (min)() 84 | { 85 | return (std::numeric_limits::min)(); 86 | } 87 | }; 88 | 89 | // Get the element type of a "scalar" 90 | // ElemType => double 91 | // ElemType> => double 92 | template 93 | using ElemType = typename Eigen::NumTraits::Real; 94 | 95 | } // namespace Spectra 96 | 97 | /// \endcond 98 | 99 | #endif // SPECTRA_TYPE_TRAITS_H 100 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/internal/SymGEigsCholeskyOp.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SYM_GEIGS_CHOLESKY_OP_H 8 | #define SPECTRA_SYM_GEIGS_CHOLESKY_OP_H 9 | 10 | #include 11 | 12 | #include "../DenseSymMatProd.h" 13 | #include "../DenseCholesky.h" 14 | 15 | namespace Spectra { 16 | 17 | /// 18 | /// \ingroup Operators 19 | /// 20 | /// This class defines the matrix operation for generalized eigen solver in the 21 | /// Cholesky decomposition mode. It calculates \f$y=L^{-1}A(L')^{-1}x\f$ for any 22 | /// vector \f$x\f$, where \f$L\f$ is the Cholesky decomposition of \f$B\f$. 23 | /// This class is intended for internal use. 24 | /// 25 | template , 26 | typename BOpType = DenseCholesky> 27 | class SymGEigsCholeskyOp 28 | { 29 | public: 30 | using Scalar = typename OpType::Scalar; 31 | 32 | private: 33 | using Index = Eigen::Index; 34 | using Vector = Eigen::Matrix; 35 | 36 | const OpType& m_op; 37 | const BOpType& m_Bop; 38 | mutable Vector m_cache; // temporary working space 39 | 40 | public: 41 | /// 42 | /// Constructor to create the matrix operation object. 43 | /// 44 | /// \param op The \f$A\f$ matrix operation object. 45 | /// \param Bop The \f$B\f$ matrix operation object. 46 | /// 47 | SymGEigsCholeskyOp(const OpType& op, const BOpType& Bop) : 48 | m_op(op), m_Bop(Bop), m_cache(op.rows()) 49 | {} 50 | 51 | /// 52 | /// Move constructor. 53 | /// 54 | SymGEigsCholeskyOp(SymGEigsCholeskyOp&& other) : 55 | m_op(other.m_op), m_Bop(other.m_Bop) 56 | { 57 | // We emulate the move constructor for Vector using Vector::swap() 58 | m_cache.swap(other.m_cache); 59 | } 60 | 61 | /// 62 | /// Return the number of rows of the underlying matrix. 63 | /// 64 | Index rows() const { return m_Bop.rows(); } 65 | /// 66 | /// Return the number of columns of the underlying matrix. 67 | /// 68 | Index cols() const { return m_Bop.rows(); } 69 | 70 | /// 71 | /// Perform the matrix operation \f$y=L^{-1}A(L')^{-1}x\f$. 72 | /// 73 | /// \param x_in Pointer to the \f$x\f$ vector. 74 | /// \param y_out Pointer to the \f$y\f$ vector. 75 | /// 76 | // y_out = inv(L) * A * inv(L') * x_in 77 | void perform_op(const Scalar* x_in, Scalar* y_out) const 78 | { 79 | m_Bop.upper_triangular_solve(x_in, y_out); 80 | m_op.perform_op(y_out, m_cache.data()); 81 | m_Bop.lower_triangular_solve(m_cache.data(), y_out); 82 | } 83 | }; 84 | 85 | } // namespace Spectra 86 | 87 | #endif // SPECTRA_SYM_GEIGS_CHOLESKY_OP_H 88 | -------------------------------------------------------------------------------- /examples/rotation_estimator.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "graph/view_graph.h" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include "utils/types.h" 39 | 40 | DEFINE_string(g2o_filename, "", "The absolute path of g2o file"); 41 | 42 | int main(int argc, char* argv[]) { 43 | gflags::ParseCommandLineFlags(&argc, &argv, false); 44 | 45 | google::InitGoogleLogging(argv[0]); 46 | FLAGS_alsologtostderr = true; 47 | FLAGS_colorlogtostderr = true; 48 | 49 | if (argc < 2) { 50 | LOG(INFO) << "[Usage]: rotation_estimator --g2o_filename=g2o_filename"; 51 | return 0; 52 | } 53 | 54 | std::string g2o_filename = FLAGS_g2o_filename; 55 | gopt::graph::ViewGraph view_graph; 56 | view_graph.ReadG2OFile(g2o_filename); 57 | 58 | gopt::RotationEstimatorOptions options; 59 | options.sdp_solver_options.verbose = true; 60 | // Set to 1e-6 for se-sync datasets. 61 | options.sdp_solver_options.tolerance = 1e-8; 62 | options.sdp_solver_options.max_iterations = 100; 63 | options.sdp_solver_options.riemannian_staircase_options. 64 | min_eigenvalue_nonnegativity_tolerance = 1e-2; 65 | options.Setup(); 66 | 67 | std::unordered_map global_rotations; 68 | view_graph.RotationAveraging(options, &global_rotations); 69 | } 70 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/coarsen.c: -------------------------------------------------------------------------------- 1 | /* 2 | * coarsen.c 3 | * 4 | * This file contains the driving routines for the coarsening process 5 | * 6 | * Started 7/23/97 7 | * George 8 | * 9 | * $Id: coarsen.c,v 1.1 1998/11/27 17:59:12 karypis Exp $ 10 | * 11 | */ 12 | 13 | #include "metis.h" 14 | 15 | 16 | /************************************************************************* 17 | * This function takes a graph and creates a sequence of coarser graphs 18 | **************************************************************************/ 19 | GraphType *Coarsen2Way(CtrlType *ctrl, GraphType *graph) 20 | { 21 | int clevel; 22 | GraphType *cgraph; 23 | 24 | IFSET(ctrl->dbglvl, DBG_TIME, starttimer(ctrl->CoarsenTmr)); 25 | 26 | cgraph = graph; 27 | 28 | /* The following is ahack to allow the multiple bisections to go through with correct 29 | coarsening */ 30 | if (ctrl->CType > 20) { 31 | clevel = 1; 32 | ctrl->CType -= 20; 33 | } 34 | else 35 | clevel = 0; 36 | 37 | do { 38 | IFSET(ctrl->dbglvl, DBG_COARSEN, printf("%6d %7d [%d] [%d %d]\n", 39 | cgraph->nvtxs, cgraph->nedges, ctrl->CoarsenTo, ctrl->maxvwgt, 40 | (cgraph->vwgt ? idxsum(cgraph->nvtxs, cgraph->vwgt) : cgraph->nvtxs))); 41 | 42 | if (cgraph->adjwgt) { 43 | switch (ctrl->CType) { 44 | case MATCH_RM: 45 | Match_RM(ctrl, cgraph); 46 | break; 47 | case MATCH_HEM: 48 | if (clevel < 1) 49 | Match_RM(ctrl, cgraph); 50 | else 51 | Match_HEM(ctrl, cgraph); 52 | break; 53 | case MATCH_HEMN: 54 | if (clevel < 1) 55 | Match_RM(ctrl, cgraph); 56 | else 57 | Match_HEMN(ctrl, cgraph); 58 | break; 59 | case MATCH_SHEMN: 60 | //if (clevel < 1) 61 | // Match_RM(ctrl, cgraph); 62 | //else 63 | Match_SHEMN(ctrl, cgraph); 64 | break; 65 | case MATCH_SHEM: 66 | if (clevel < 1) 67 | Match_RM(ctrl, cgraph); 68 | else 69 | Match_SHEM(ctrl, cgraph); 70 | break; 71 | case MATCH_SHEMKWAY: 72 | Match_SHEM(ctrl, cgraph); 73 | break; 74 | default: 75 | graclus_errexit("Unknown CType: %d\n", ctrl->CType); 76 | } 77 | } 78 | else { 79 | Match_RM_NVW(ctrl, cgraph); 80 | } 81 | 82 | cgraph = cgraph->coarser; 83 | clevel++; 84 | //printf("Coarsening to level %d, number of vertices is %d...\n", clevel, cgraph->nvtxs); 85 | } while (cgraph->nvtxs > ctrl->CoarsenTo && cgraph->nvtxs < COARSEN_FRACTION2*cgraph->finer->nvtxs && cgraph->nedges > cgraph->nvtxs/2); 86 | 87 | 88 | IFSET(ctrl->dbglvl, DBG_COARSEN, printf("%6d %7d [%d] [%d %d]\n", 89 | cgraph->nvtxs, cgraph->nedges, ctrl->CoarsenTo, ctrl->maxvwgt, 90 | (cgraph->vwgt ? idxsum(cgraph->nvtxs, cgraph->vwgt) : cgraph->nvtxs))); 91 | 92 | IFSET(ctrl->dbglvl, DBG_TIME, stoptimer(ctrl->CoarsenTmr)); 93 | 94 | return cgraph; 95 | } 96 | 97 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/internal/SymGEigsBucklingOp.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2020-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SYM_GEIGS_BUCKLING_OP_H 8 | #define SPECTRA_SYM_GEIGS_BUCKLING_OP_H 9 | 10 | #include 11 | 12 | #include "../SymShiftInvert.h" 13 | #include "../SparseSymMatProd.h" 14 | 15 | namespace Spectra { 16 | 17 | /// 18 | /// \ingroup Operators 19 | /// 20 | /// This class defines the matrix operation for generalized eigen solver in the 21 | /// buckling mode. It computes \f$y=(K-\sigma K_G)^{-1}Kx\f$ for any 22 | /// vector \f$x\f$, where \f$K\f$ is positive definite, \f$K_G\f$ is symmetric, 23 | /// and \f$\sigma\f$ is a real shift. 24 | /// This class is intended for internal use. 25 | /// 26 | template , 27 | typename BOpType = SparseSymMatProd> 28 | class SymGEigsBucklingOp 29 | { 30 | public: 31 | using Scalar = typename OpType::Scalar; 32 | 33 | private: 34 | using Index = Eigen::Index; 35 | using Vector = Eigen::Matrix; 36 | 37 | OpType& m_op; 38 | const BOpType& m_Bop; 39 | mutable Vector m_cache; // temporary working space 40 | 41 | public: 42 | /// 43 | /// Constructor to create the matrix operation object. 44 | /// 45 | /// \param op The \f$(K-\sigma K_G)^{-1}\f$ matrix operation object. 46 | /// \param Bop The \f$K\f$ matrix operation object. 47 | /// 48 | SymGEigsBucklingOp(OpType& op, const BOpType& Bop) : 49 | m_op(op), m_Bop(Bop), m_cache(op.rows()) 50 | {} 51 | 52 | /// 53 | /// Move constructor. 54 | /// 55 | SymGEigsBucklingOp(SymGEigsBucklingOp&& other) : 56 | m_op(other.m_op), m_Bop(other.m_Bop) 57 | { 58 | // We emulate the move constructor for Vector using Vector::swap() 59 | m_cache.swap(other.m_cache); 60 | } 61 | 62 | /// 63 | /// Return the number of rows of the underlying matrix. 64 | /// 65 | Index rows() const { return m_op.rows(); } 66 | /// 67 | /// Return the number of columns of the underlying matrix. 68 | /// 69 | Index cols() const { return m_op.rows(); } 70 | 71 | /// 72 | /// Set the real shift \f$\sigma\f$. 73 | /// 74 | void set_shift(const Scalar& sigma) 75 | { 76 | m_op.set_shift(sigma); 77 | } 78 | 79 | /// 80 | /// Perform the matrix operation \f$y=(K-\sigma K_G)^{-1}Kx\f$. 81 | /// 82 | /// \param x_in Pointer to the \f$x\f$ vector. 83 | /// \param y_out Pointer to the \f$y\f$ vector. 84 | /// 85 | // y_out = inv(K - sigma * K_G) * K * x_in 86 | void perform_op(const Scalar* x_in, Scalar* y_out) const 87 | { 88 | m_Bop.perform_op(x_in, m_cache.data()); 89 | m_op.perform_op(m_cache.data(), y_out); 90 | } 91 | }; 92 | 93 | } // namespace Spectra 94 | 95 | #endif // SPECTRA_SYM_GEIGS_BUCKLING_OP_H 96 | -------------------------------------------------------------------------------- /src/graph/view_graph_test.cc: -------------------------------------------------------------------------------- 1 | #include "graph/view_graph.h" 2 | 3 | #include 4 | 5 | #include "geometry/rotation_utils.h" 6 | #include "test/view_graph_generator.h" 7 | 8 | namespace gopt { 9 | namespace graph { 10 | 11 | TEST(VIEW_GRAPH_TEST, TEST_INITIALIZE_GLOBAL_ROTATIONS_FROM_MST) { 12 | const size_t num_nodes = 10; 13 | const double completeness = 0.5; 14 | const double sigma = 10; 15 | const double outlier_ratio = 0; 16 | 17 | ViewGraphGenerator::ViewGraphGeneratorOptions options; 18 | ViewGraphGenerator generator(options); 19 | std::unordered_map gt_rotations; 20 | 21 | ViewGraph view_graph = generator.GenerateRandomGraph( 22 | num_nodes, completeness, sigma, outlier_ratio, 0, 0, 23 | >_rotations, nullptr); 24 | 25 | // Change the gauge freedom of ground truth rotations. 26 | const Eigen::Vector3d gt_angle_axis0 = gt_rotations.at(0); 27 | for (image_t i = 0; i < num_nodes; i++) { 28 | const Eigen::Vector3d angle_axis = gt_rotations[i]; 29 | gt_rotations[i] = geometry::RelativeRotationFromTwoRotations( 30 | gt_angle_axis0, angle_axis); 31 | } 32 | 33 | std::unordered_map mst_rotations; 34 | view_graph.InitializeGlobalRotationsFromMST(&mst_rotations); 35 | // Change the gauge freedom of ground truth rotations. 36 | const Eigen::Vector3d mst_angle_axis0 = mst_rotations.at(0); 37 | for (image_t i = 0; i < num_nodes; i++) { 38 | const Eigen::Vector3d angle_axis = mst_rotations[i]; 39 | mst_rotations[i] = geometry::RelativeRotationFromTwoRotations( 40 | mst_angle_axis0, angle_axis); 41 | } 42 | 43 | // Align the rotations and measure the error. 44 | geometry::AlignOrientations(gt_rotations, &mst_rotations); 45 | double sum_angular_error = 0.0; 46 | std::vector angular_errors; 47 | 48 | for (size_t i = 0; i < gt_rotations.size(); i++) { 49 | const auto& rotation = gt_rotations.at(i); 50 | const Eigen::Vector3d& mst_rotation = FindOrDie(mst_rotations, i); 51 | const Eigen::Vector3d relative_rotation = 52 | geometry::RelativeRotationFromTwoRotations(mst_rotation, 53 | rotation, 0.0); 54 | const double angular_error = geometry::RadToDeg(relative_rotation.norm()); 55 | 56 | sum_angular_error += angular_error; 57 | angular_errors.push_back(angular_error); 58 | } 59 | 60 | std::sort(angular_errors.begin(), angular_errors.end()); 61 | const double mean_angular_error = sum_angular_error / angular_errors.size(); 62 | const double median_angular_error = angular_errors[num_nodes / 2]; 63 | 64 | CHECK_LE(mean_angular_error, 15); 65 | CHECK_LE(median_angular_error, 15); 66 | 67 | std::cout << "\n"; 68 | LOG(INFO) << "Mean Angular Residual (deg): " << mean_angular_error; 69 | LOG(INFO) << "Median Angular Residual (deg): " << median_angular_error; 70 | LOG(INFO) << "Max Angular Residual (deg): " << angular_errors[num_nodes - 1]; 71 | LOG(INFO) << "Min Angular Residual (deg): " << angular_errors[0]; 72 | } 73 | 74 | } // namespace graph 75 | } // namespace gopt 76 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/mutil.c: -------------------------------------------------------------------------------- 1 | /* 2 | * mutil.c 3 | * 4 | * This file contains various utility functions for the MOC portion of the 5 | * code 6 | * 7 | * Started 2/15/98 8 | * George 9 | * 10 | * $Id: mutil.c,v 1.1 1998/11/27 17:59:27 karypis Exp $ 11 | * 12 | */ 13 | 14 | #include "metis.h" 15 | 16 | 17 | /************************************************************************* 18 | * This function checks if the vertex weights of two vertices are below 19 | * a given set of values 20 | **************************************************************************/ 21 | int AreAllVwgtsBelow(int ncon, float alpha, float *vwgt1, float beta, float *vwgt2, float limit) 22 | { 23 | int i; 24 | 25 | for (i=0; i limit) 27 | return 0; 28 | 29 | return 1; 30 | } 31 | 32 | 33 | /************************************************************************* 34 | * This function checks if the vertex weights of two vertices are below 35 | * a given set of values 36 | **************************************************************************/ 37 | int AreAnyVwgtsBelow(int ncon, float alpha, float *vwgt1, float beta, float *vwgt2, float limit) 38 | { 39 | int i; 40 | 41 | for (i=0; i max) 79 | max = npwgts[j*ncon+i]; 80 | } 81 | if (max*nparts > lb) 82 | lb = max*nparts; 83 | } 84 | 85 | return lb; 86 | } 87 | 88 | /************************************************************************* 89 | * This function checks if the vertex weights of two vertices are below 90 | * a given set of values 91 | **************************************************************************/ 92 | int AreAllBelow(int ncon, float *v1, float *v2) 93 | { 94 | int i; 95 | 96 | for (i=0; i v2[i]) 98 | return 0; 99 | 100 | return 1; 101 | } 102 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/internal/SymGEigsShiftInvertOp.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2020-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SYM_GEIGS_SHIFT_INVERT_OP_H 8 | #define SPECTRA_SYM_GEIGS_SHIFT_INVERT_OP_H 9 | 10 | #include 11 | 12 | #include "../SymShiftInvert.h" 13 | #include "../SparseSymMatProd.h" 14 | 15 | namespace Spectra { 16 | 17 | /// 18 | /// \ingroup Operators 19 | /// 20 | /// This class defines the matrix operation for generalized eigen solver in the 21 | /// shift-and-invert mode. It computes \f$y=(A-\sigma B)^{-1}Bx\f$ for any 22 | /// vector \f$x\f$, where \f$A\f$ is a symmetric matrix, \f$B\f$ is positive definite, 23 | /// and \f$\sigma\f$ is a real shift. 24 | /// This class is intended for internal use. 25 | /// 26 | template , 27 | typename BOpType = SparseSymMatProd> 28 | class SymGEigsShiftInvertOp 29 | { 30 | public: 31 | using Scalar = typename OpType::Scalar; 32 | 33 | private: 34 | using Index = Eigen::Index; 35 | using Vector = Eigen::Matrix; 36 | 37 | OpType& m_op; 38 | const BOpType& m_Bop; 39 | mutable Vector m_cache; // temporary working space 40 | 41 | public: 42 | /// 43 | /// Constructor to create the matrix operation object. 44 | /// 45 | /// \param op The \f$(A-\sigma B)^{-1}\f$ matrix operation object. 46 | /// \param Bop The \f$B\f$ matrix operation object. 47 | /// 48 | SymGEigsShiftInvertOp(OpType& op, const BOpType& Bop) : 49 | m_op(op), m_Bop(Bop), m_cache(op.rows()) 50 | {} 51 | 52 | /// 53 | /// Move constructor. 54 | /// 55 | SymGEigsShiftInvertOp(SymGEigsShiftInvertOp&& other) : 56 | m_op(other.m_op), m_Bop(other.m_Bop) 57 | { 58 | // We emulate the move constructor for Vector using Vector::swap() 59 | m_cache.swap(other.m_cache); 60 | } 61 | 62 | /// 63 | /// Return the number of rows of the underlying matrix. 64 | /// 65 | Index rows() const { return m_op.rows(); } 66 | /// 67 | /// Return the number of columns of the underlying matrix. 68 | /// 69 | Index cols() const { return m_op.rows(); } 70 | 71 | /// 72 | /// Set the real shift \f$\sigma\f$. 73 | /// 74 | void set_shift(const Scalar& sigma) 75 | { 76 | m_op.set_shift(sigma); 77 | } 78 | 79 | /// 80 | /// Perform the matrix operation \f$y=(A-\sigma B)^{-1}Bx\f$. 81 | /// 82 | /// \param x_in Pointer to the \f$x\f$ vector. 83 | /// \param y_out Pointer to the \f$y\f$ vector. 84 | /// 85 | // y_out = inv(A - sigma * B) * B * x_in 86 | void perform_op(const Scalar* x_in, Scalar* y_out) const 87 | { 88 | m_Bop.perform_op(x_in, m_cache.data()); 89 | m_op.perform_op(m_cache.data(), y_out); 90 | } 91 | }; 92 | 93 | } // namespace Spectra 94 | 95 | #endif // SPECTRA_SYM_GEIGS_SHIFT_INVERT_OP_H 96 | -------------------------------------------------------------------------------- /src/solver/summary.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef SOLVER_SUMMARY_H_ 32 | #define SOLVER_SUMMARY_H_ 33 | 34 | #include 35 | #include 36 | 37 | namespace gopt { 38 | namespace solver { 39 | 40 | struct Summary { 41 | unsigned total_iterations_num = 0; 42 | 43 | std::chrono::high_resolution_clock::time_point begin_time; 44 | std::chrono::high_resolution_clock::time_point end_time; 45 | std::chrono::high_resolution_clock::time_point prev_time; 46 | 47 | Summary() { total_iterations_num = 0; } 48 | 49 | Summary(const Summary& summary) { 50 | total_iterations_num = summary.total_iterations_num; 51 | begin_time = summary.begin_time; 52 | end_time = summary.end_time; 53 | } 54 | 55 | double TotalTime() { 56 | return std::chrono::duration_cast>( 57 | end_time - begin_time) 58 | .count(); 59 | } 60 | 61 | double Duration() { 62 | if (total_iterations_num == 1) { 63 | prev_time = begin_time; 64 | } else { 65 | prev_time = end_time; 66 | } 67 | end_time = std::chrono::high_resolution_clock::now(); 68 | return std::chrono::duration_cast>( 69 | end_time - prev_time) 70 | .count(); 71 | } 72 | 73 | void Report() { 74 | std::cout << "\nLagrange Dual Rotation Averaging Report: \n"; 75 | std::cout << "Total iterations: " << total_iterations_num << std::endl; 76 | std::cout << "Time took: " << TotalTime() << " milliseconds\n"; 77 | } 78 | }; 79 | } // namespace solver 80 | } // namespace gopt 81 | 82 | #endif // SOLVER_SUMMARY_H_ 83 | -------------------------------------------------------------------------------- /3rd_party/Spectra/Util/SimpleRandom.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SIMPLE_RANDOM_H 8 | #define SPECTRA_SIMPLE_RANDOM_H 9 | 10 | #include 11 | 12 | /// \cond 13 | 14 | namespace Spectra { 15 | 16 | // We need a simple pseudo random number generator here: 17 | // 1. It is used to generate initial and restarted residual vector. 18 | // 2. It is not necessary to be so "random" and advanced. All we hope 19 | // is that the residual vector is not in the space spanned by the 20 | // current Krylov space. This should be met almost surely. 21 | // 3. We don't want to call RNG in C++, since we actually want the 22 | // algorithm to be deterministic. Also, calling RNG in C/C++ is not 23 | // allowed in R packages submitted to CRAN. 24 | // 4. The method should be as simple as possible, so an LCG is enough. 25 | // 5. Based on public domain code by Ray Gardner 26 | // http://stjarnhimlen.se/snippets/rg_rand.c 27 | 28 | template 29 | class SimpleRandom 30 | { 31 | private: 32 | using Index = Eigen::Index; 33 | using Vector = Eigen::Matrix; 34 | 35 | static constexpr unsigned int m_a = 16807; // multiplier 36 | static constexpr unsigned long m_max = 2147483647L; // 2^31 - 1 37 | long m_rand; // RNG state 38 | 39 | inline long next_long_rand(long seed) const 40 | { 41 | unsigned long lo, hi; 42 | 43 | lo = m_a * (long) (seed & 0xFFFF); 44 | hi = m_a * (long) ((unsigned long) seed >> 16); 45 | lo += (hi & 0x7FFF) << 16; 46 | if (lo > m_max) 47 | { 48 | lo &= m_max; 49 | ++lo; 50 | } 51 | lo += hi >> 15; 52 | if (lo > m_max) 53 | { 54 | lo &= m_max; 55 | ++lo; 56 | } 57 | return (long) lo; 58 | } 59 | 60 | public: 61 | SimpleRandom(unsigned long init_seed) : 62 | m_rand(init_seed ? (init_seed & m_max) : 1) 63 | {} 64 | 65 | // Return a single random number, ranging from -0.5 to 0.5 66 | Scalar random() 67 | { 68 | m_rand = next_long_rand(m_rand); 69 | return Scalar(m_rand) / Scalar(m_max) - Scalar(0.5); 70 | } 71 | 72 | // Fill the given vector with random numbers 73 | // Ranging from -0.5 to 0.5 74 | void random_vec(Vector& vec) 75 | { 76 | const Index len = vec.size(); 77 | for (Index i = 0; i < len; i++) 78 | { 79 | m_rand = next_long_rand(m_rand); 80 | vec[i] = Scalar(m_rand); 81 | } 82 | vec.array() = vec.array() / Scalar(m_max) - Scalar(0.5); 83 | } 84 | 85 | // Return a vector of random numbers 86 | // Ranging from -0.5 to 0.5 87 | Vector random_vec(const Index len) 88 | { 89 | Vector res(len); 90 | random_vec(res); 91 | return res; 92 | } 93 | }; 94 | 95 | } // namespace Spectra 96 | 97 | /// \endcond 98 | 99 | #endif // SPECTRA_SIMPLE_RANDOM_H 100 | -------------------------------------------------------------------------------- /applications/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | 3 | # Copyright (c) 2022, Chenyu 4 | # All rights reserved. 5 | 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | 9 | # 1. Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | cmake_minimum_required(VERSION 3.5) 32 | 33 | project(GlobalSfM) 34 | 35 | set(CMAKE_CXX_FLAGS "-std=c++11") 36 | set(CMAKE_BUILD_TYPE "Release") 37 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 38 | 39 | SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/../bin) 40 | message(STATUS "${CMAKE_SOURCE_DIR}") 41 | 42 | # or to require a specific version: find_package(COLMAP 3.4 REQUIRED) 43 | find_package(COLMAP REQUIRED) 44 | if(COLMAP_FOUND) 45 | message(STATUS "Found COLMAP INCLUDE DIR ${COLMAP_INCLUDE_DIRS}") 46 | message(STATUS "Found COLMAP LINK DIR ${COLMAP_LINK_DIRS}") 47 | message(STATUS "Found COLMAP LIB DIR ${COLMAP_LIBRARIES}") 48 | message(STATUS "COLMAP ENABLED") 49 | add_definitions(-DCOLMAP_ENABLED) 50 | else(COLMAP_FOUND) 51 | message(FATAL "COLMAP not found!") 52 | endif(COLMAP_FOUND) 53 | 54 | find_package(Gopt REQUIRED) 55 | if(GOPT_FOUND) 56 | message(STATUS "Found GOPT_INCLUDE_DIRs ${GOPT_INCLUDE_DIRS}") 57 | message(STATUS "Found GOPT_LINK_DIRs ${GOPT_LINK_DIRS}") 58 | message(STATUS "Found GOPT_LIBRARIES ${GOPT_LIBRARIES}") 59 | else(GOPT_FOUND) 60 | message(FATAL "GOPT not found!") 61 | endif(GOPT_FOUND) 62 | 63 | include_directories(${COLMAP_INCLUDE_DIRS} ${GOPT_INCLUDE_DIRS}) 64 | link_directories(${COLMAP_LINK_DIRS} ${GOPT_LINK_DIRS}) 65 | 66 | 67 | configure_file(run_global_sfm.sh.in ${PROJECT_SOURCE_DIR}/run_global_sfm.sh) 68 | # install(FILES "${PROJECT_SOURCE_DIR}/run_global_sfm.sh" 69 | # DESTINATION "../../scripts") 70 | 71 | add_library(global_sfm 72 | global_mapper_controller.h global_mapper_controller.cc 73 | global_mapper.h global_mapper.cc 74 | utils.h utils.cc 75 | ) 76 | target_link_libraries(global_sfm ${COLMAP_LIBRARIES} ${GOPT_LIBRARIES}) 77 | 78 | add_executable(run_global_mapper run_global_mapper.cc) 79 | target_link_libraries(run_global_mapper 80 | global_sfm 81 | ${COLMAP_LIBRARIES} 82 | ${GOPT_LIBRARIES} 83 | ) 84 | -------------------------------------------------------------------------------- /src/geometry/align_point_clouds.cc: -------------------------------------------------------------------------------- 1 | #include "geometry/align_point_clouds.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace gopt { 8 | 9 | void AlignPointCloudsUmeyama(const std::vector& left, 10 | const std::vector& right, 11 | Eigen::Matrix3d* rotation, 12 | Eigen::Vector3d* translation, double* scale) { 13 | std::vector weights(left.size(), 1.0); 14 | AlignPointCloudsUmeyamaWithWeights(left, right, weights, rotation, 15 | translation, scale); 16 | } 17 | 18 | void AlignPointCloudsUmeyamaWithWeights( 19 | const std::vector& left, 20 | const std::vector& right, 21 | const std::vector& weights, Eigen::Matrix3d* rotation, 22 | Eigen::Vector3d* translation, double* scale) { 23 | CHECK_EQ(left.size(), right.size()); 24 | CHECK_EQ(left.size(), weights.size()); 25 | CHECK_NOTNULL(rotation); 26 | CHECK_NOTNULL(translation); 27 | CHECK_NOTNULL(scale); 28 | 29 | // Fill outputs (useful when it fails) 30 | *scale = 1.0; 31 | *translation = Eigen::Vector3d::Zero(); 32 | *rotation = Eigen::Matrix3d::Identity(); 33 | 34 | const size_t num_points = left.size(); 35 | Eigen::Map > left_points( 36 | left[0].data(), 3, num_points); 37 | Eigen::Map > right_points( 38 | right[0].data(), 3, num_points); 39 | 40 | Eigen::Vector3d left_centroid, right_centroid; 41 | left_centroid.setZero(); 42 | right_centroid.setZero(); 43 | double weights_sum = 0.0; 44 | for (size_t i = 0; i < num_points; i++) { 45 | CHECK_GE(weights[i], 0) 46 | << "The point weight must be greater or equal to zero."; 47 | weights_sum += weights[i]; 48 | left_centroid += left[i] * weights[i]; 49 | right_centroid += right[i] * weights[i]; 50 | } 51 | // Check if the sum is valid 52 | CHECK_GT(weights_sum, 0) << "The sum of weights must be greater than zero."; 53 | 54 | left_centroid /= weights_sum; 55 | right_centroid /= weights_sum; 56 | 57 | double sigma = 0.0; 58 | for (size_t i = 0; i < num_points; i++) { 59 | sigma += (left[i] - left_centroid).squaredNorm() * weights[i]; 60 | } 61 | sigma /= weights_sum; 62 | 63 | // Calculate cross correlation matrix based on the points shifted about the 64 | // centroid. 65 | Eigen::Matrix3d cross_correlation = Eigen::Matrix3d::Zero(); 66 | for (size_t i = 0; i < num_points; i++) { 67 | cross_correlation += weights[i] * (left_points.col(i) - left_centroid) * 68 | (right_points.col(i) - right_centroid).transpose(); 69 | } 70 | cross_correlation /= weights_sum; 71 | 72 | // Compute SVD decomposition of the cross correlation. 73 | Eigen::JacobiSVD svd( 74 | cross_correlation.transpose(), Eigen::ComputeFullU | Eigen::ComputeFullV); 75 | 76 | Eigen::Matrix3d umatrix = svd.matrixU(); 77 | Eigen::Matrix3d vtmatrix = svd.matrixV().transpose(); 78 | const Eigen::Vector3d& singular_values = svd.singularValues(); 79 | 80 | double det = umatrix.determinant() * vtmatrix.determinant(); 81 | Eigen::Matrix3d s = Eigen::Matrix3d::Identity(); 82 | s(2, 2) = det > 0 ? 1 : -1; 83 | 84 | *scale = 85 | (singular_values(0) + singular_values(1) + s(2, 2) * singular_values(2)) / 86 | sigma; 87 | *rotation = umatrix * s * vtmatrix; 88 | *translation = right_centroid - (*scale) * (*rotation) * left_centroid; 89 | } 90 | 91 | } // namespace gopt 92 | -------------------------------------------------------------------------------- /cmake/CMakeConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # Find package module for GOPT library. 2 | # 3 | # The following variables are set by this module: 4 | # 5 | # GOPT_FOUND: TRUE if GOPT is found. 6 | # GOPT_VERSION: GOPT version. 7 | # GOPT_INCLUDE_DIRS: Include directories for GOPT. 8 | # GOPT_LINK_DIRS: Link directories for GOPT. 9 | # GOPT_LIBRARIES: Libraries required to link GOPT. 10 | # GOPT_CUDA_ENABLED: Whether GOPT was compiled with CUDA support. 11 | 12 | get_filename_component(GOPT_INSTALL_PREFIX ${CMAKE_CURRENT_LIST_FILE} PATH) 13 | set(GOPT_INSTALL_PREFIX "${GOPT_INSTALL_PREFIX}/../..") 14 | 15 | set(GOPT_FOUND FALSE) 16 | 17 | # Set hints for finding dependency packages. 18 | 19 | set(EIGEN3_INCLUDE_DIR_HINTS @EIGEN3_INCLUDE_DIR_HINTS@) 20 | 21 | set(GLOG_INCLUDE_DIR_HINTS @GLOG_INCLUDE_DIR_HINTS@) 22 | set(GLOG_LIBRARY_DIR_HINTS @GLOG_LIBRARY_DIR_HINTS@) 23 | 24 | # Find dependency packages. 25 | 26 | set(TEMP_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH}) 27 | set(CMAKE_MODULE_PATH ${GOPT_INSTALL_PREFIX}/share/gopt/cmake) 28 | 29 | if(GOPT_FIND_QUIETLY) 30 | find_package(Ceres QUIET) 31 | 32 | find_package(Eigen3 QUIET) 33 | 34 | find_package(Glog QUIET) 35 | find_package(GTest QUIET) 36 | find_package(GFlags QUIET) 37 | 38 | find_package(SuiteSparse QUIET) 39 | else() 40 | find_package(Ceres REQUIRED) 41 | 42 | find_package(Eigen3 REQUIRED) 43 | 44 | find_package(Glog REQUIRED) 45 | find_package(GTest REQUIRED) 46 | find_package(GFlags REQUIRED) 47 | 48 | find_package(SuiteSparse REQUIRED) 49 | 50 | endif() 51 | 52 | # Set the exported variables. 53 | 54 | set(GOPT_FOUND TRUE) 55 | 56 | # set(GOPT_VERSION @GOPT_VERSION@) 57 | 58 | set(GOPT_OPENMP_ENABLED @OPENMP_ENABLED@) 59 | 60 | # set(GOPT_CUDA_ENABLED @CUDA_ENABLED@) 61 | # set(GOPT_CUDA_MIN_VERSION @CUDA_MIN_VERSION@) 62 | 63 | 64 | set(GOPT_INCLUDE_DIRS 65 | ${GOPT_INSTALL_PREFIX}/include/ 66 | ${GOPT_INSTALL_PREFIX}/include/gopt 67 | ${GOPT_INSTALL_PREFIX}/include/gopt/lib 68 | ${EIGEN3_INCLUDE_DIRS} 69 | ${GLOG_INCLUDE_DIRS} 70 | ${CERES_INCLUDE_DIRS} 71 | ${SUITESPARSE_INCLUDE_DIRS} 72 | ${GTEST_INCLUDE_DIRS} 73 | ) 74 | 75 | set(GOPT_LINK_DIRS 76 | ${GOPT_INSTALL_PREFIX}/lib/gopt 77 | ) 78 | 79 | set(GOPT_INTERNAL_LIBRARIES 80 | graclus 81 | ) 82 | 83 | set(GOPT_EXTERNAL_LIBRARIES 84 | ${CMAKE_DL_LIBS} 85 | ${GLOG_LIBRARIES} 86 | ${GTEST_LIBRARIES} 87 | ${GFLAGS_LIBRARIES} 88 | ${CERES_LIBRARIES} 89 | ) 90 | 91 | if(UNIX) 92 | list(APPEND GOPT_EXTERNAL_LIBRARIES 93 | pthread) 94 | endif() 95 | 96 | if(GOPT_OPENMP_ENABLED) 97 | find_package(OpenMP QUIET) 98 | add_definitions("-DOPENMP_ENABLED") 99 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 100 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 101 | endif() 102 | 103 | # if(GOPT_CUDA_ENABLED) 104 | # find_package(CUDA ${GOPT_CUDA_MIN_VERSION} QUIET) 105 | # list(APPEND GOPT_EXTERNAL_LIBRARIES ${CUDA_LIBRARIES}) 106 | # list(APPEND GOPT_INTERNAL_LIBRARIES gopt_cuda) 107 | # endif() 108 | 109 | set(GOPT_LIBRARIES 110 | gopt 111 | ${GOPT_INTERNAL_LIBRARIES} 112 | ${GOPT_EXTERNAL_LIBRARIES} 113 | ) 114 | 115 | # Cleanup of configuration variables. 116 | 117 | set(CMAKE_MODULE_PATH ${TEMP_CMAKE_MODULE_PATH}) 118 | 119 | unset(GOPT_INSTALL_PREFIX) 120 | unset(EIGEN3_INCLUDE_DIR_HINTS) 121 | unset(GLOG_INCLUDE_DIR_HINTS) 122 | unset(GLOG_LIBRARY_DIR_HINTS) 123 | unset(GFLAGS_INCLUDE_DIR_HINTS) 124 | unset(GFLAGS_LIBRARY_DIR_HINTS) 125 | unset(GFLAGS_LIBRARY_DIR_HINTS) 126 | -------------------------------------------------------------------------------- /src/geometry/rotation_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021, Chenyu 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 | // 1. Redistributions of source code must retain the above copyright notice, this 8 | // list of conditions and the following disclaimer. 9 | 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // 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 ARE 21 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "geometry/rotation.h" 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | namespace mvgplus { 37 | 38 | namespace { 39 | Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry, 40 | const double rz) { 41 | const Eigen::Matrix3d Rx = 42 | Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix(); 43 | const Eigen::Matrix3d Ry = 44 | Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix(); 45 | const Eigen::Matrix3d Rz = 46 | Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix(); 47 | return Rz * Ry * Rx; 48 | } 49 | 50 | } // namespace 51 | 52 | TEST(TEST_ROTATION, TestQuaternionToRotationMatrix) { 53 | const double rx = 0; 54 | const double ry = 0; 55 | const double rz = 0.3; 56 | const Eigen::Matrix3d rot_mat0 = EulerAnglesToRotationMatrix(rx, ry, rz); 57 | const Eigen::Matrix3d rot_mat1 = 58 | QuaternionToRotationMatrix(RotationMatrixToQuaternion(rot_mat0)); 59 | EXPECT_EQ(rot_mat0.isApprox(rot_mat1), true); 60 | } 61 | 62 | TEST(TEST_ROTATION, TestAngleAxisToRotationMatrix) { 63 | const double rx = 0; 64 | const double ry = 0; 65 | const double rz = 0.3; 66 | const Eigen::Matrix3d R1 = EulerAnglesToRotationMatrix(rx, ry, rz); 67 | const Eigen::Vector3d angle_axis = RotationMatrixToAngleAxis(R1); 68 | const Eigen::Matrix3d R2 = 69 | AngleAxisToRotationMatrix(angle_axis); 70 | 71 | EXPECT_EQ(R1.isApprox(R2), true); 72 | } 73 | 74 | TEST(TEST_ROTATION, TestQuaternionToAngleAxis) { 75 | const double rx = 0; 76 | const double ry = 0; 77 | const double rz = 0.3; 78 | const Eigen::Matrix3d R = EulerAnglesToRotationMatrix(rx, ry, rz); 79 | 80 | const Eigen::Vector4d quat1 = RotationMatrixToQuaternion(R); 81 | const Eigen::Vector4d quat2 = 82 | AngleAxisToQuaternion(QuaternionToAngleAxis(quat1)); 83 | 84 | EXPECT_EQ(quat1.isApprox(quat2), true); 85 | } 86 | 87 | } // mvgplus 88 | -------------------------------------------------------------------------------- /src/graph/node.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef GRAPH_NODE_H_ 32 | #define GRAPH_NODE_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "geometry/rotation.h" 41 | 42 | namespace gopt { 43 | namespace graph { 44 | 45 | typedef size_t node_t; 46 | const node_t kInvalidNodeId = std::numeric_limits::max(); 47 | 48 | struct Node { 49 | node_t id = kInvalidNodeId; 50 | 51 | Node() {} 52 | 53 | Node(node_t idx) { 54 | id = idx; 55 | } 56 | 57 | Node(const Node& node) { id = node.id; } 58 | 59 | bool operator==(const Node& node) { return id == node.id; } 60 | 61 | // sort in ascending order 62 | static bool CompareById(const Node& node1, const Node& node2) { 63 | return node1.id < node2.id; 64 | } 65 | }; 66 | 67 | struct ImageNode : Node { 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 69 | 70 | // Absolute rotation of the image node. 71 | Eigen::Vector3d rotation = Eigen::Vector3d::Zero(); 72 | 73 | // Absolute position of the image node (camera center). 74 | Eigen::Vector3d position = Eigen::Vector3d::Zero(); 75 | 76 | // Absolute path of image. 77 | std::string img_path = ""; 78 | 79 | ImageNode() 80 | : Node(kInvalidNodeId) { 81 | img_path = ""; 82 | } 83 | 84 | ImageNode(const node_t& idx, const std::string& path = "") : Node(idx) { 85 | img_path = path; 86 | } 87 | 88 | ImageNode(const ImageNode& img_node) { 89 | id = img_node.id; 90 | img_path = img_node.img_path; 91 | rotation = img_node.rotation; 92 | position = img_node.position; 93 | } 94 | 95 | Eigen::Vector3d Translation() const { 96 | return -AngleAxisRotatePoint(rotation, position); 97 | } 98 | 99 | // ImageNode& operator=(const ImageNode& img_node) = delete; 100 | }; 101 | using ViewNode = ImageNode; 102 | 103 | } // namespace graph 104 | } // namespace gopt 105 | 106 | #endif // GRAPH_NODE_H_ 107 | -------------------------------------------------------------------------------- /3rd_party/Spectra/DavidsonSymEigsSolver.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2020 Netherlands eScience Center 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_DAVIDSON_SYM_EIGS_SOLVER_H 8 | #define SPECTRA_DAVIDSON_SYM_EIGS_SOLVER_H 9 | 10 | #include 11 | 12 | #include "JDSymEigsBase.h" 13 | #include "Util/SelectionRule.h" 14 | 15 | namespace Spectra { 16 | 17 | /// 18 | /// \ingroup EigenSolver 19 | /// 20 | /// This class implement the DPR correction for the Davidson algorithms. 21 | /// The algorithms in the Davidson family only differ in how the correction 22 | /// vectors are computed and optionally in the initial orthogonal basis set. 23 | /// 24 | /// the DPR correction compute the new correction vector using the following expression: 25 | /// \f[ correction = -(\boldsymbol{D} - \rho \boldsymbol{I})^{-1} \boldsymbol{r} \f] 26 | /// where 27 | /// \f$D\f$ is the diagonal of the target matrix, \f$\rho\f$ the Ritz eigenvalue, 28 | /// \f$I\f$ the identity matrix and \f$r\f$ the residue vector. 29 | /// 30 | template 31 | class DavidsonSymEigsSolver : public JDSymEigsBase, OpType> 32 | { 33 | private: 34 | using Index = Eigen::Index; 35 | using Scalar = typename OpType::Scalar; 36 | using Matrix = Eigen::Matrix; 37 | using Vector = Eigen::Matrix; 38 | 39 | Vector m_diagonal; 40 | 41 | public: 42 | DavidsonSymEigsSolver(OpType& op, Index nev, Index nvec_init, Index nvec_max) : 43 | JDSymEigsBase, OpType>(op, nev, nvec_init, nvec_max) 44 | { 45 | m_diagonal.resize(this->m_matrix_operator.rows()); 46 | for (Index i = 0; i < op.rows(); i++) 47 | { 48 | m_diagonal(i) = op(i, i); 49 | } 50 | } 51 | 52 | DavidsonSymEigsSolver(OpType& op, Index nev) : 53 | DavidsonSymEigsSolver(op, nev, 2 * nev, 10 * nev) {} 54 | 55 | /// Create initial search space based on the diagonal 56 | /// and the spectrum'target (highest or lowest) 57 | /// 58 | /// \param selection Spectrum section to target (e.g. lowest, etc.) 59 | /// \return Matrix with the initial orthonormal basis 60 | Matrix setup_initial_search_space(SortRule selection) const 61 | { 62 | std::vector indices_sorted = argsort(selection, m_diagonal); 63 | 64 | Matrix initial_basis = Matrix::Zero(this->m_matrix_operator.rows(), this->m_initial_search_space_size); 65 | 66 | for (Index k = 0; k < this->m_initial_search_space_size; k++) 67 | { 68 | Index row = indices_sorted[k]; 69 | initial_basis(row, k) = 1.0; 70 | } 71 | return initial_basis; 72 | } 73 | 74 | /// Compute the corrections using the DPR method. 75 | /// 76 | /// \return New correction vectors. 77 | Matrix calculate_correction_vector() const 78 | { 79 | const Matrix& residues = this->m_ritz_pairs.residues(); 80 | const Vector& eigvals = this->m_ritz_pairs.ritz_values(); 81 | Matrix correction = Matrix::Zero(this->m_matrix_operator.rows(), this->m_correction_size); 82 | for (Index k = 0; k < this->m_correction_size; k++) 83 | { 84 | Vector tmp = eigvals(k) - m_diagonal.array(); 85 | correction.col(k) = residues.col(k).array() / tmp.array(); 86 | } 87 | return correction; 88 | } 89 | }; 90 | 91 | } // namespace Spectra 92 | 93 | #endif // SPECTRA_DAVIDSON_SYM_EIGS_SOLVER_H 94 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/internal/SymGEigsCayleyOp.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2020-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SYM_GEIGS_CAYLEY_OP_H 8 | #define SPECTRA_SYM_GEIGS_CAYLEY_OP_H 9 | 10 | #include 11 | 12 | #include "../SymShiftInvert.h" 13 | #include "../SparseSymMatProd.h" 14 | 15 | namespace Spectra { 16 | 17 | /// 18 | /// \ingroup Operators 19 | /// 20 | /// This class defines the matrix operation for generalized eigen solver in the 21 | /// Cayley mode. It computes \f$y=(A-\sigma B)^{-1}(A+\sigma B)x\f$ for any 22 | /// vector \f$x\f$, where \f$A\f$ is a symmetric matrix, \f$B\f$ is positive definite, 23 | /// and \f$\sigma\f$ is a real shift. 24 | /// This class is intended for internal use. 25 | /// 26 | template , 27 | typename BOpType = SparseSymMatProd> 28 | class SymGEigsCayleyOp 29 | { 30 | public: 31 | using Scalar = typename OpType::Scalar; 32 | 33 | private: 34 | using Index = Eigen::Index; 35 | using Vector = Eigen::Matrix; 36 | using MapConstVec = Eigen::Map; 37 | using MapVec = Eigen::Map; 38 | 39 | OpType& m_op; 40 | const BOpType& m_Bop; 41 | mutable Vector m_cache; // temporary working space 42 | Scalar m_sigma; 43 | 44 | public: 45 | /// 46 | /// Constructor to create the matrix operation object. 47 | /// 48 | /// \param op The \f$(A-\sigma B)^{-1}\f$ matrix operation object. 49 | /// \param Bop The \f$B\f$ matrix operation object. 50 | /// 51 | SymGEigsCayleyOp(OpType& op, const BOpType& Bop) : 52 | m_op(op), m_Bop(Bop), m_cache(op.rows()) 53 | {} 54 | 55 | /// 56 | /// Move constructor. 57 | /// 58 | SymGEigsCayleyOp(SymGEigsCayleyOp&& other) : 59 | m_op(other.m_op), m_Bop(other.m_Bop), m_sigma(other.m_sigma) 60 | { 61 | // We emulate the move constructor for Vector using Vector::swap() 62 | m_cache.swap(other.m_cache); 63 | } 64 | 65 | /// 66 | /// Return the number of rows of the underlying matrix. 67 | /// 68 | Index rows() const { return m_op.rows(); } 69 | /// 70 | /// Return the number of columns of the underlying matrix. 71 | /// 72 | Index cols() const { return m_op.rows(); } 73 | 74 | /// 75 | /// Set the real shift \f$\sigma\f$. 76 | /// 77 | void set_shift(const Scalar& sigma) 78 | { 79 | m_op.set_shift(sigma); 80 | m_sigma = sigma; 81 | } 82 | 83 | /// 84 | /// Perform the matrix operation \f$y=(A-\sigma B)^{-1}(A+\sigma B)x\f$. 85 | /// 86 | /// \param x_in Pointer to the \f$x\f$ vector. 87 | /// \param y_out Pointer to the \f$y\f$ vector. 88 | /// 89 | // y_out = inv(A - sigma * B) * (A + sigma * B) * x_in 90 | void perform_op(const Scalar* x_in, Scalar* y_out) const 91 | { 92 | // inv(A - sigma * B) * (A + sigma * B) * x 93 | // = inv(A - sigma * B) * (A - sigma * B + 2 * sigma * B) * x 94 | // = x + 2 * sigma * inv(A - sigma * B) * B * x 95 | m_Bop.perform_op(x_in, m_cache.data()); 96 | m_op.perform_op(m_cache.data(), y_out); 97 | MapConstVec x(x_in, this->rows()); 98 | MapVec y(y_out, this->rows()); 99 | y.noalias() = x + (Scalar(2) * m_sigma) * y; 100 | } 101 | }; 102 | 103 | } // namespace Spectra 104 | 105 | #endif // SPECTRA_SYM_GEIGS_CAYLEY_OP_H 106 | -------------------------------------------------------------------------------- /cmake/CMakeHelper.cmake: -------------------------------------------------------------------------------- 1 | if(POLICY CMP0043) 2 | cmake_policy(SET CMP0043 NEW) 3 | endif() 4 | 5 | if(POLICY CMP0054) 6 | cmake_policy(SET CMP0054 NEW) 7 | endif() 8 | 9 | # Determine project compiler. 10 | if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") 11 | set(IS_MSVC TRUE) 12 | endif() 13 | if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 14 | set(IS_GNU TRUE) 15 | endif() 16 | if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") 17 | set(IS_CLANG TRUE) 18 | endif() 19 | 20 | # Determine project architecture. 21 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "[ix].?86|amd64|AMD64") 22 | set(IS_X86 TRUE) 23 | endif() 24 | 25 | # Determine project operating system. 26 | string(REGEX MATCH "Linux" IS_LINUX ${CMAKE_SYSTEM_NAME}) 27 | string(REGEX MATCH "DragonFly|BSD" IS_BSD ${CMAKE_SYSTEM_NAME}) 28 | string(REGEX MATCH "SunOS" IS_SOLARIS ${CMAKE_SYSTEM_NAME}) 29 | if(WIN32) 30 | SET(IS_WINDOWS TRUE BOOL INTERNAL) 31 | endif() 32 | if(APPLE) 33 | SET(IS_MACOS TRUE BOOL INTERNAL) 34 | endif() 35 | 36 | string(TOLOWER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_LOWER) 37 | if(CMAKE_BUILD_TYPE_LOWER STREQUAL "debug" 38 | OR CMAKE_BUILD_TYPE_LOWER STREQUAL "relwithdebinfo") 39 | set(IS_DEBUG TRUE) 40 | endif() 41 | 42 | set(GOPT_LIB "gopt") 43 | 44 | # Macro to add source files to GOPT library. 45 | macro(OPTIMIZER_ADD_SOURCES) 46 | set(SOURCE_FILES "") 47 | foreach(SOURCE_FILE ${ARGN}) 48 | if(SOURCE_FILE MATCHES "^/.*") 49 | list(APPEND SOURCE_FILES ${SOURCE_FILE}) 50 | else() 51 | list(APPEND SOURCE_FILES 52 | "${CMAKE_CURRENT_SOURCE_DIR}/${SOURCE_FILE}") 53 | endif() 54 | endforeach() 55 | set(GRAPH_OPTIMIZER_SOURCES 56 | ${GRAPH_OPTIMIZER_SOURCES} ${SOURCE_FILES} PARENT_SCOPE) 57 | endmacro(OPTIMIZER_ADD_SOURCES) 58 | 59 | # Macro to add header files to GOPT library. 60 | macro(OPTIMIZER_ADD_HEADERS) 61 | set(SOURCE_FILES "") 62 | foreach(SOURCE_FILE ${ARGN}) 63 | if(SOURCE_FILE MATCHES "^/.*") 64 | list(APPEND SOURCE_FILES ${SOURCE_FILE}) 65 | else() 66 | list(APPEND SOURCE_FILES 67 | "${CMAKE_CURRENT_SOURCE_DIR}/${SOURCE_FILE}") 68 | endif() 69 | endforeach() 70 | set(GRAPH_OPTIMIZER_HEADERS 71 | ${GRAPH_OPTIMIZER_HEADERS} ${SOURCE_FILES} PARENT_SCOPE) 72 | endmacro(OPTIMIZER_ADD_HEADERS) 73 | 74 | macro(OPTIMIZER_ADD_GTEST TEST_TARGET_NAME TEST_SOURCE_FILE) 75 | # set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin/test) 76 | 77 | add_executable(${TEST_TARGET_NAME} ${TEST_SOURCE_FILE}) 78 | target_link_libraries(${TEST_TARGET_NAME} 79 | ${GOPT_EXTERNAL_LIBRARIES} ${GOPT_INTERNAL_LIBRARIES} ${GOPT_LIB}) 80 | add_test(${TEST_TARGET_NAME} ${EXECUTATLE_OUTPUT_PATH}/${TEST_TARGET_NAME}) 81 | endmacro(OPTIMIZER_ADD_GTEST) 82 | 83 | macro(OPTIMIZER_ADD_EXE TARGET_NAME SOURCE_FILE) 84 | # set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin/test) 85 | add_executable(${TARGET_NAME} ${SOURCE_FILE}) 86 | target_link_libraries(${TARGET_NAME} 87 | ${GOPT_EXTERNAL_LIBRARIES} ${GOPT_INTERNAL_LIBRARIES} ${GOPT_LIB}) 88 | endmacro(OPTIMIZER_ADD_EXE) 89 | 90 | # Replacement for the normal add_library() command. The syntax remains the same 91 | # in that the first argument is the target name, and the following arguments 92 | # are the source files to use when building the target. 93 | macro(OPTIMIZER_ADD_LIBRARY TARGET_NAME) 94 | # ${ARGN} will store the list of source files passed to this function. 95 | add_library(${TARGET_NAME} ${ARGN}) 96 | # set_target_properties(${TARGET_NAME} PROPERTIES FOLDER 97 | # ${OPTIMIZER_TARGETS_ROOT_FOLDER}/${FOLDER_NAME}) 98 | # install(TARGETS ${TARGET_NAME} DESTINATION 3rd_party/gopt) 99 | endmacro(OPTIMIZER_ADD_LIBRARY) 100 | -------------------------------------------------------------------------------- /src/rotation_averaging/robust_l1l2_rotation_estimator.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef ROTATION_AVERAGING_ROBUST_L1L2_ROTATION_ESTIMATOR_H_ 32 | #define ROTATION_AVERAGING_ROBUST_L1L2_ROTATION_ESTIMATOR_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "rotation_averaging/rotation_estimator.h" 38 | #include "rotation_averaging/irls_rotation_local_refiner.h" 39 | #include "rotation_averaging/l1_rotation_global_estimator.h" 40 | #include "solver/sdp_solver.h" 41 | #include "solver/solver_options.h" 42 | #include "utils/hash.h" 43 | #include "utils/types.h" 44 | 45 | namespace gopt { 46 | 47 | class RobustL1L2RotationEstimator : public RotationEstimator { 48 | public: 49 | struct RobustL1L2RotationEstimatorOptions { 50 | bool verbose = true; 51 | L1RotationGlobalEstimator::L1RotationOptions l1_options; 52 | IRLSRotationLocalRefiner::IRLSRefinerOptions irls_options; 53 | 54 | void Setup() { 55 | l1_options.verbose = verbose; 56 | irls_options.verbose = verbose; 57 | } 58 | }; 59 | 60 | RobustL1L2RotationEstimator( 61 | const RobustL1L2RotationEstimatorOptions& options); 62 | 63 | // Estimate the absolute rotations, given pairs of relative rotations 64 | bool EstimateRotations( 65 | const std::unordered_map& view_pairs, 66 | std::unordered_map* global_rotations) override; 67 | 68 | private: 69 | void GlobalRotationsToTangentSpace( 70 | const std::unordered_map& global_rotations, 71 | Eigen::VectorXd* tangent_space_step); 72 | 73 | RobustL1L2RotationEstimatorOptions options_; 74 | 75 | // this hash table is used for non-continuous index, such as 76 | // unordered internet datasets that composed of many unconnected components 77 | std::unordered_map view_id_to_index_; 78 | 79 | std::unique_ptr l1_rotation_estimator_; 80 | std::unique_ptr irls_rotation_refiner_; 81 | }; 82 | 83 | } // namespace gopt 84 | 85 | #endif // ROTATION_AVERAGING_ROBUST_L1L2_ROTATION_ESTIMATOR_H_ 86 | -------------------------------------------------------------------------------- /src/rotation_averaging/hybrid_rotation_estimator.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef ROTATION_AVERAGING_HYBRID_ROTATION_AVERAGING_H_ 32 | #define ROTATION_AVERAGING_HYBRID_ROTATION_AVERAGING_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "rotation_averaging/rotation_estimator.h" 38 | #include "rotation_averaging/irls_rotation_local_refiner.h" 39 | #include "rotation_averaging/lagrange_dual_rotation_estimator.h" 40 | #include "solver/sdp_solver.h" 41 | #include "solver/solver_options.h" 42 | #include "utils/hash.h" 43 | #include "utils/types.h" 44 | 45 | namespace gopt { 46 | 47 | class HybridRotationEstimator : public RotationEstimator { 48 | public: 49 | struct HybridRotationEstimatorOptions { 50 | solver::SDPSolverOptions sdp_solver_options; 51 | IRLSRotationLocalRefiner::IRLSRefinerOptions irls_options; 52 | }; 53 | 54 | HybridRotationEstimator(const int N, const int dim); 55 | HybridRotationEstimator( 56 | const int N, const int dim, 57 | const HybridRotationEstimator::HybridRotationEstimatorOptions& options); 58 | 59 | // Estimate the absolute rotations, given pairs of relative rotations 60 | bool EstimateRotations( 61 | const std::unordered_map& view_pairs, 62 | std::unordered_map* global_rotations) override; 63 | 64 | private: 65 | void GlobalRotationsToTangentSpace( 66 | const std::unordered_map& global_rotations, 67 | Eigen::VectorXd* tangent_space_step); 68 | 69 | HybridRotationEstimatorOptions options_; 70 | 71 | // number of images/frames 72 | int images_num_; 73 | 74 | int dim_; 75 | 76 | // this hash table is used for non-continuous index, such as 77 | // unordered internet datasets that composed of many unconnected components 78 | std::unordered_map view_id_to_index_; 79 | 80 | std::unique_ptr ld_rotation_estimator_; 81 | std::unique_ptr irls_rotation_refiner_; 82 | }; 83 | 84 | } // namespace gopt 85 | 86 | #endif // ROTATION_AVERAGING_HYBRID_ROTATION_AVERAGING_H_ 87 | -------------------------------------------------------------------------------- /3rd_party/Spectra/LinAlg/SearchSpace.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2020 Netherlands eScience Center 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SEARCH_SPACE_H 8 | #define SPECTRA_SEARCH_SPACE_H 9 | 10 | #include 11 | 12 | #include "RitzPairs.h" 13 | #include "Orthogonalization.h" 14 | 15 | namespace Spectra { 16 | 17 | /// This class handles the creation and manipulation of the search space 18 | /// for iterative eigensolvers such as Davidson, Jacobi-Davidson, etc. 19 | template 20 | class SearchSpace 21 | { 22 | private: 23 | using Index = Eigen::Index; 24 | using Matrix = Eigen::Matrix; 25 | 26 | Matrix m_basis_vectors; 27 | Matrix m_op_basis_product; 28 | 29 | /// Append new vector to the basis 30 | /// 31 | /// \param new_vect Matrix of new correction vectors 32 | void append_new_vectors_to_basis(const Matrix& new_vect) 33 | { 34 | Index num_update = new_vect.cols(); 35 | m_basis_vectors.conservativeResize(Eigen::NoChange, m_basis_vectors.cols() + num_update); 36 | m_basis_vectors.rightCols(num_update).noalias() = new_vect; 37 | } 38 | 39 | public: 40 | SearchSpace() = default; 41 | 42 | /// Returns the current size of the search space 43 | Index size() const { return m_basis_vectors.cols(); } 44 | 45 | void initialize_search_space(const Eigen::Ref& initial_vectors) 46 | { 47 | m_basis_vectors = initial_vectors; 48 | m_op_basis_product = Matrix(initial_vectors.rows(), 0); 49 | } 50 | 51 | /// Updates the matrix formed by the operator applied to the search space 52 | /// after the addition of new vectors in the search space. Only the product 53 | /// of the operator with the new vectors is computed and the result is appended 54 | /// to the op_basis_product member variable 55 | /// 56 | /// \param OpType Operator representing the matrix 57 | template 58 | void update_operator_basis_product(OpType& op) 59 | { 60 | Index nvec = m_basis_vectors.cols() - m_op_basis_product.cols(); 61 | m_op_basis_product.conservativeResize(Eigen::NoChange, m_basis_vectors.cols()); 62 | m_op_basis_product.rightCols(nvec).noalias() = op * m_basis_vectors.rightCols(nvec); 63 | } 64 | 65 | /// Restart the search space by reducing the basis vector to the last 66 | /// Ritz eigenvector 67 | /// 68 | /// \param ritz_pair Instance of a RitzPair class 69 | /// \param size Size of the restart 70 | void restart(const RitzPairs& ritz_pairs, Index size) 71 | { 72 | m_basis_vectors = ritz_pairs.ritz_vectors().leftCols(size); 73 | m_op_basis_product = m_op_basis_product * ritz_pairs.small_ritz_vectors().leftCols(size); 74 | } 75 | 76 | /// Append new vectors to the search space and 77 | /// orthogonalize the resulting matrix 78 | /// 79 | /// \param new_vect Matrix of new correction vectors 80 | void extend_basis(const Matrix& new_vect) 81 | { 82 | Index left_cols_to_skip = size(); 83 | append_new_vectors_to_basis(new_vect); 84 | twice_is_enough_orthogonalisation(m_basis_vectors, left_cols_to_skip); 85 | } 86 | 87 | /// Returns the basis vectors 88 | const Matrix& basis_vectors() const { return m_basis_vectors; } 89 | 90 | /// Returns the operator applied to basis vector 91 | const Matrix& operator_basis_product() const { return m_op_basis_product; } 92 | }; 93 | 94 | } // namespace Spectra 95 | 96 | #endif // SPECTRA_SEARCH_SPACE_H 97 | -------------------------------------------------------------------------------- /src/solver/solver_options.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef SOLVER_SOLVER_OPTIONS_H_ 32 | #define SOLVER_SOLVER_OPTIONS_H_ 33 | 34 | #include 35 | 36 | namespace gopt { 37 | namespace solver { 38 | 39 | enum SDPSolverType { 40 | RBR_BCM, 41 | RANK_DEFICIENT_BCM, 42 | RIEMANNIAN_STAIRCASE, 43 | SE_SYNC, // not implemented 44 | SHONAN // not implemented 45 | }; 46 | 47 | enum PreconditionerType { 48 | None, 49 | JACOBI, 50 | INCOMPLETE_CHOLESKY, 51 | REGULARIZED_CHOLESKY 52 | }; 53 | 54 | struct RiemannianStaircaseOptions { 55 | size_t min_rank = 3; 56 | size_t max_rank = 10; 57 | 58 | size_t max_eigen_solver_iterations = 20; 59 | 60 | double min_eigenvalue_nonnegativity_tolerance = 1e-5; 61 | 62 | size_t num_Lanczos_vectors = 20; 63 | 64 | SDPSolverType local_solver_type = SDPSolverType::RANK_DEFICIENT_BCM; 65 | 66 | double gradient_tolerance = 1e-2; 67 | 68 | double preconditioned_gradient_tolerance = 1e-4; 69 | }; 70 | 71 | struct SDPSolverOptions { 72 | // maximum iteration number 73 | size_t max_iterations = 500; 74 | 75 | // tolerance for convergence 76 | double tolerance = 1e-8; 77 | 78 | bool verbose = true; 79 | 80 | int num_threads = 8; 81 | 82 | SDPSolverType solver_type = RIEMANNIAN_STAIRCASE; 83 | 84 | PreconditionerType preconditioner_type = PreconditionerType::None; 85 | 86 | RiemannianStaircaseOptions riemannian_staircase_options; 87 | 88 | SDPSolverOptions(size_t max_iter = 500, double tol = 1e-8, bool log = true) { 89 | max_iterations = max_iter; 90 | tolerance = tol; 91 | verbose = log; 92 | solver_type = RIEMANNIAN_STAIRCASE; 93 | } 94 | 95 | SDPSolverOptions(const SDPSolverOptions& option) { 96 | max_iterations = option.max_iterations; 97 | tolerance = option.tolerance; 98 | verbose = option.verbose; 99 | solver_type = option.solver_type; 100 | riemannian_staircase_options = option.riemannian_staircase_options; 101 | } 102 | }; 103 | 104 | } // namespace solver 105 | } // namespace gopt 106 | 107 | #endif // SOLVER_SOLVER_OPTIONS_H_ 108 | -------------------------------------------------------------------------------- /3rd_party/Spectra/GenEigsRealShiftSolver.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_GEN_EIGS_REAL_SHIFT_SOLVER_H 8 | #define SPECTRA_GEN_EIGS_REAL_SHIFT_SOLVER_H 9 | 10 | #include 11 | 12 | #include "GenEigsBase.h" 13 | #include "Util/SelectionRule.h" 14 | #include "MatOp/DenseGenRealShiftSolve.h" 15 | 16 | namespace Spectra { 17 | 18 | /// 19 | /// \ingroup EigenSolver 20 | /// 21 | /// This class implements the eigen solver for general real matrices with 22 | /// a real shift value in the **shift-and-invert mode**. The background 23 | /// knowledge of the shift-and-invert mode can be found in the documentation 24 | /// of the SymEigsShiftSolver class. 25 | /// 26 | /// \tparam OpType The name of the matrix operation class. Users could either 27 | /// use the wrapper classes such as DenseGenRealShiftSolve and 28 | /// SparseGenRealShiftSolve, or define their own that implements the type 29 | /// definition `Scalar` and all the public member functions as in 30 | /// DenseGenRealShiftSolve. 31 | /// 32 | template > 33 | class GenEigsRealShiftSolver : public GenEigsBase 34 | { 35 | private: 36 | using Scalar = typename OpType::Scalar; 37 | using Index = Eigen::Index; 38 | using Complex = std::complex; 39 | using ComplexArray = Eigen::Array; 40 | 41 | using Base = GenEigsBase; 42 | using Base::m_nev; 43 | using Base::m_ritz_val; 44 | 45 | const Scalar m_sigma; 46 | 47 | // First transform back the Ritz values, and then sort 48 | void sort_ritzpair(SortRule sort_rule) override 49 | { 50 | // The eigenvalues we get from the iteration is nu = 1 / (lambda - sigma) 51 | // So the eigenvalues of the original problem is lambda = 1 / nu + sigma 52 | m_ritz_val.head(m_nev) = Scalar(1) / m_ritz_val.head(m_nev).array() + m_sigma; 53 | Base::sort_ritzpair(sort_rule); 54 | } 55 | 56 | public: 57 | /// 58 | /// Constructor to create a eigen solver object using the shift-and-invert mode. 59 | /// 60 | /// \param op The matrix operation object that implements 61 | /// the shift-solve operation of \f$A\f$: calculating 62 | /// \f$(A-\sigma I)^{-1}v\f$ for any vector \f$v\f$. Users could either 63 | /// create the object from the wrapper class such as DenseGenRealShiftSolve, or 64 | /// define their own that implements all the public members 65 | /// as in DenseGenRealShiftSolve. 66 | /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$, 67 | /// where \f$n\f$ is the size of matrix. 68 | /// \param ncv Parameter that controls the convergence speed of the algorithm. 69 | /// Typically a larger `ncv` means faster convergence, but it may 70 | /// also result in greater memory use and more matrix operations 71 | /// in each iteration. This parameter must satisfy \f$nev+2 \le ncv \le n\f$, 72 | /// and is advised to take \f$ncv \ge 2\cdot nev + 1\f$. 73 | /// \param sigma The real-valued shift. 74 | /// 75 | GenEigsRealShiftSolver(OpType& op, Index nev, Index ncv, const Scalar& sigma) : 76 | Base(op, IdentityBOp(), nev, ncv), 77 | m_sigma(sigma) 78 | { 79 | op.set_shift(m_sigma); 80 | } 81 | }; 82 | 83 | } // namespace Spectra 84 | 85 | #endif // SPECTRA_GEN_EIGS_REAL_SHIFT_SOLVER_H 86 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/DenseGenRealShiftSolve.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_DENSE_GEN_REAL_SHIFT_SOLVE_H 8 | #define SPECTRA_DENSE_GEN_REAL_SHIFT_SOLVE_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Spectra { 15 | 16 | /// 17 | /// \ingroup MatOp 18 | /// 19 | /// This class defines the shift-solve operation on a general real matrix \f$A\f$, 20 | /// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and 21 | /// vector \f$x\f$. It is mainly used in the GenEigsRealShiftSolver eigen solver. 22 | /// 23 | /// \tparam Scalar_ The element type of the matrix, for example, 24 | /// `float`, `double`, and `long double`. 25 | /// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating 26 | /// the storage format of the input matrix. 27 | /// 28 | template 29 | class DenseGenRealShiftSolve 30 | { 31 | public: 32 | /// 33 | /// Element type of the matrix. 34 | /// 35 | using Scalar = Scalar_; 36 | 37 | private: 38 | using Index = Eigen::Index; 39 | using Matrix = Eigen::Matrix; 40 | using Vector = Eigen::Matrix; 41 | using MapConstVec = Eigen::Map; 42 | using MapVec = Eigen::Map; 43 | using ConstGenericMatrix = const Eigen::Ref; 44 | 45 | ConstGenericMatrix m_mat; 46 | const Index m_n; 47 | Eigen::PartialPivLU m_solver; 48 | 49 | public: 50 | /// 51 | /// Constructor to create the matrix operation object. 52 | /// 53 | /// \param mat An **Eigen** matrix object, whose type can be 54 | /// `Eigen::Matrix` (e.g. `Eigen::MatrixXd` and 55 | /// `Eigen::MatrixXf`), or its mapped version 56 | /// (e.g. `Eigen::Map`). 57 | /// 58 | template 59 | DenseGenRealShiftSolve(const Eigen::MatrixBase& mat) : 60 | m_mat(mat), m_n(mat.rows()) 61 | { 62 | static_assert( 63 | static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), 64 | "DenseGenRealShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); 65 | 66 | if (mat.rows() != mat.cols()) 67 | throw std::invalid_argument("DenseGenRealShiftSolve: matrix must be square"); 68 | } 69 | 70 | /// 71 | /// Return the number of rows of the underlying matrix. 72 | /// 73 | Index rows() const { return m_n; } 74 | /// 75 | /// Return the number of columns of the underlying matrix. 76 | /// 77 | Index cols() const { return m_n; } 78 | 79 | /// 80 | /// Set the real shift \f$\sigma\f$. 81 | /// 82 | void set_shift(const Scalar& sigma) 83 | { 84 | m_solver.compute(m_mat - sigma * Matrix::Identity(m_n, m_n)); 85 | } 86 | 87 | /// 88 | /// Perform the shift-solve operation \f$y=(A-\sigma I)^{-1}x\f$. 89 | /// 90 | /// \param x_in Pointer to the \f$x\f$ vector. 91 | /// \param y_out Pointer to the \f$y\f$ vector. 92 | /// 93 | // y_out = inv(A - sigma * I) * x_in 94 | void perform_op(const Scalar* x_in, Scalar* y_out) const 95 | { 96 | MapConstVec x(x_in, m_n); 97 | MapVec y(y_out, m_n); 98 | y.noalias() = m_solver.solve(x); 99 | } 100 | }; 101 | 102 | } // namespace Spectra 103 | 104 | #endif // SPECTRA_DENSE_GEN_REAL_SHIFT_SOLVE_H 105 | -------------------------------------------------------------------------------- /src/solver/sdp_solver.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef SOLVER_SDP_SOLVER_H_ 32 | #define SOLVER_SDP_SOLVER_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include "solver/solver_options.h" 42 | #include "solver/summary.h" 43 | 44 | namespace gopt { 45 | namespace solver { 46 | 47 | class SDPSolver { 48 | public: 49 | SDPSolver(const size_t n, const size_t block_dim) 50 | : SDPSolver(n, block_dim, solver::SDPSolverOptions()) { } 51 | 52 | SDPSolver(const size_t n, const size_t block_dim, 53 | const solver::SDPSolverOptions& options) 54 | : n_(n), 55 | dim_(block_dim), 56 | sdp_solver_options_(options) { 57 | Q_ = Eigen::SparseMatrix(dim_ * n, dim_ * n); 58 | } 59 | 60 | virtual ~SDPSolver() {} 61 | 62 | void SetSolverOptions(const solver::SDPSolverOptions& options) { 63 | sdp_solver_options_ = options; 64 | } 65 | 66 | virtual void SetCovariance(const Eigen::SparseMatrix& Q) { 67 | Q_ = Q; 68 | } 69 | 70 | virtual void SetAdjacentEdges( 71 | const std::unordered_map>& adj_edges) { 72 | adj_edges_ = adj_edges; 73 | } 74 | 75 | size_t NumUnknowns() const { 76 | return n_; 77 | } 78 | 79 | size_t Dimension() const { 80 | return dim_; 81 | } 82 | 83 | // Get the final optimal solution 84 | virtual Eigen::MatrixXd GetSolution() const = 0; 85 | 86 | virtual void Solve(solver::Summary& summary) = 0; 87 | 88 | virtual double EvaluateFuncVal() const = 0; 89 | virtual double EvaluateFuncVal(const Eigen::MatrixXd& Y) const = 0; 90 | 91 | protected: 92 | // number of unknown blocks. 93 | size_t n_; 94 | 95 | // the dimension of matrix sub-block. 96 | size_t dim_; 97 | 98 | // covariance matrix in SDP problem: min tr(QX). 99 | Eigen::SparseMatrix Q_; 100 | 101 | std::unordered_map> adj_edges_; 102 | 103 | solver::SDPSolverOptions sdp_solver_options_; 104 | }; 105 | 106 | } // namespace solver 107 | } // namespace gopt 108 | 109 | #endif // SOLVER_SDP_SOLVER_H_ 110 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/DenseGenMatProd.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_DENSE_GEN_MAT_PROD_H 8 | #define SPECTRA_DENSE_GEN_MAT_PROD_H 9 | 10 | #include 11 | 12 | namespace Spectra { 13 | 14 | /// 15 | /// \defgroup MatOp Matrix Operations 16 | /// 17 | /// Define matrix operations on existing matrix objects 18 | /// 19 | 20 | /// 21 | /// \ingroup MatOp 22 | /// 23 | /// This class defines the matrix-vector multiplication operation on a 24 | /// general real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector 25 | /// \f$x\f$. It is mainly used in the GenEigsSolver and 26 | /// SymEigsSolver eigen solvers. 27 | /// 28 | /// \tparam Scalar_ The element type of the matrix, for example, 29 | /// `float`, `double`, and `long double`. 30 | /// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating 31 | /// the storage format of the input matrix. 32 | /// 33 | template 34 | class DenseGenMatProd 35 | { 36 | public: 37 | /// 38 | /// Element type of the matrix. 39 | /// 40 | using Scalar = Scalar_; 41 | 42 | private: 43 | using Index = Eigen::Index; 44 | using Matrix = Eigen::Matrix; 45 | using Vector = Eigen::Matrix; 46 | using MapConstVec = Eigen::Map; 47 | using MapVec = Eigen::Map; 48 | using ConstGenericMatrix = const Eigen::Ref; 49 | 50 | ConstGenericMatrix m_mat; 51 | 52 | public: 53 | /// 54 | /// Constructor to create the matrix operation object. 55 | /// 56 | /// \param mat An **Eigen** matrix object, whose type can be 57 | /// `Eigen::Matrix` (e.g. `Eigen::MatrixXd` and 58 | /// `Eigen::MatrixXf`), or its mapped version 59 | /// (e.g. `Eigen::Map`). 60 | /// 61 | template 62 | DenseGenMatProd(const Eigen::MatrixBase& mat) : 63 | m_mat(mat) 64 | { 65 | static_assert( 66 | static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), 67 | "DenseGenMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); 68 | } 69 | 70 | /// 71 | /// Return the number of rows of the underlying matrix. 72 | /// 73 | Index rows() const { return m_mat.rows(); } 74 | /// 75 | /// Return the number of columns of the underlying matrix. 76 | /// 77 | Index cols() const { return m_mat.cols(); } 78 | 79 | /// 80 | /// Perform the matrix-vector multiplication operation \f$y=Ax\f$. 81 | /// 82 | /// \param x_in Pointer to the \f$x\f$ vector. 83 | /// \param y_out Pointer to the \f$y\f$ vector. 84 | /// 85 | // y_out = A * x_in 86 | void perform_op(const Scalar* x_in, Scalar* y_out) const 87 | { 88 | MapConstVec x(x_in, m_mat.cols()); 89 | MapVec y(y_out, m_mat.rows()); 90 | y.noalias() = m_mat * x; 91 | } 92 | 93 | /// 94 | /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. 95 | /// 96 | Matrix operator*(const Eigen::Ref& mat_in) const 97 | { 98 | return m_mat * mat_in; 99 | } 100 | 101 | /// 102 | /// Extract (i,j) element of the underlying matrix. 103 | /// 104 | Scalar operator()(Index i, Index j) const 105 | { 106 | return m_mat(i, j); 107 | } 108 | }; 109 | 110 | } // namespace Spectra 111 | 112 | #endif // SPECTRA_DENSE_GEN_MAT_PROD_H 113 | -------------------------------------------------------------------------------- /src/solver/rbr_sdp_solver.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef SOLVER_RBR_SDP_SOLVER_H_ 32 | #define SOLVER_RBR_SDP_SOLVER_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "solver/bcm_sdp_solver.h" 40 | #include "solver/sdp_solver.h" 41 | 42 | namespace gopt { 43 | namespace solver { 44 | 45 | // Semidefinite positive solver with Block Coordinate Minimization(BCM) 46 | // This algorithm is a generalization of the RBR approach of the paper: 47 | // Z. Wen, D. Goldfarb, S. Ma, and K. Scheinberg. Row by row methods for 48 | // semidefinite programming. Technical report, Columbia University, 2009. 7 49 | // As introduced in Eriksson's paper 50 | // Eriksson A, Olsson C, Kahl F, et al. Rotation averaging and strong 51 | // duality[C]//Proceedings of the IEEE Conference on Computer Vision and 52 | // Pattern Recognition. 2018: 127-135. 53 | // This algorithm actually solves the SDP problem: 54 | // ----------------------------------------------------------------------------------------- 55 | // min. -tr(RY) 56 | // s.t. Y_{ii} = I_3, i=1, ..., n, 57 | // Y \succeq 0 58 | // ----------------------------------------------------------------------------------------- 59 | // and the optimal solution could be retrieved by reading the first three rows 60 | // of Y^* 61 | // 62 | class RBRSDPSolver : public BCMSDPSolver { 63 | public: 64 | RBRSDPSolver(const size_t n, const size_t block_dim); 65 | 66 | RBRSDPSolver(const size_t n, const size_t block_dim, 67 | const solver::SDPSolverOptions& options); 68 | 69 | void Solve(solver::Summary& summary) override; 70 | 71 | Eigen::MatrixXd GetSolution() const override { return X_; } 72 | 73 | double EvaluateFuncVal() const override; 74 | double EvaluateFuncVal(const Eigen::MatrixXd& Y) const override; 75 | 76 | private: 77 | void ReformingB(const size_t k, Eigen::MatrixXd& Bk); 78 | void ReformingW(const size_t k, Eigen::MatrixXd& Wk); 79 | void ReorderingUnknown(const size_t k, const Eigen::MatrixXd& B, 80 | const Eigen::MatrixXd& W); 81 | 82 | Eigen::MatrixXd X_; 83 | }; 84 | 85 | } // namespace solver 86 | } // namespace gopt 87 | 88 | #endif // SOLVER_RBR_SDP_SOLVER_H_ 89 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/DenseSymMatProd.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_DENSE_SYM_MAT_PROD_H 8 | #define SPECTRA_DENSE_SYM_MAT_PROD_H 9 | 10 | #include 11 | 12 | namespace Spectra { 13 | 14 | /// 15 | /// \ingroup MatOp 16 | /// 17 | /// This class defines the matrix-vector multiplication operation on a 18 | /// symmetric real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector 19 | /// \f$x\f$. It is mainly used in the SymEigsSolver eigen solver. 20 | /// 21 | /// \tparam Scalar_ The element type of the matrix, for example, 22 | /// `float`, `double`, and `long double`. 23 | /// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which 24 | /// triangular part of the matrix is used. 25 | /// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating 26 | /// the storage format of the input matrix. 27 | /// 28 | template 29 | class DenseSymMatProd 30 | { 31 | public: 32 | /// 33 | /// Element type of the matrix. 34 | /// 35 | using Scalar = Scalar_; 36 | 37 | private: 38 | using Index = Eigen::Index; 39 | using Matrix = Eigen::Matrix; 40 | using Vector = Eigen::Matrix; 41 | using MapConstVec = Eigen::Map; 42 | using MapVec = Eigen::Map; 43 | using ConstGenericMatrix = const Eigen::Ref; 44 | 45 | ConstGenericMatrix m_mat; 46 | 47 | public: 48 | /// 49 | /// Constructor to create the matrix operation object. 50 | /// 51 | /// \param mat An **Eigen** matrix object, whose type can be 52 | /// `Eigen::Matrix` (e.g. `Eigen::MatrixXd` and 53 | /// `Eigen::MatrixXf`), or its mapped version 54 | /// (e.g. `Eigen::Map`). 55 | /// 56 | template 57 | DenseSymMatProd(const Eigen::MatrixBase& mat) : 58 | m_mat(mat) 59 | { 60 | static_assert( 61 | static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), 62 | "DenseSymMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); 63 | } 64 | 65 | /// 66 | /// Return the number of rows of the underlying matrix. 67 | /// 68 | Index rows() const { return m_mat.rows(); } 69 | /// 70 | /// Return the number of columns of the underlying matrix. 71 | /// 72 | Index cols() const { return m_mat.cols(); } 73 | 74 | /// 75 | /// Perform the matrix-vector multiplication operation \f$y=Ax\f$. 76 | /// 77 | /// \param x_in Pointer to the \f$x\f$ vector. 78 | /// \param y_out Pointer to the \f$y\f$ vector. 79 | /// 80 | // y_out = A * x_in 81 | void perform_op(const Scalar* x_in, Scalar* y_out) const 82 | { 83 | MapConstVec x(x_in, m_mat.cols()); 84 | MapVec y(y_out, m_mat.rows()); 85 | y.noalias() = m_mat.template selfadjointView() * x; 86 | } 87 | 88 | /// 89 | /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. 90 | /// 91 | Matrix operator*(const Eigen::Ref& mat_in) const 92 | { 93 | return m_mat.template selfadjointView() * mat_in; 94 | } 95 | 96 | /// 97 | /// Extract (i,j) element of the underlying matrix. 98 | /// 99 | Scalar operator()(Index i, Index j) const 100 | { 101 | return m_mat(i, j); 102 | } 103 | }; 104 | 105 | } // namespace Spectra 106 | 107 | #endif // SPECTRA_DENSE_SYM_MAT_PROD_H 108 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/SparseGenMatProd.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_SPARSE_GEN_MAT_PROD_H 8 | #define SPECTRA_SPARSE_GEN_MAT_PROD_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Spectra { 14 | /// 15 | /// \ingroup MatOp 16 | /// 17 | /// This class defines the matrix-vector multiplication operation on a 18 | /// sparse real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector 19 | /// \f$x\f$. It is mainly used in the GenEigsSolver and SymEigsSolver 20 | /// eigen solvers. 21 | /// 22 | /// \tparam Scalar_ The element type of the matrix, for example, 23 | /// `float`, `double`, and `long double`. 24 | /// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating 25 | /// the storage format of the input matrix. 26 | /// \tparam StorageIndex The type of the indices for the sparse matrix. 27 | /// 28 | template 29 | class SparseGenMatProd 30 | { 31 | public: 32 | /// 33 | /// Element type of the matrix. 34 | /// 35 | using Scalar = Scalar_; 36 | 37 | private: 38 | using Index = Eigen::Index; 39 | using Vector = Eigen::Matrix; 40 | using MapConstVec = Eigen::Map; 41 | using MapVec = Eigen::Map; 42 | using Matrix = Eigen::Matrix; 43 | using SparseMatrix = Eigen::SparseMatrix; 44 | using ConstGenericSparseMatrix = const Eigen::Ref; 45 | 46 | ConstGenericSparseMatrix m_mat; 47 | 48 | public: 49 | /// 50 | /// Constructor to create the matrix operation object. 51 | /// 52 | /// \param mat An **Eigen** sparse matrix object, whose type can be 53 | /// `Eigen::SparseMatrix` or its mapped version 54 | /// `Eigen::Map >`. 55 | /// 56 | template 57 | SparseGenMatProd(const Eigen::SparseMatrixBase& mat) : 58 | m_mat(mat) 59 | { 60 | static_assert( 61 | static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), 62 | "SparseGenMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); 63 | } 64 | 65 | /// 66 | /// Return the number of rows of the underlying matrix. 67 | /// 68 | Index rows() const { return m_mat.rows(); } 69 | /// 70 | /// Return the number of columns of the underlying matrix. 71 | /// 72 | Index cols() const { return m_mat.cols(); } 73 | 74 | /// 75 | /// Perform the matrix-vector multiplication operation \f$y=Ax\f$. 76 | /// 77 | /// \param x_in Pointer to the \f$x\f$ vector. 78 | /// \param y_out Pointer to the \f$y\f$ vector. 79 | /// 80 | // y_out = A * x_in 81 | void perform_op(const Scalar* x_in, Scalar* y_out) const 82 | { 83 | MapConstVec x(x_in, m_mat.cols()); 84 | MapVec y(y_out, m_mat.rows()); 85 | y.noalias() = m_mat * x; 86 | } 87 | 88 | /// 89 | /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. 90 | /// 91 | Matrix operator*(const Eigen::Ref& mat_in) const 92 | { 93 | return m_mat * mat_in; 94 | } 95 | 96 | /// 97 | /// Extract (i,j) element of the underlying matrix. 98 | /// 99 | Scalar operator()(Index i, Index j) const 100 | { 101 | return m_mat.coeff(i, j); 102 | } 103 | }; 104 | 105 | } // namespace Spectra 106 | 107 | #endif // SPECTRA_SPARSE_GEN_MAT_PROD_H 108 | -------------------------------------------------------------------------------- /applications/utils.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2022, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "utils.h" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace gopt { 39 | 40 | void LoadTracksFromDB( 41 | const std::string& database_path, 42 | TrackElements* track_elements, 43 | std::vector>* track_element_pairs) { 44 | colmap::Database database(database_path); 45 | LOG(INFO) << "Loading tracks..."; 46 | std::vector two_view_geometries; 47 | std::vector image_pair_ids; 48 | 49 | database.ReadTwoViewGeometries(&image_pair_ids, &two_view_geometries); 50 | 51 | std::unordered_map track_key_to_idx; 52 | 53 | // #pragma omp parallel for 54 | for (size_t i = 0; i < image_pair_ids.size(); i++) { 55 | image_t image_id1; 56 | image_t image_id2; 57 | colmap::Database::PairIdToImagePair(image_pair_ids[i], &image_id1, &image_id2); 58 | 59 | const colmap::TwoViewGeometry& two_view_geometry = two_view_geometries[i]; 60 | const auto& inlier_matches = two_view_geometry.inlier_matches; 61 | 62 | for (const colmap::FeatureMatch& match : inlier_matches) { 63 | const point2D_t point2d_idx1 = match.point2D_idx1; 64 | const point2D_t point2d_idx2 = match.point2D_idx2; 65 | const TrackElement track_element1(image_id1, point2d_idx1); 66 | const TrackElement track_element2(image_id2, point2d_idx2); 67 | 68 | const std::string track_key1 = 69 | std::to_string(image_id1) + "_" + std::to_string(point2d_idx1); 70 | const std::string track_key2 = 71 | std::to_string(image_id2) + "_" + std::to_string(point2d_idx2); 72 | 73 | if (track_key_to_idx.count(track_key1) == 0) { 74 | track_key_to_idx[track_key1] = track_key_to_idx.size(); 75 | track_elements->emplace_back(track_element1); 76 | } 77 | 78 | if (track_key_to_idx.count(track_key2) == 0) { 79 | track_key_to_idx[track_key2] = track_key_to_idx.size(); 80 | track_elements->emplace_back(track_element2); 81 | } 82 | 83 | const track_t track_id1 = track_key_to_idx.at(track_key1); 84 | const track_t track_id2 = track_key_to_idx.at(track_key2); 85 | track_element_pairs->emplace_back(track_id1, track_id2); 86 | } 87 | } 88 | } 89 | 90 | } // namespace gopt 91 | -------------------------------------------------------------------------------- /src/graph/edge.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef GRAPH_EDGE_H_ 32 | #define GRAPH_EDGE_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace gopt { 40 | namespace graph { 41 | 42 | typedef double weight_d; 43 | const weight_d kDefaultWeight = 1.0; 44 | const weight_d kInvalidWeight = std::numeric_limits::max(); 45 | 46 | struct Edge { 47 | node_t src = kInvalidNodeId; 48 | node_t dst = kInvalidNodeId; 49 | weight_d weight = kDefaultWeight; 50 | 51 | Edge() {} 52 | 53 | Edge(node_t i, node_t j) { 54 | src = i; 55 | dst = j; 56 | weight = kDefaultWeight; 57 | } 58 | 59 | Edge(node_t i, node_t j, weight_d w) { 60 | src = i; 61 | dst = j; 62 | weight = w; 63 | } 64 | 65 | Edge(const Edge& edge) { 66 | src = edge.src; 67 | dst = edge.dst; 68 | weight = edge.weight; 69 | } 70 | }; 71 | 72 | template 73 | struct CmpAscent { 74 | bool operator()(const EdgeType& edge1, const EdgeType& edge2) { 75 | return edge1.weight > edge2.weight; 76 | } 77 | }; 78 | 79 | template 80 | struct CmpDescent { 81 | bool operator()(const EdgeType& edge1, const EdgeType& edge2) { 82 | return edge1.weight < edge2.weight; 83 | } 84 | }; 85 | 86 | template 87 | using LargerEdgePriorityQueue = 88 | std::priority_queue, CmpDescent>; 89 | 90 | template 91 | using SmallerEdgePriorityQueue = 92 | std::priority_queue, CmpAscent>; 93 | 94 | struct ViewEdge : Edge { 95 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 96 | 97 | // Relative rotation from src to dst. 98 | Eigen::Vector3d rel_rotation; 99 | 100 | // Relative translation from src to dst. 101 | Eigen::Vector3d rel_translation; 102 | 103 | ViewEdge() : Edge() {} 104 | 105 | ViewEdge(node_t i, node_t j) : Edge(i, j) {} 106 | 107 | ViewEdge(node_t i, node_t j, weight_d w) : Edge(i, j, w) {} 108 | 109 | ViewEdge(const ViewEdge& edge) { 110 | src = edge.src; 111 | dst = edge.dst; 112 | weight = edge.weight; 113 | rel_rotation = edge.rel_rotation; 114 | rel_translation = edge.rel_translation; 115 | } 116 | }; 117 | 118 | } // namespace graph 119 | } // namespace gopt 120 | 121 | #endif // GRAPH_EDGE_H_ 122 | -------------------------------------------------------------------------------- /3rd_party/Graclus/metisLib/fortran.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 1997, Regents of the University of Minnesota 3 | * 4 | * fortran.c 5 | * 6 | * This file contains code for the fortran to C interface 7 | * 8 | * Started 8/19/97 9 | * George 10 | * 11 | * $Id: fortran.c,v 1.1 1998/11/27 17:59:14 karypis Exp $ 12 | * 13 | */ 14 | 15 | #include "metis.h" 16 | 17 | 18 | /************************************************************************* 19 | * This function changes the numbering to start from 0 instead of 1 20 | **************************************************************************/ 21 | void Change2CNumbering(int nvtxs, idxtype *xadj, idxtype *adjncy) 22 | { 23 | int i, nedges; 24 | 25 | for (i=0; i<=nvtxs; i++) 26 | xadj[i]--; 27 | 28 | nedges = xadj[nvtxs]; 29 | for (i=0; iInit(n); 39 | } 40 | 41 | void UnionFind::Init(size_t n) { 42 | // Reserve enough space. 43 | parents_.reserve(n); 44 | ranks_.reserve(n); 45 | nodes_.reserve(n); 46 | nodes_mapper_.reserve(n); 47 | num_components_.resize(n); 48 | 49 | for (size_t i = 0; i < n; i++) { 50 | parents_.push_back(i); 51 | ranks_.push_back(0); 52 | nodes_.push_back(i); 53 | nodes_mapper_[i] = i; 54 | num_components_[i] = 1; 55 | } 56 | } 57 | 58 | void UnionFind::InitWithNodes(const std::vector& nodes) { 59 | // Clear the space of nodes, if Init(size_t n) is called first. 60 | std::vector{}.swap(nodes_); 61 | nodes_.reserve(nodes.size()); 62 | nodes_.assign(nodes.begin(), nodes.end()); 63 | 64 | // Clear the space of nodes mapper, if Init(size_t n) is called first. 65 | nodes_mapper_.clear(); 66 | nodes_mapper_.reserve(nodes.size()); 67 | 68 | for (uint i = 0; i < nodes.size(); i++) { 69 | nodes_mapper_[nodes[i]] = i; 70 | } 71 | } 72 | 73 | size_t UnionFind::FindRoot(size_t x) { 74 | size_t idx = nodes_mapper_[x]; 75 | return (parents_[idx] == idx) 76 | ? idx 77 | : (parents_[idx] = FindRoot(nodes_[parents_[idx]])); 78 | } 79 | 80 | void UnionFind::Union(size_t x, size_t y) { 81 | x = FindRoot(x); 82 | y = FindRoot(y); 83 | if ((x == y) || 84 | (num_components_[x] + num_components_[y] > max_num_components_)) { 85 | return; 86 | } 87 | 88 | if (ranks_[x] < ranks_[y]) { 89 | parents_[x] = y; 90 | num_components_[y] += num_components_[x]; 91 | } else { 92 | parents_[y] = x; 93 | num_components_[x] += num_components_[y]; 94 | if (ranks_[x] == ranks_[y]) { 95 | ranks_[x]++; 96 | } 97 | } 98 | } 99 | 100 | std::vector UnionFind::GetRanks() const { return ranks_; } 101 | 102 | std::vector UnionFind::GetParents() const { return parents_; } 103 | 104 | std::unordered_set UnionFind::GetConnectedComponents() const { 105 | std::unordered_set components(parents_.begin(), parents_.end()); 106 | return components; 107 | } 108 | 109 | } // namespace graph 110 | } // namespace gopt -------------------------------------------------------------------------------- /data/synthetic/20_2.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT 1 0 0 0 0 0 0 0 2 | VERTEX_SE3:QUAT 2 0 0 0 0 0 0 0 3 | VERTEX_SE3:QUAT 3 0 0 0 0 0 0 0 4 | VERTEX_SE3:QUAT 4 0 0 0 0 0 0 0 5 | VERTEX_SE3:QUAT 5 0 0 0 0 0 0 0 6 | VERTEX_SE3:QUAT 6 0 0 0 0 0 0 0 7 | VERTEX_SE3:QUAT 7 0 0 0 0 0 0 0 8 | VERTEX_SE3:QUAT 8 0 0 0 0 0 0 0 9 | VERTEX_SE3:QUAT 9 0 0 0 0 0 0 0 10 | VERTEX_SE3:QUAT 10 0 0 0 0 0 0 0 11 | VERTEX_SE3:QUAT 11 0 0 0 0 0 0 0 12 | VERTEX_SE3:QUAT 12 0 0 0 0 0 0 0 13 | VERTEX_SE3:QUAT 13 0 0 0 0 0 0 0 14 | VERTEX_SE3:QUAT 14 0 0 0 0 0 0 0 15 | VERTEX_SE3:QUAT 15 0 0 0 0 0 0 0 16 | VERTEX_SE3:QUAT 16 0 0 0 0 0 0 0 17 | VERTEX_SE3:QUAT 17 0 0 0 0 0 0 0 18 | VERTEX_SE3:QUAT 18 0 0 0 0 0 0 0 19 | VERTEX_SE3:QUAT 19 0 0 0 0 0 0 0 20 | EDGE_SE3:QUAT 0 7 0 0 0 -6.30441e-08 7.61969e-08 0.891006 0.453991 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 21 | EDGE_SE3:QUAT 0 17 0 0 0 2.11507e-08 -6.09217e-08 -0.453991 0.891006 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 22 | EDGE_SE3:QUAT 9 15 0 0 0 4.73717e-08 -6.71311e-08 0.809017 0.587785 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 23 | EDGE_SE3:QUAT 10 18 0 0 0 -3.96951e-08 -7.36157e-08 0.951056 0.309017 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 24 | EDGE_SE3:QUAT 6 9 0 0 0 5.23675e-08 -8.59956e-08 0.453991 0.891006 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 25 | EDGE_SE3:QUAT 2 5 0 0 0 -1.26021e-08 1.25642e-07 0.453991 0.891007 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 26 | EDGE_SE3:QUAT 0 12 0 0 0 -5.0966e-08 1.10983e-07 0.951057 -0.309017 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 27 | EDGE_SE3:QUAT 9 10 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 28 | EDGE_SE3:QUAT 8 9 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 29 | EDGE_SE3:QUAT 7 8 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 30 | EDGE_SE3:QUAT 6 7 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 31 | EDGE_SE3:QUAT 7 16 0 0 0 4.86609e-08 -1.17745e-07 0.987688 0.156434 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 32 | EDGE_SE3:QUAT 5 6 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 33 | EDGE_SE3:QUAT 4 5 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 34 | EDGE_SE3:QUAT 1 2 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 35 | EDGE_SE3:QUAT 2 3 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 36 | EDGE_SE3:QUAT 3 14 0 0 0 9.38345e-08 -8.14983e-08 0.987688 -0.156434 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 37 | EDGE_SE3:QUAT 3 4 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 38 | EDGE_SE3:QUAT 10 11 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 39 | EDGE_SE3:QUAT 11 12 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 40 | EDGE_SE3:QUAT 12 13 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 41 | EDGE_SE3:QUAT 8 15 0 0 0 1.20863e-07 2.30782e-08 0.891007 0.45399 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 42 | EDGE_SE3:QUAT 13 14 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 43 | EDGE_SE3:QUAT 14 15 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 44 | EDGE_SE3:QUAT 15 16 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 45 | EDGE_SE3:QUAT 0 1 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 46 | EDGE_SE3:QUAT 2 11 0 0 0 -2.28444e-08 -1.05233e-07 0.987688 0.156435 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 47 | EDGE_SE3:QUAT 16 17 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 48 | EDGE_SE3:QUAT 17 18 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 49 | EDGE_SE3:QUAT 18 19 0 0 0 0 0 0.156434 0.987688 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 50 | -------------------------------------------------------------------------------- /src/graph/graph_cut.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 | // BSD 3-Clause License 33 | 34 | // Copyright (c) 2021, Chenyu 35 | // All rights reserved. 36 | 37 | // Redistribution and use in source and binary forms, with or without 38 | // modification, are permitted provided that the following conditions are met: 39 | 40 | // 1. Redistributions of source code must retain the above copyright notice, this 41 | // list of conditions and the following disclaimer. 42 | 43 | // 2. Redistributions in binary form must reproduce the above copyright notice, 44 | // this list of conditions and the following disclaimer in the documentation 45 | // and/or other materials provided with the distribution. 46 | 47 | // 3. Neither the name of the copyright holder nor the names of its 48 | // contributors may be used to endorse or promote products derived from 49 | // this software without specific prior written permission. 50 | 51 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 52 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 53 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 54 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 55 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 56 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 57 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 58 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 59 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 60 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 61 | 62 | #ifndef GRAPH_GRAPH_CUT_H_ 63 | #define GRAPH_GRAPH_CUT_H_ 64 | 65 | #include 66 | 67 | #include 68 | #include 69 | 70 | namespace gopt { 71 | namespace graph { 72 | 73 | // Compute the normalized min-cut of an undirected graph using Graclus. 74 | // Partitions the graph into clusters and returns the cluster labels per vertex. 75 | std::unordered_map ComputeNormalizedMinGraphCut( 76 | const std::vector>& edges, 77 | const std::vector& weights, const int num_parts); 78 | 79 | } // namespace graph 80 | } // namespace gopt 81 | 82 | #endif // GRAPH_GRAPH_CUT_H_ 83 | -------------------------------------------------------------------------------- /cmake/FindGlog.cmake: -------------------------------------------------------------------------------- 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 | # Find package module for Glog library. 33 | # 34 | # The following variables are set by this module: 35 | # 36 | # GLOG_FOUND: TRUE if Glog is found. 37 | # GLOG_INCLUDE_DIRS: Include directories for Glog. 38 | # GLOG_LIBRARIES: Libraries required to link Glog. 39 | # 40 | # The following variables control the behavior of this module: 41 | # 42 | # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to 43 | # search for Glog includes. 44 | # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to 45 | # search for Glog libraries. 46 | 47 | set(GLOG_INCLUDE_DIR_HINTS "" CACHE PATH "Glog include directory") 48 | set(GLOG_LIBRARY_DIR_HINTS "" CACHE PATH "Glog library directory") 49 | 50 | unset(GLOG_FOUND) 51 | unset(GLOG_INCLUDE_DIRS) 52 | unset(GLOG_LIBRARIES) 53 | 54 | include(FindPackageHandleStandardArgs) 55 | 56 | list(APPEND GLOG_CHECK_INCLUDE_DIRS 57 | /usr/local/include 58 | /usr/local/homebrew/include 59 | /opt/local/var/macports/software 60 | /opt/local/include 61 | /usr/include) 62 | list(APPEND GLOG_CHECK_PATH_SUFFIXES 63 | glog/include 64 | glog/Include 65 | Glog/include 66 | Glog/Include 67 | src/windows) 68 | 69 | list(APPEND GLOG_CHECK_LIBRARY_DIRS 70 | /usr/local/lib 71 | /usr/local/homebrew/lib 72 | /opt/local/lib 73 | /usr/lib) 74 | list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES 75 | glog/lib 76 | glog/Lib 77 | Glog/lib 78 | Glog/Lib 79 | x64/Release) 80 | 81 | find_path(GLOG_INCLUDE_DIRS 82 | NAMES 83 | glog/logging.h 84 | PATHS 85 | ${GLOG_INCLUDE_DIR_HINTS} 86 | ${GLOG_CHECK_INCLUDE_DIRS} 87 | PATH_SUFFIXES 88 | ${GLOG_CHECK_PATH_SUFFIXES}) 89 | find_library(GLOG_LIBRARIES 90 | NAMES 91 | glog 92 | libglog 93 | PATHS 94 | ${GLOG_LIBRARY_DIR_HINTS} 95 | ${GLOG_CHECK_LIBRARY_DIRS} 96 | PATH_SUFFIXES 97 | ${GLOG_CHECK_LIBRARY_SUFFIXES}) 98 | 99 | if (GLOG_INCLUDE_DIRS AND GLOG_LIBRARIES) 100 | set(GLOG_FOUND TRUE) 101 | message(STATUS "Found Glog") 102 | message(STATUS " Includes : ${GLOG_INCLUDE_DIRS}") 103 | message(STATUS " Libraries : ${GLOG_LIBRARIES}") 104 | else() 105 | if(Glog_FIND_REQUIRED) 106 | message(FATAL_ERROR "Could not find Glog") 107 | endif() 108 | endif() 109 | -------------------------------------------------------------------------------- /src/graph/view_graph.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #ifndef GRAPH_VIEW_GRAPH_H_ 32 | #define GRAPH_VIEW_GRAPH_H_ 33 | 34 | #include 35 | #include 36 | 37 | #include "graph/graph.h" 38 | #include "graph/node.h" 39 | #include "graph/edge.h" 40 | 41 | #include "rotation_averaging/rotation_estimator.h" 42 | #include "translation_averaging/position_estimator.h" 43 | 44 | namespace gopt { 45 | namespace graph { 46 | 47 | class ViewGraph : public Graph { 48 | public: 49 | struct ViewGraphOptions { 50 | 51 | }; 52 | 53 | ViewGraph(); 54 | 55 | bool MotionAveraging( 56 | const RotationEstimatorOptions& rotation_estimator_options, 57 | const PositionEstimatorOptions& position_estimator_options, 58 | std::unordered_map* global_rotations, 59 | std::unordered_map* positions); 60 | 61 | bool RotationAveraging( 62 | const RotationEstimatorOptions& options, 63 | std::unordered_map* global_rotations); 64 | 65 | bool TranslationAveraging( 66 | const PositionEstimatorOptions& options, 67 | std::unordered_map* positions); 68 | 69 | void InitializeGlobalRotationsRandomly( 70 | std::unordered_map* global_rotations); 71 | void InitializeGlobalRotationsFromMST( 72 | std::unordered_map* global_rotations); 73 | 74 | void InitializeGlobalRotations( 75 | const RotationEstimatorOptions& options, 76 | std::unordered_map* global_rotations); 77 | 78 | void InitializeGlobalPositions( 79 | std::unordered_map* positions); 80 | 81 | bool ReadG2OFile(const std::string& filename); 82 | void WriteG2OFile(const std::string& filename); 83 | 84 | private: 85 | void ViewEdgesToViewPairs( 86 | std::unordered_map* view_pairs); 87 | 88 | std::unique_ptr CreateRotationEstimator( 89 | const RotationEstimatorOptions& options); 90 | 91 | std::unique_ptr CreatePositionEstimator( 92 | const PositionEstimatorOptions& options); 93 | 94 | ViewGraphOptions options_; 95 | }; 96 | // A view graph conceptually is equivalents to a pose graph. 97 | using PoseGraph = ViewGraph; 98 | 99 | } // namespace graph 100 | } // namespace gopt 101 | 102 | #endif // GRAPH_VIEW_GRAPH_H_ 103 | -------------------------------------------------------------------------------- /3rd_party/Spectra/MatOp/DenseSymShiftSolve.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016-2022 Yixuan Qiu 2 | // 3 | // This Source Code Form is subject to the terms of the Mozilla 4 | // Public License v. 2.0. If a copy of the MPL was not distributed 5 | // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. 6 | 7 | #ifndef SPECTRA_DENSE_SYM_SHIFT_SOLVE_H 8 | #define SPECTRA_DENSE_SYM_SHIFT_SOLVE_H 9 | 10 | #include 11 | #include 12 | 13 | #include "../LinAlg/BKLDLT.h" 14 | #include "../Util/CompInfo.h" 15 | 16 | namespace Spectra { 17 | 18 | /// 19 | /// \ingroup MatOp 20 | /// 21 | /// This class defines the shift-solve operation on a real symmetric matrix \f$A\f$, 22 | /// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and 23 | /// vector \f$x\f$. It is mainly used in the SymEigsShiftSolver eigen solver. 24 | /// 25 | /// \tparam Scalar_ The element type of the matrix, for example, 26 | /// `float`, `double`, and `long double`. 27 | /// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which 28 | /// triangular part of the matrix is used. 29 | /// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating 30 | /// the storage format of the input matrix. 31 | /// 32 | template 33 | class DenseSymShiftSolve 34 | { 35 | public: 36 | /// 37 | /// Element type of the matrix. 38 | /// 39 | using Scalar = Scalar_; 40 | 41 | private: 42 | using Index = Eigen::Index; 43 | using Matrix = Eigen::Matrix; 44 | using Vector = Eigen::Matrix; 45 | using MapConstVec = Eigen::Map; 46 | using MapVec = Eigen::Map; 47 | using ConstGenericMatrix = const Eigen::Ref; 48 | 49 | ConstGenericMatrix m_mat; 50 | const Index m_n; 51 | BKLDLT m_solver; 52 | 53 | public: 54 | /// 55 | /// Constructor to create the matrix operation object. 56 | /// 57 | /// \param mat An **Eigen** matrix object, whose type can be 58 | /// `Eigen::Matrix` (e.g. `Eigen::MatrixXd` and 59 | /// `Eigen::MatrixXf`), or its mapped version 60 | /// (e.g. `Eigen::Map`). 61 | /// 62 | template 63 | DenseSymShiftSolve(const Eigen::MatrixBase& mat) : 64 | m_mat(mat), m_n(mat.rows()) 65 | { 66 | static_assert( 67 | static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), 68 | "DenseSymShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); 69 | 70 | if (m_n != mat.cols()) 71 | throw std::invalid_argument("DenseSymShiftSolve: matrix must be square"); 72 | } 73 | 74 | /// 75 | /// Return the number of rows of the underlying matrix. 76 | /// 77 | Index rows() const { return m_n; } 78 | /// 79 | /// Return the number of columns of the underlying matrix. 80 | /// 81 | Index cols() const { return m_n; } 82 | 83 | /// 84 | /// Set the real shift \f$\sigma\f$. 85 | /// 86 | void set_shift(const Scalar& sigma) 87 | { 88 | m_solver.compute(m_mat, Uplo, sigma); 89 | if (m_solver.info() != CompInfo::Successful) 90 | throw std::invalid_argument("DenseSymShiftSolve: factorization failed with the given shift"); 91 | } 92 | 93 | /// 94 | /// Perform the shift-solve operation \f$y=(A-\sigma I)^{-1}x\f$. 95 | /// 96 | /// \param x_in Pointer to the \f$x\f$ vector. 97 | /// \param y_out Pointer to the \f$y\f$ vector. 98 | /// 99 | // y_out = inv(A - sigma * I) * x_in 100 | void perform_op(const Scalar* x_in, Scalar* y_out) const 101 | { 102 | MapConstVec x(x_in, m_n); 103 | MapVec y(y_out, m_n); 104 | y.noalias() = m_solver.solve(x); 105 | } 106 | }; 107 | 108 | } // namespace Spectra 109 | 110 | #endif // SPECTRA_DENSE_SYM_SHIFT_SOLVE_H 111 | -------------------------------------------------------------------------------- /src/graph/graph_cut_test.cc: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | 3 | // Copyright (c) 2021, Chenyu 4 | // All rights reserved. 5 | 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | 9 | // 1. Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | 12 | // 2. Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | 16 | // 3. Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "graph_cut.h" 32 | 33 | #include "gtest/gtest.h" 34 | 35 | namespace gopt { 36 | namespace graph { 37 | 38 | TEST(GRAPH_CUT_TEST, TestComputeNormalizedMinGraphCut) { 39 | const std::vector> edges = { 40 | {3, 4}, {3, 6}, {3, 5}, {0, 4}, {0, 1}, {0, 6}, {0, 7}, {0, 5}, 41 | {0, 2}, {4, 1}, {1, 6}, {1, 5}, {6, 7}, {7, 5}, {5, 2}, {3, 4}}; 42 | const std::vector weights = {0, 3, 1, 3, 1, 2, 6, 1, 43 | 8, 1, 1, 80, 2, 1, 1, 4}; 44 | const auto cut_labels = ComputeNormalizedMinGraphCut(edges, weights, 2); 45 | EXPECT_EQ(cut_labels.size(), 8); 46 | for (const auto& label : cut_labels) { 47 | EXPECT_GE(label.second, 0); 48 | EXPECT_LT(label.second, 2); 49 | } 50 | } 51 | 52 | TEST(GRAPH_CUT_TEST, TestComputeNormalizedMinGraphCutDuplicateEdge) { 53 | const std::vector> edges = { 54 | {3, 4}, {3, 6}, {3, 5}, {0, 4}, {0, 1}, {0, 6}, {0, 7}, {0, 5}, {0, 2}, 55 | {4, 1}, {1, 6}, {1, 5}, {6, 7}, {7, 5}, {5, 2}, {3, 4}, {3, 4}}; 56 | const std::vector weights = {0, 3, 1, 3, 1, 2, 6, 1, 8, 57 | 1, 1, 80, 2, 1, 1, 4, 4}; 58 | const auto cut_labels = ComputeNormalizedMinGraphCut(edges, weights, 2); 59 | EXPECT_EQ(cut_labels.size(), 8); 60 | for (const auto& label : cut_labels) { 61 | EXPECT_GE(label.second, 0); 62 | EXPECT_LT(label.second, 2); 63 | } 64 | } 65 | 66 | TEST(GRAPH_CUT_TEST, TestComputeNormalizedMinGraphCutMissingVertex) { 67 | const std::vector> edges = { 68 | {3, 4}, {3, 6}, {3, 5}, {0, 1}, {0, 6}, {0, 7}, {0, 5}, 69 | {0, 2}, {4, 1}, {1, 6}, {1, 5}, {6, 7}, {7, 5}, {5, 2}}; 70 | const std::vector weights = {0, 3, 1, 3, 1, 2, 6, 1, 8, 1, 1, 80, 2, 1}; 71 | const auto cut_labels = ComputeNormalizedMinGraphCut(edges, weights, 2); 72 | EXPECT_EQ(cut_labels.size(), 8); 73 | for (const auto& label : cut_labels) { 74 | EXPECT_GE(label.second, 0); 75 | EXPECT_LT(label.second, 2); 76 | } 77 | } 78 | 79 | TEST(GRAPH_CUT_TEST, TestComputeNormalizedMinGraphCutDisconnected) { 80 | const std::vector> edges = {{0, 1}, {1, 2}, {3, 4}}; 81 | const std::vector weights = {1, 3, 1}; 82 | const auto cut_labels = ComputeNormalizedMinGraphCut(edges, weights, 2); 83 | EXPECT_EQ(cut_labels.size(), 5); 84 | for (const auto& label : cut_labels) { 85 | EXPECT_GE(label.second, 0); 86 | EXPECT_LT(label.second, 2); 87 | } 88 | } 89 | 90 | } // namespace graph 91 | } // namespace gopt 92 | --------------------------------------------------------------------------------