├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── Doxyfile ├── License.txt ├── Makefile.ros ├── README.txt ├── doc └── addons │ ├── 01_installation.dox │ ├── 02_how_to_use.dox │ ├── 03_matlab.dox │ ├── 04_how_to_contribute.dox │ ├── 05_contact.dox │ ├── 06_references.dox │ ├── images │ ├── absolute_central.dia │ ├── absolute_central.eps │ ├── absolute_central.pdf │ ├── absolute_central.png │ ├── absolute_noncentral.dia │ ├── absolute_noncentral.eps │ ├── absolute_noncentral.pdf │ ├── absolute_noncentral.png │ ├── central.dia │ ├── central.eps │ ├── central.pdf │ ├── central.png │ ├── multi_viewpoint.dia │ ├── multi_viewpoint.eps │ ├── multi_viewpoint.pdf │ ├── multi_viewpoint.png │ ├── noncentral.dia │ ├── noncentral.eps │ ├── noncentral.pdf │ ├── noncentral.png │ ├── nonoverlapping.dia │ ├── nonoverlapping.eps │ ├── nonoverlapping.pdf │ ├── nonoverlapping.png │ ├── point_cloud.dia │ ├── point_cloud.eps │ ├── point_cloud.pdf │ ├── point_cloud.png │ ├── relative_central.dia │ ├── relative_central.eps │ ├── relative_central.pdf │ ├── relative_central.png │ ├── relative_noncentral.dia │ ├── relative_noncentral.eps │ ├── relative_noncentral.pdf │ ├── relative_noncentral.png │ ├── reprojectionError.dia │ ├── reprojectionError.eps │ ├── reprojectionError.pdf │ ├── reprojectionError.png │ ├── triangulation_central.dia │ ├── triangulation_central.eps │ ├── triangulation_central.pdf │ ├── triangulation_central.png │ ├── triangulation_noncentral.dia │ ├── triangulation_noncentral.eps │ ├── triangulation_noncentral.pdf │ └── triangulation_noncentral.png │ └── mainpage.dox ├── include └── opengv │ ├── Indices.hpp │ ├── OptimizationFunctor.hpp │ ├── absolute_pose │ ├── AbsoluteAdapterBase.hpp │ ├── AbsoluteMultiAdapterBase.hpp │ ├── CentralAbsoluteAdapter.hpp │ ├── MACentralAbsolute.hpp │ ├── MANoncentralAbsolute.hpp │ ├── NoncentralAbsoluteAdapter.hpp │ ├── NoncentralAbsoluteMultiAdapter.hpp │ ├── methods.hpp │ └── modules │ │ ├── Epnp.hpp │ │ ├── gp3p │ │ └── modules.hpp │ │ ├── gpnp1 │ │ └── modules.hpp │ │ ├── gpnp2 │ │ └── modules.hpp │ │ ├── gpnp3 │ │ └── modules.hpp │ │ ├── gpnp4 │ │ └── modules.hpp │ │ ├── gpnp5 │ │ └── modules.hpp │ │ ├── main.hpp │ │ ├── upnp2.hpp │ │ └── upnp4.hpp │ ├── math │ ├── Sturm.hpp │ ├── arun.hpp │ ├── cayley.hpp │ ├── gauss_jordan.hpp │ ├── quaternion.hpp │ └── roots.hpp │ ├── point_cloud │ ├── MAPointCloud.hpp │ ├── PointCloudAdapter.hpp │ ├── PointCloudAdapterBase.hpp │ └── methods.hpp │ ├── relative_pose │ ├── CentralRelativeAdapter.hpp │ ├── CentralRelativeMultiAdapter.hpp │ ├── CentralRelativeWeightingAdapter.hpp │ ├── MACentralRelative.hpp │ ├── MANoncentralRelative.hpp │ ├── MANoncentralRelativeMulti.hpp │ ├── NoncentralRelativeAdapter.hpp │ ├── NoncentralRelativeMultiAdapter.hpp │ ├── RelativeAdapterBase.hpp │ ├── RelativeMultiAdapterBase.hpp │ ├── methods.hpp │ └── modules │ │ ├── eigensolver │ │ └── modules.hpp │ │ ├── fivept_kneip │ │ └── modules.hpp │ │ ├── fivept_nister │ │ └── modules.hpp │ │ ├── fivept_stewenius │ │ └── modules.hpp │ │ ├── ge │ │ └── modules.hpp │ │ ├── main.hpp │ │ └── sixpt │ │ └── modules.hpp │ ├── sac │ ├── Lmeds.hpp │ ├── MultiRansac.hpp │ ├── MultiSampleConsensus.hpp │ ├── MultiSampleConsensusProblem.hpp │ ├── Ransac.hpp │ ├── SampleConsensus.hpp │ ├── SampleConsensusProblem.hpp │ └── implementation │ │ ├── Lmeds.hpp │ │ ├── MultiRansac.hpp │ │ ├── MultiSampleConsensus.hpp │ │ ├── MultiSampleConsensusProblem.hpp │ │ ├── Ransac.hpp │ │ ├── SampleConsensus.hpp │ │ └── SampleConsensusProblem.hpp │ ├── sac_problems │ ├── absolute_pose │ │ ├── AbsolutePoseSacProblem.hpp │ │ └── MultiNoncentralAbsolutePoseSacProblem.hpp │ ├── point_cloud │ │ └── PointCloudSacProblem.hpp │ └── relative_pose │ │ ├── CentralRelativePoseSacProblem.hpp │ │ ├── EigensolverSacProblem.hpp │ │ ├── MultiCentralRelativePoseSacProblem.hpp │ │ ├── MultiNoncentralRelativePoseSacProblem.hpp │ │ ├── NoncentralRelativePoseSacProblem.hpp │ │ ├── RotationOnlySacProblem.hpp │ │ └── TranslationOnlySacProblem.hpp │ ├── triangulation │ └── methods.hpp │ └── types.hpp ├── manifest.xml ├── matlab ├── benchmark_absolute_pose.m ├── benchmark_absolute_pose_execution_times.m ├── benchmark_absolute_pose_noncentral.m ├── benchmark_absolute_pose_noncentral_execution_timing.m ├── benchmark_relative_pose.m ├── benchmark_relative_pose_execution_times.m ├── benchmark_relative_pose_noncentral.m ├── benchmark_relative_pose_noncentral2.m ├── benchmark_relative_pose_noncentral_execution_times.m ├── benchmark_relative_pose_noncentral_execution_times2.m ├── helpers │ ├── addNoise.m │ ├── cayley2rot.m │ ├── create2D2DExperiment.m │ ├── create2D2DOmniExperiment.m │ ├── create2D3DExperiment.m │ ├── createMulti2D2DExperiment.m │ ├── createMulti2D2DOmniExperiment.m │ ├── evaluateRotationError.m │ ├── evaluateTransformationError.m │ ├── generateBoundedR.m │ ├── generateRandomR.m │ ├── perturb.m │ ├── rodrigues.m │ ├── rot2cayley.m │ └── transformEssentials.m ├── make_mex.sh ├── opengv.cpp ├── opengv_donotuse.cpp ├── opengv_experimental1.cpp ├── opengv_experimental2.cpp ├── plot_arun_error.m ├── plot_expected_iterations.m ├── plot_ge_costfunction.m ├── ransac_experiment.m ├── ransac_experiment2.m ├── ransac_experiment3.m └── ransac_test.m ├── modules ├── Config.cmake.in ├── FindEigen.cmake └── FindNumPy.cmake ├── python ├── CMakeLists.txt ├── pyopengv.cpp ├── tests.py └── types.hpp ├── src ├── absolute_pose │ ├── CentralAbsoluteAdapter.cpp │ ├── MACentralAbsolute.cpp │ ├── MANoncentralAbsolute.cpp │ ├── NoncentralAbsoluteAdapter.cpp │ ├── NoncentralAbsoluteMultiAdapter.cpp │ ├── methods.cpp │ └── modules │ │ ├── Epnp.cpp │ │ ├── gp3p │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── gpnp1 │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── gpnp2 │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── gpnp3 │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── gpnp4 │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── gpnp5 │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── main.cpp │ │ ├── upnp2.cpp │ │ └── upnp4.cpp ├── math │ ├── Sturm.cpp │ ├── arun.cpp │ ├── cayley.cpp │ ├── gauss_jordan.cpp │ ├── quaternion.cpp │ └── roots.cpp ├── point_cloud │ ├── MAPointCloud.cpp │ ├── PointCloudAdapter.cpp │ └── methods.cpp ├── relative_pose │ ├── CentralRelativeAdapter.cpp │ ├── CentralRelativeMultiAdapter.cpp │ ├── CentralRelativeWeightingAdapter.cpp │ ├── MACentralRelative.cpp │ ├── MANoncentralRelative.cpp │ ├── MANoncentralRelativeMulti.cpp │ ├── NoncentralRelativeAdapter.cpp │ ├── NoncentralRelativeMultiAdapter.cpp │ ├── methods.cpp │ └── modules │ │ ├── eigensolver │ │ └── modules.cpp │ │ ├── fivept_kneip │ │ ├── code.cpp │ │ ├── init.cpp │ │ ├── reductors.cpp │ │ └── spolynomials.cpp │ │ ├── fivept_nister │ │ └── modules.cpp │ │ ├── fivept_stewenius │ │ └── modules.cpp │ │ ├── ge │ │ └── modules.cpp │ │ ├── main.cpp │ │ └── sixpt │ │ └── modules2.cpp ├── sac_problems │ ├── absolute_pose │ │ ├── AbsolutePoseSacProblem.cpp │ │ └── MultiNoncentralAbsolutePoseSacProblem.cpp │ ├── point_cloud │ │ └── PointCloudSacProblem.cpp │ └── relative_pose │ │ ├── CentralRelativePoseSacProblem.cpp │ │ ├── EigensolverSacProblem.cpp │ │ ├── MultiCentralRelativePoseSacProblem.cpp │ │ ├── MultiNoncentralRelativePoseSacProblem.cpp │ │ ├── NoncentralRelativePoseSacProblem.cpp │ │ ├── RotationOnlySacProblem.cpp │ │ └── TranslationOnlySacProblem.cpp └── triangulation │ └── methods.cpp └── test ├── experiment_helpers.cpp ├── experiment_helpers.hpp ├── random_generators.cpp ├── random_generators.hpp ├── test_Sturm.cpp ├── test_absolute_pose.cpp ├── test_absolute_pose_sac.cpp ├── test_eigensolver.cpp ├── test_eigensolver_sac.cpp ├── test_multi_noncentral_absolute_pose_sac.cpp ├── test_multi_noncentral_relative_pose_sac.cpp ├── test_noncentral_absolute_pose.cpp ├── test_noncentral_absolute_pose_sac.cpp ├── test_noncentral_relative_pose.cpp ├── test_noncentral_relative_pose_sac.cpp ├── test_point_cloud.cpp ├── test_point_cloud_sac.cpp ├── test_relative_pose.cpp ├── test_relative_pose_rotationOnly.cpp ├── test_relative_pose_rotationOnly_sac.cpp ├── test_relative_pose_sac.cpp ├── test_triangulation.cpp ├── time_measurement.cpp └── time_measurement.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | lib 2 | build 3 | *.mexa64 4 | *.pyc 5 | .vscode 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "python/pybind11"] 2 | path = python/pybind11 3 | url = https://github.com/pybind/pybind11.git 4 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: trusty 2 | sudo: false 3 | 4 | language: cpp 5 | 6 | compiler: 7 | - clang 8 | - gcc 9 | 10 | cache: 11 | apt: true 12 | 13 | addons: 14 | apt: 15 | packages: 16 | - build-essential 17 | - python-dev 18 | - libboost-python-dev 19 | - python-numpy-dev 20 | - libeigen3-dev 21 | 22 | env: 23 | global: 24 | # CMAKE minimal required version 3.1.0 25 | - CMAKE_URL="https://cmake.org/files/v3.1/cmake-3.1.3-Linux-x86_64.tar.gz" 26 | - CMAKE_ROOT=${TRAVIS_BUILD_DIR}/cmake 27 | - CMAKE_SOURCE=${CMAKE_ROOT}/source 28 | - CMAKE_INSTALL=${CMAKE_ROOT}/install 29 | 30 | before_install: 31 | # CMAKE most recent version 32 | - > 33 | if [ "$(ls -A ${CMAKE_INSTALL})" ]; then 34 | echo "CMake found in cache."; 35 | ls -A ${CMAKE_INSTALL} 36 | export PATH=${CMAKE_INSTALL}/bin:${PATH}; 37 | cmake --version 38 | else 39 | mkdir --parent ${CMAKE_SOURCE} 40 | mkdir --parent ${CMAKE_INSTALL} 41 | ls -A ${CMAKE_INSTALL} 42 | travis_retry wget --no-check-certificate --quiet -O - ${CMAKE_URL} | tar --strip-components=1 -xz -C ${CMAKE_INSTALL} 43 | export PATH=${CMAKE_INSTALL}/bin:${PATH}; 44 | cmake --version 45 | fi 46 | 47 | 48 | before_script: 49 | - mkdir build 50 | - cd build 51 | - > 52 | if [ $CXX = "clang++" ]; then 53 | cmake .. -DBUILD_TESTS:BOOL=ON -DBUILD_PYTHON:BOOL=ON -DCMAKE_CXX_FLAGS="-Wno-deprecated-register" 54 | else 55 | cmake .. -DBUILD_TESTS:BOOL=ON -DBUILD_PYTHON:BOOL=ON 56 | fi 57 | 58 | script: 59 | - make -j2 VERBOSE=1 60 | - make test 61 | 62 | cache: 63 | directories: 64 | - $CMAKE_INSTALL 65 | -------------------------------------------------------------------------------- /License.txt: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | -------------------------------------------------------------------------------- /Makefile.ros: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | library: OpenGV 2 | pages: http://laurentkneip.github.io/opengv 3 | brief: OpenGV is a collection of computer vision methods for solving 4 | geometric vision problems. It contains absolute-pose, relative-pose, 5 | triangulation, and point-cloud alignment methods for the calibrated 6 | case. All problems can be solved with central or non-central cameras, 7 | and embedded into a random sample consensus or nonlinear optimization 8 | context. Matlab and Python interfaces are implemented as well. The link 9 | to the above pages also shows links to precompiled Matlab mex-libraries. 10 | Please consult the documentation for more information. 11 | author: Laurent Kneip, ShanghaiTech, Mobile Perception Lab (http://mpl.sist.shanghaitech.edu.cn) 12 | contact: kneip.laurent@gmail.com 13 | -------------------------------------------------------------------------------- /doc/addons/05_contact.dox: -------------------------------------------------------------------------------- 1 | /** \page page_contact Contact 2 | * 3 | * For any questions or inquiries, please contact: 4 | * 5 | \verbatim 6 | kneip.laurent@gmail.com 7 | \endverbatim 8 | * 9 | */ 10 | -------------------------------------------------------------------------------- /doc/addons/06_references.dox: -------------------------------------------------------------------------------- 1 | /** \page page_references References 2 | * 3 | [1] L. Kneip, D. Scaramuzza, R. Siegwart, "A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation of Absolute Camera Position and Orientation", Proc. of The IEEE Conference on Computer Vision and Pattern Recognition (CVPR), Colorado Springs, USA. June 2011. 4 | 5 | [2] X. Gao, X. Hou, J. Tang, H. Cheng. "Complete solution classification for the perspective-three-point problem", IEEE Transactions on Pattern Analysis and Machine Intelligence, 25(8):930–943, 2003. 6 | 7 | [3] L. Kneip, P. Furgale, R. Siegwart, "Using Multi-Camera Systems in Robotics: Efficient Solutions to the NPnP Problem", Proc. of The IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany. May 2013. 8 | 9 | [4] V. Lepetit, F. Moreno-Noguer, P. Fua. "Epnp: An accurate O(n) solution to the pnp problem", International Journal of Computer Vision (IJCV), 81(2):578–589, 2009. 10 | 11 | [5] H. D. Stewénius, C. Engels, D. Nistér. "Recent developments on direct relative orientation", ISPRS Journal of Photogrammetry and Remote Sensing, 60(4):284–294, 2006. 12 | 13 | [6] D. Nistér, "An efficient solution to the five-point relative pose problem", IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 26(6):756–777, 2004. 14 | 15 | [7] L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two Images Independently of the Translation", Proc. of The European Conference on Computer Vision (ECCV), Florence, Italy. October 2012. 16 | 17 | [8] R. Hartley, A. Zisserman. "Multiple View Geometry in Computer Vision", Cambridge University Press, New York, NY, USA, second edition, 2004. 18 | 19 | [9] H. Longuet-Higgins, "Readings in computer vision: issues, problems, principles, and paradigms", Morgan Kaufmann Publishers Inc., San Francisco, CA, USA, 1987. 20 | 21 | [10] R. Hartley, "In Defense of the Eight-Point Algorithm", IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 19(6):580–593, 1997. 22 | 23 | [11] L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of The International Conference on Computer Vision (ICCV), Sydney, Australia. December 2013. (Accepted for publication) 24 | 25 | [12] H. Li, R. Hartley, J. Kim, "A Linear Approach to Motion Estimation Using Generalized Camera Models", Proc. of The IEEE Conference on Computer Vision and Pattern Recognition (CVPR), Anchorage, Alaska, USA. June 2008. 26 | 27 | [13] K.S. Arun, T.S. Huang, S.D. Blostein, "Least-Squares Fitting of Two 3-D Point Sets", IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 9(5), 698-700, 1987. 28 | 29 | [14] A. Cayley. "About the algebraic structure of the orthogonal group and the other classical groups in a field of characteristic zero or a prime characteristic", Reine Angewandte Mathematik, 32, 1846. 30 | 31 | [15] M. Fischler, R. Bolles, "Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography", Communications of the ACM, 24(6):381–395, 1981. 32 | 33 | [16] H. Stewenius, D. Nister, M. Oskarsson, K. Aström, "Solutions to Minimal Generalized Relative Pose Problems", Workshop on omni-directional vision, 2005. 34 | 35 | [17] L. Kneip, P. Furgale, "OpenGV: A unified and generalized approach to real-time calibrated geometric vision", Proc. of The IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China. May 2014. 36 | 37 | [18] L. Kneip, H. Li, "Efficient Computation of Relative Pose for Multi-Camera Systems", In Proc. of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), Columbus, USA. June 2014. 38 | 39 | [19] L. Kneip, H. Li, Y. Seo, "UPnP: An optimal O(n) solution to the absolute pose problem with universal applicability", In Proc. of The European Conference on Computer Vision (ECCV), Zurich, Switzerland. September 2014. 40 | 41 | * 42 | */ 43 | -------------------------------------------------------------------------------- /doc/addons/images/absolute_central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/absolute_central.dia -------------------------------------------------------------------------------- /doc/addons/images/absolute_central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/absolute_central.pdf -------------------------------------------------------------------------------- /doc/addons/images/absolute_central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/absolute_central.png -------------------------------------------------------------------------------- /doc/addons/images/absolute_noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/absolute_noncentral.dia -------------------------------------------------------------------------------- /doc/addons/images/absolute_noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/absolute_noncentral.pdf -------------------------------------------------------------------------------- /doc/addons/images/absolute_noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/absolute_noncentral.png -------------------------------------------------------------------------------- /doc/addons/images/central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/central.dia -------------------------------------------------------------------------------- /doc/addons/images/central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/central.pdf -------------------------------------------------------------------------------- /doc/addons/images/central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/central.png -------------------------------------------------------------------------------- /doc/addons/images/multi_viewpoint.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/multi_viewpoint.dia -------------------------------------------------------------------------------- /doc/addons/images/multi_viewpoint.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/multi_viewpoint.pdf -------------------------------------------------------------------------------- /doc/addons/images/multi_viewpoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/multi_viewpoint.png -------------------------------------------------------------------------------- /doc/addons/images/noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/noncentral.dia -------------------------------------------------------------------------------- /doc/addons/images/noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/noncentral.pdf -------------------------------------------------------------------------------- /doc/addons/images/noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/noncentral.png -------------------------------------------------------------------------------- /doc/addons/images/nonoverlapping.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/nonoverlapping.dia -------------------------------------------------------------------------------- /doc/addons/images/nonoverlapping.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/nonoverlapping.pdf -------------------------------------------------------------------------------- /doc/addons/images/nonoverlapping.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/nonoverlapping.png -------------------------------------------------------------------------------- /doc/addons/images/point_cloud.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/point_cloud.dia -------------------------------------------------------------------------------- /doc/addons/images/point_cloud.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/point_cloud.pdf -------------------------------------------------------------------------------- /doc/addons/images/point_cloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/point_cloud.png -------------------------------------------------------------------------------- /doc/addons/images/relative_central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/relative_central.dia -------------------------------------------------------------------------------- /doc/addons/images/relative_central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/relative_central.pdf -------------------------------------------------------------------------------- /doc/addons/images/relative_central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/relative_central.png -------------------------------------------------------------------------------- /doc/addons/images/relative_noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/relative_noncentral.dia -------------------------------------------------------------------------------- /doc/addons/images/relative_noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/relative_noncentral.pdf -------------------------------------------------------------------------------- /doc/addons/images/relative_noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/relative_noncentral.png -------------------------------------------------------------------------------- /doc/addons/images/reprojectionError.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/reprojectionError.dia -------------------------------------------------------------------------------- /doc/addons/images/reprojectionError.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/reprojectionError.pdf -------------------------------------------------------------------------------- /doc/addons/images/reprojectionError.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/reprojectionError.png -------------------------------------------------------------------------------- /doc/addons/images/triangulation_central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/triangulation_central.dia -------------------------------------------------------------------------------- /doc/addons/images/triangulation_central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/triangulation_central.pdf -------------------------------------------------------------------------------- /doc/addons/images/triangulation_central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/triangulation_central.png -------------------------------------------------------------------------------- /doc/addons/images/triangulation_noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/triangulation_noncentral.dia -------------------------------------------------------------------------------- /doc/addons/images/triangulation_noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/triangulation_noncentral.pdf -------------------------------------------------------------------------------- /doc/addons/images/triangulation_noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laurentkneip/opengv/91f4b19c73450833a40e463ad3648aae80b3a7f3/doc/addons/images/triangulation_noncentral.png -------------------------------------------------------------------------------- /include/opengv/Indices.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file Indices.hpp 33 | * \brief Generic functor base for use with the Eigen-nonlinear optimization 34 | * toolbox. 35 | */ 36 | 37 | #ifndef OPENGV_INDICES_HPP_ 38 | #define OPENGV_INDICES_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | using namespace std; 45 | using namespace Eigen; 46 | 47 | /** 48 | * \brief The namespace of this library. 49 | */ 50 | namespace opengv 51 | { 52 | 53 | /** 54 | * Index class used internally so we can efficiently access either all or 55 | * just a subset of the indices, using the same syntax 56 | */ 57 | struct Indices 58 | { 59 | /** Indexing into vector of indices? */ 60 | bool _useIndices; 61 | /** Pointer to a vector of indices (if used) */ 62 | const std::vector * _indices; 63 | /** The number of correspondences */ 64 | size_t _numberCorrespondences; 65 | 66 | /** 67 | * \brief Constructor using index-vector. 68 | * \param[in] indices The index-vector. 69 | */ 70 | Indices( const std::vector & indices) : 71 | _useIndices(true), 72 | _indices(&indices), 73 | _numberCorrespondences(indices.size()) 74 | {} 75 | 76 | /** 77 | * \brief Constructor without index-vector (uses all correspondences). 78 | * \param[in] numberCorrespondences The number of correspondences. 79 | */ 80 | Indices(size_t numberCorrespondences) : 81 | _useIndices(false), 82 | _numberCorrespondences(numberCorrespondences) 83 | {} 84 | 85 | /** 86 | * \brief Get the number of correspondences. 87 | * \return The number of correspondences. 88 | */ 89 | size_t size() const 90 | { 91 | return _numberCorrespondences; 92 | } 93 | 94 | /** 95 | * \brief Get an index. 96 | * \param[in] i The index. 97 | * \return The index (either directly or from the index-vector). 98 | */ 99 | int operator[](int i) const 100 | { 101 | if( _useIndices ) 102 | return (*_indices)[i]; 103 | return i; 104 | } 105 | }; 106 | 107 | } 108 | 109 | #endif /* OPENGV_INDICES_HPP_ */ 110 | -------------------------------------------------------------------------------- /include/opengv/OptimizationFunctor.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file OptimizationFunctor.hpp 33 | * \brief Generic functor base for use with the Eigen-nonlinear optimization 34 | * toolbox. 35 | */ 36 | 37 | #ifndef OPENGV_OPTIMIZATIONFUNCTOR_HPP_ 38 | #define OPENGV_OPTIMIZATIONFUNCTOR_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | using namespace std; 45 | using namespace Eigen; 46 | 47 | /** 48 | * \brief The namespace of this library. 49 | */ 50 | namespace opengv 51 | { 52 | 53 | /** 54 | * Generic functor base for use with the Eigen-nonlinear optimization 55 | * toolbox. Please refer to the Eigen-documentation for further information. 56 | */ 57 | template 58 | struct OptimizationFunctor 59 | { 60 | /** undocumented */ 61 | typedef _Scalar Scalar; 62 | /** undocumented */ 63 | enum 64 | { 65 | InputsAtCompileTime = NX, 66 | ValuesAtCompileTime = NY 67 | }; 68 | /** undocumented */ 69 | typedef Matrix InputType; 70 | /** undocumented */ 71 | typedef Matrix ValueType; 72 | /** undocumented */ 73 | typedef Matrix JacobianType; 74 | 75 | /** undocumented */ 76 | const int m_inputs; 77 | /** undocumented */ 78 | const int m_values; 79 | 80 | /** undocumented */ 81 | OptimizationFunctor() : 82 | m_inputs(InputsAtCompileTime), 83 | m_values(ValuesAtCompileTime) {} 84 | /** undocumented */ 85 | OptimizationFunctor(int inputs, int values) : 86 | m_inputs(inputs), 87 | m_values(values) {} 88 | 89 | /** undocumented */ 90 | int inputs() const 91 | { 92 | return m_inputs; 93 | } 94 | /** undocumented */ 95 | int values() const 96 | { 97 | return m_values; 98 | } 99 | }; 100 | 101 | } 102 | 103 | #endif /* OPENGV_OPTIMIZATIONFUNCTOR_HPP_ */ 104 | -------------------------------------------------------------------------------- /include/opengv/absolute_pose/modules/gpnp1/modules.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP1_MODULES_HPP_ 33 | #define OPENGV_ABSOLUTE_POSE_MODULES_GPNP1_MODULES_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace opengv 41 | { 42 | namespace absolute_pose 43 | { 44 | namespace modules 45 | { 46 | namespace gpnp1 47 | { 48 | 49 | void init( 50 | Eigen::Matrix & groebnerMatrix, 51 | const Eigen::Matrix & a, 52 | Eigen::Matrix & n, 53 | Eigen::Vector3d & c0, 54 | Eigen::Vector3d & c1, 55 | Eigen::Vector3d & c2, 56 | Eigen::Vector3d & c3 ); 57 | void compute( Eigen::Matrix & groebnerMatrix ); 58 | void sPolynomial3( Eigen::Matrix & groebnerMatrix ); 59 | void sPolynomial4( Eigen::Matrix & groebnerMatrix ); 60 | void groebnerRow3_0_f( Eigen::Matrix & groebnerMatrix, int targetRow ); 61 | 62 | } 63 | } 64 | } 65 | } 66 | 67 | #endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP1_MODULES_HPP_ */ 68 | -------------------------------------------------------------------------------- /include/opengv/absolute_pose/modules/gpnp2/modules.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP2_MODULES_HPP_ 33 | #define OPENGV_ABSOLUTE_POSE_MODULES_GPNP2_MODULES_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace opengv 41 | { 42 | namespace absolute_pose 43 | { 44 | namespace modules 45 | { 46 | namespace gpnp2 47 | { 48 | 49 | void init( 50 | Eigen::Matrix & groebnerMatrix, 51 | const Eigen::Matrix & a, 52 | Eigen::Matrix & n, 53 | Eigen::Matrix & m, 54 | Eigen::Vector3d & c0, 55 | Eigen::Vector3d & c1, 56 | Eigen::Vector3d & c2, 57 | Eigen::Vector3d & c3 ); 58 | void compute( Eigen::Matrix & groebnerMatrix ); 59 | void sPolynomial5( Eigen::Matrix & groebnerMatrix ); 60 | void sPolynomial6( Eigen::Matrix & groebnerMatrix ); 61 | void groebnerRow5_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ); 62 | void sPolynomial7( Eigen::Matrix & groebnerMatrix ); 63 | void groebnerRow6_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ); 64 | void sPolynomial8( Eigen::Matrix & groebnerMatrix ); 65 | void groebnerRow7_10_f( Eigen::Matrix & groebnerMatrix, int targetRow ); 66 | void groebnerRow7_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ); 67 | void sPolynomial9( Eigen::Matrix & groebnerMatrix ); 68 | void groebnerRow8_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ); 69 | 70 | } 71 | } 72 | } 73 | } 74 | 75 | #endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP2_MODULES_HPP_ */ 76 | -------------------------------------------------------------------------------- /include/opengv/absolute_pose/modules/main.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_ABSOLUTE_POSE_MODULES_MAIN_HPP_ 33 | #define OPENGV_ABSOLUTE_POSE_MODULES_MAIN_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | namespace opengv 39 | { 40 | namespace absolute_pose 41 | { 42 | namespace modules 43 | { 44 | 45 | void p3p_kneip_main( 46 | const bearingVectors_t & f, 47 | const points_t & p, 48 | transformations_t & solutions ); 49 | void p3p_gao_main( 50 | const bearingVectors_t & f, 51 | const points_t & p, 52 | transformations_t & solutions ); 53 | void gp3p_main( 54 | const Eigen::Matrix3d & f, 55 | const Eigen::Matrix3d & v, 56 | const Eigen::Matrix3d & p, 57 | transformations_t & solutions ); 58 | void gpnp_main( 59 | const Eigen::Matrix & a, 60 | const Eigen::Matrix & V, 61 | const points_t & c, 62 | transformation_t & transformation ); 63 | double gpnp_evaluate( 64 | const Eigen::Matrix & solution, 65 | const points_t & c, 66 | translation_t & t, 67 | rotation_t & R ); 68 | void gpnp_optimize( 69 | const Eigen::Matrix & a, 70 | const Eigen::Matrix & V, 71 | const points_t & c, 72 | std::vector & factors ); 73 | void upnp_fill_s( 74 | const Eigen::Vector4d & quaternion, 75 | Eigen::Matrix & s ); 76 | void upnp_main( 77 | const Eigen::Matrix & M, 78 | const Eigen::Matrix & C, 79 | double gamma, 80 | std::vector< 81 | std::pair, 82 | Eigen::aligned_allocator< std::pair > 83 | > & quaternions ); 84 | void upnp_main_sym( 85 | const Eigen::Matrix & M, 86 | const Eigen::Matrix & C, 87 | double gamma, 88 | std::vector< 89 | std::pair, 90 | Eigen::aligned_allocator< std::pair > 91 | > & quaternions ); 92 | 93 | } 94 | } 95 | } 96 | 97 | #endif /* OPENGV_ABSOLUTE_POSE_MODULES_MAIN_HPP_ */ 98 | -------------------------------------------------------------------------------- /include/opengv/absolute_pose/modules/upnp2.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_ABSOLUTE_POSE_MODULES_UPNP2_HPP_ 33 | #define OPENGV_ABSOLUTE_POSE_MODULES_UPNP2_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | namespace opengv 39 | { 40 | namespace absolute_pose 41 | { 42 | namespace modules 43 | { 44 | namespace upnp 45 | { 46 | 47 | void setupAction_gj( 48 | const Eigen::Matrix & M, 49 | const Eigen::Matrix & C, 50 | double gamma, 51 | Eigen::Matrix & Action ); 52 | 53 | } 54 | } 55 | } 56 | } 57 | 58 | #endif /* OPENGV_ABSOLUTE_POSE_MODULES_UPNP2_HPP_ */ 59 | -------------------------------------------------------------------------------- /include/opengv/absolute_pose/modules/upnp4.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_ABSOLUTE_POSE_MODULES_UPNP4_HPP_ 33 | #define OPENGV_ABSOLUTE_POSE_MODULES_UPNP4_HPP_ 34 | 35 | #include 36 | #include 37 | 38 | namespace opengv 39 | { 40 | namespace absolute_pose 41 | { 42 | namespace modules 43 | { 44 | namespace upnp 45 | { 46 | 47 | void setupAction_sym_gj( 48 | const Eigen::Matrix & M, 49 | const Eigen::Matrix & C, 50 | double gamma, 51 | Eigen::Matrix & Action ); 52 | 53 | } 54 | } 55 | } 56 | } 57 | 58 | #endif /* OPENGV_ABSOLUTE_POSE_MODULES_UPNP4_HPP_ */ 59 | -------------------------------------------------------------------------------- /include/opengv/math/arun.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file arun.hpp 33 | * \brief Arun's method for computing the rotation between two point sets. 34 | */ 35 | 36 | #ifndef OPENGV_ARUN_HPP_ 37 | #define OPENGV_ARUN_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | /** 45 | * \brief The namespace of this library. 46 | */ 47 | namespace opengv 48 | { 49 | /** 50 | * \brief The namespace of the math tools. 51 | */ 52 | namespace math 53 | { 54 | 55 | /** 56 | * \brief Arun's method for computing the rotation between two point sets. 57 | * Core function [13]. 58 | * 59 | * \param[in] Hcross The summation over the exterior products between the 60 | * normalized points. 61 | * \return The rotation matrix that aligns the points. 62 | */ 63 | rotation_t arun( const Eigen::MatrixXd & Hcross ); 64 | 65 | /** 66 | * \brief Arun's method for complete point cloud alignment [13]. The method 67 | * actually does the same than threept_arun, but has a different 68 | * interface. 69 | * 70 | * \param[in] p1 The points expressed in the first frame. 71 | * \param[in] p2 The points expressed in the second frame. 72 | * \return The Transformation from frame 2 to frame 1 ( 73 | * \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$, 74 | * with \f$ \mathbf{t} \f$ being the position of frame 2 seen from 75 | * frame 1, and \f$ \mathbf{R} \f$ being the rotation from 76 | * frame 2 to frame 1). 77 | */ 78 | transformation_t arun_complete( const points_t & p1, const points_t & p2 ); 79 | 80 | } 81 | } 82 | 83 | #endif /* OPENGV_ARUN_HPP_ */ 84 | -------------------------------------------------------------------------------- /include/opengv/math/cayley.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file cayley.hpp 33 | * \brief Functions for back-and-forth transformation between rotation matrices 34 | * and Cayley-parameters. 35 | */ 36 | 37 | #ifndef OPENGV_CAYLEY_HPP_ 38 | #define OPENGV_CAYLEY_HPP_ 39 | 40 | #include 41 | #include 42 | 43 | /** 44 | * \brief The namespace of this library. 45 | */ 46 | namespace opengv 47 | { 48 | /** 49 | * \brief The namespace of the math tools. 50 | */ 51 | namespace math 52 | { 53 | 54 | /** 55 | * \brief Compute a rotation matrix from Cayley-parameters, following [14]. 56 | * 57 | * \param[in] cayley The Cayley-parameters of a rotation. 58 | * \return The 3x3 rotation matrix. 59 | */ 60 | rotation_t cayley2rot( const cayley_t & cayley); 61 | 62 | /** 63 | * \brief Compute a fake rotation matrix from Cayley-parameters, following [14]. 64 | * The rotation matrix is missing the scaling parameter of the 65 | * Cayley-transform. This short form is useful for the Jacobian-based 66 | * iterative rotation optimization of the eigensolver [11]. 67 | * 68 | * \param[in] cayley The Cayley-parameters of the rotation. 69 | * \return The false 3x3 rotation matrix. 70 | */ 71 | rotation_t cayley2rot_reduced( const cayley_t & cayley); 72 | 73 | /** 74 | * \brief Compute the Cayley-parameters of a rotation matrix, following [14]. 75 | * 76 | * \param[in] R The 3x3 rotation matrix. 77 | * \return The Cayley-parameters. 78 | */ 79 | cayley_t rot2cayley( const rotation_t & R ); 80 | 81 | } 82 | } 83 | 84 | #endif /* OPENGV_CAYLEY_HPP_ */ 85 | -------------------------------------------------------------------------------- /include/opengv/math/gauss_jordan.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file gauss_jordan.hpp 33 | * \brief Sparse, fast Gauss Jordan elimination. 34 | */ 35 | 36 | #ifndef OPENGV_GAUSS_JORDAN_HPP_ 37 | #define OPENGV_GAUSS_JORDAN_HPP_ 38 | 39 | #include 40 | #include 41 | 42 | /** 43 | * \brief The namespace of this library. 44 | */ 45 | namespace opengv 46 | { 47 | /** 48 | * \brief The namespace of the math tools. 49 | */ 50 | namespace math 51 | { 52 | 53 | /** 54 | * \brief Sparse, fast Gauss Jordan elimination on matrices with a left square 55 | * invertible block. 56 | * 57 | * \param[in] matrix The matrix. 58 | * \param[in] exitCondition The last row we process when stepping up. 59 | */ 60 | void gauss_jordan( 61 | std::vector*> & matrix, 62 | int exitCondition = 0 ); 63 | 64 | } 65 | } 66 | 67 | #endif /* OPENGV_GAUSS_JORDAN_HPP_ */ 68 | -------------------------------------------------------------------------------- /include/opengv/math/quaternion.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file quaternion.hpp 33 | * \brief Functions for back-and-forth transformation between rotation matrices 34 | * and quaternion-parameters. 35 | */ 36 | 37 | #ifndef OPENGV_QUATERNION_HPP_ 38 | #define OPENGV_QUATERNION_HPP_ 39 | 40 | #include 41 | #include 42 | 43 | /** 44 | * \brief The namespace of this library. 45 | */ 46 | namespace opengv 47 | { 48 | /** 49 | * \brief The namespace of the math tools. 50 | */ 51 | namespace math 52 | { 53 | 54 | /** 55 | * \brief Compute a rotation matrix from quaternion-parameters. Assumes that the 56 | * quaternion has unit norm. 57 | * 58 | * \param[in] quaternion The quaternion-parameters of a rotation. 59 | * \return The 3x3 rotation matrix. 60 | */ 61 | rotation_t quaternion2rot( const quaternion_t & quaternion); 62 | 63 | /** 64 | * \brief Compute the quaternion-parameters of a rotation matrix. 65 | * 66 | * \param[in] R The 3x3 rotation matrix. 67 | * \return The quaternion-parameters. 68 | */ 69 | quaternion_t rot2quaternion( const rotation_t & R ); 70 | 71 | } 72 | } 73 | 74 | #endif /* OPENGV_QUATERNION_HPP_ */ 75 | -------------------------------------------------------------------------------- /include/opengv/math/roots.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file roots.hpp 33 | * \brief Closed-form solutions for computing the roots of a polynomial. 34 | */ 35 | 36 | #ifndef OPENGV_ROOTS_HPP_ 37 | #define OPENGV_ROOTS_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | /** 45 | * \brief The namespace of this library. 46 | */ 47 | namespace opengv 48 | { 49 | /** 50 | * \brief The namespace of the math tools. 51 | */ 52 | namespace math 53 | { 54 | 55 | /** 56 | * \brief The roots of a third-order polynomial. 57 | * 58 | * \param[in] p The polynomial coefficients (poly = p[0]*x^3 + p[1]*x^2 ...). 59 | * \return The roots of the polynomial (only real ones). 60 | */ 61 | std::vector o3_roots( const std::vector & p ); 62 | 63 | /** 64 | * \brief Ferrari's method for computing the roots of a fourth order polynomial. 65 | * 66 | * \param[in] p The polynomial coefficients (poly = p(0,0)*x^4 + p(1,0)*x^3 ...). 67 | * \return The roots of the polynomial (only real ones). 68 | */ 69 | std::vector o4_roots( const Eigen::MatrixXd & p ); 70 | 71 | /** 72 | * \brief Ferrari's method for computing the roots of a fourth order polynomial. 73 | * With a different interface. 74 | * 75 | * \param[in] p The polynomial coefficients (poly = p[0]*x^4 + p[1]*x^3 ...). 76 | * \return The roots of the polynomial (only real ones). 77 | */ 78 | std::vector o4_roots( const std::vector & p ); 79 | 80 | } 81 | } 82 | 83 | #endif /* OPENGV_ROOTS_HPP_ */ 84 | -------------------------------------------------------------------------------- /include/opengv/relative_pose/modules/eigensolver/modules.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_RELATIVE_POSE_MODULES_EIGENSOLVER_MODULES_HPP_ 33 | #define OPENGV_RELATIVE_POSE_MODULES_EIGENSOLVER_MODULES_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace opengv 41 | { 42 | namespace relative_pose 43 | { 44 | namespace modules 45 | { 46 | namespace eigensolver 47 | { 48 | 49 | double getSmallestEV( 50 | const Eigen::Matrix3d & xxF, 51 | const Eigen::Matrix3d & yyF, 52 | const Eigen::Matrix3d & zzF, 53 | const Eigen::Matrix3d & xyF, 54 | const Eigen::Matrix3d & yzF, 55 | const Eigen::Matrix3d & zxF, 56 | const cayley_t & cayley, 57 | Eigen::Matrix3d & M ); 58 | double getSmallestEVwithJacobian( 59 | const Eigen::Matrix3d & xxF, 60 | const Eigen::Matrix3d & yyF, 61 | const Eigen::Matrix3d & zzF, 62 | const Eigen::Matrix3d & xyF, 63 | const Eigen::Matrix3d & yzF, 64 | const Eigen::Matrix3d & zxF, 65 | const cayley_t & cayley, 66 | Eigen::Matrix & jacobian); 67 | Eigen::Matrix3d composeM( 68 | const Eigen::Matrix3d & xxF, 69 | const Eigen::Matrix3d & yyF, 70 | const Eigen::Matrix3d & zzF, 71 | const Eigen::Matrix3d & xyF, 72 | const Eigen::Matrix3d & yzF, 73 | const Eigen::Matrix3d & zxF, 74 | const cayley_t & cayley); 75 | Eigen::Matrix3d composeMwithJacobians( 76 | const Eigen::Matrix3d & xxF, 77 | const Eigen::Matrix3d & yyF, 78 | const Eigen::Matrix3d & zzF, 79 | const Eigen::Matrix3d & xyF, 80 | const Eigen::Matrix3d & yzF, 81 | const Eigen::Matrix3d & zxF, 82 | const cayley_t & cayley, 83 | Eigen::Matrix3d & M_jac1, 84 | Eigen::Matrix3d & M_jac2, 85 | Eigen::Matrix3d & M_jac3 ); 86 | 87 | } 88 | } 89 | } 90 | } 91 | 92 | #endif /* OPENGV_RELATIVE_POSE_MODULES_EIGENSOLVER_MODULES_HPP_ */ 93 | 94 | 95 | -------------------------------------------------------------------------------- /include/opengv/relative_pose/modules/fivept_nister/modules.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_RELATIVE_POSE_MODULES_FIVEPT_NISTER_MODULES_HPP_ 33 | #define OPENGV_RELATIVE_POSE_MODULES_FIVEPT_NISTER_MODULES_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace opengv 41 | { 42 | namespace relative_pose 43 | { 44 | namespace modules 45 | { 46 | namespace fivept_nister 47 | { 48 | 49 | void composeA( 50 | const Eigen::Matrix & EE, 51 | Eigen::Matrix & A); 52 | double polyVal(const Eigen::MatrixXd & p, double x); 53 | void computeSeventhOrderPolynomial( 54 | const Eigen::Matrix & p1, 55 | const Eigen::Matrix & p2, 56 | Eigen::Matrix & p_out ); 57 | void computeSixthOrderPolynomial( 58 | const Eigen::Matrix & p1, 59 | const Eigen::Matrix & p2, 60 | Eigen::Matrix & p_out ); 61 | void computeTenthOrderPolynomialFrom73( 62 | const Eigen::Matrix & p1, 63 | const Eigen::Matrix & p2, 64 | Eigen::Matrix & p_out ); 65 | void computeTenthOrderPolynomialFrom64( 66 | const Eigen::Matrix & p1, 67 | const Eigen::Matrix & p2, 68 | Eigen::Matrix & p_out ); 69 | void pollishCoefficients( 70 | const Eigen::Matrix & A, 71 | double & x, 72 | double & y, 73 | double & z); 74 | 75 | } 76 | } 77 | } 78 | } 79 | 80 | #endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_NISTER_MODULES_HPP_ */ 81 | 82 | 83 | -------------------------------------------------------------------------------- /include/opengv/relative_pose/modules/fivept_stewenius/modules.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_ 33 | #define OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace opengv 41 | { 42 | namespace relative_pose 43 | { 44 | namespace modules 45 | { 46 | namespace fivept_stewenius 47 | { 48 | 49 | void composeA( 50 | const Eigen::Matrix & EE, 51 | Eigen::Matrix & A); 52 | 53 | } 54 | } 55 | } 56 | } 57 | 58 | #endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_ */ 59 | 60 | 61 | -------------------------------------------------------------------------------- /include/opengv/relative_pose/modules/sixpt/modules.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #ifndef OPENGV_RELATIVE_POSE_MODULES_SIXPT_MODULES_HPP_ 33 | #define OPENGV_RELATIVE_POSE_MODULES_SIXPT_MODULES_HPP_ 34 | 35 | #define EQS 60 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace opengv 43 | { 44 | namespace relative_pose 45 | { 46 | namespace modules 47 | { 48 | namespace sixpt 49 | { 50 | 51 | //my solver 52 | void fillRow1( 53 | const Eigen::Matrix & l01, 54 | const Eigen::Matrix & l02, 55 | const Eigen::Matrix & l11, 56 | const Eigen::Matrix & l12, 57 | std::vector & c0, 58 | std::vector & c1, 59 | std::vector & c2 ); 60 | void fillRow2( 61 | Eigen::Matrix & M1, 62 | int row, 63 | const std::vector (&m0)[3], 64 | const std::vector (&m1)[3], 65 | const std::vector (&m2)[3] ); 66 | void setupAction( 67 | const std::vector,Eigen::aligned_allocator< Eigen::Matrix > > & L1, 68 | const std::vector,Eigen::aligned_allocator< Eigen::Matrix > > & L2, 69 | Eigen::Matrix & Action ); 70 | 71 | } 72 | } 73 | } 74 | } 75 | 76 | #endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_ */ 77 | 78 | 79 | -------------------------------------------------------------------------------- /include/opengv/sac/Lmeds.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Authors: Johannes Mikulasch * 3 | * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. * 4 | * * 5 | * Redistribution and use in source and binary forms, with or without * 6 | * modification, are permitted provided that the following conditions * 7 | * are met: * 8 | * * Redistributions of source code must retain the above copyright * 9 | * notice, this list of conditions and the following disclaimer. * 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 | * * Neither the name of ANU nor the names of its contributors may be * 14 | * used to endorse or promote products derived from this software without * 15 | * specific prior written permission. * 16 | * * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 20 | * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * 21 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * 22 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 23 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 24 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 26 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 27 | * SUCH DAMAGE. * 28 | ******************************************************************************/ 29 | 30 | //Note: has been derived from Ransac which has been derived from ROS 31 | 32 | /** 33 | * \file Lmeds.hpp 34 | * \brief Implementation of the Lmeds algorithm 35 | */ 36 | 37 | #ifndef OPENGV_SAC_LMEDS_HPP_ 38 | #define OPENGV_SAC_LMEDS_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | /** 45 | * \brief The namespace of this library. 46 | */ 47 | namespace opengv 48 | { 49 | /** 50 | * \brief The namespace for the sample consensus methods. 51 | */ 52 | namespace sac 53 | { 54 | 55 | /** 56 | * The LMedS (Least Median of Squares) sample consensus method 57 | */ 58 | template 59 | class Lmeds : public SampleConsensus 60 | { 61 | public: 62 | /** A child of SampleConsensusProblem */ 63 | typedef PROBLEM_T problem_t; 64 | /** The model we trying to fit */ 65 | typedef typename problem_t::model_t model_t; 66 | 67 | using SampleConsensus::max_iterations_; 68 | using SampleConsensus::threshold_; 69 | using SampleConsensus::iterations_; 70 | using SampleConsensus::sac_model_; 71 | using SampleConsensus::model_; 72 | using SampleConsensus::model_coefficients_; 73 | using SampleConsensus::inliers_; 74 | using SampleConsensus::probability_; 75 | 76 | /** 77 | * \brief Constructor. 78 | */ 79 | Lmeds( 80 | int maxIterations = 1000, 81 | double threshold = 1.0, 82 | double probability = 0.99); 83 | /** 84 | * \brief Destructor. 85 | */ 86 | virtual ~Lmeds(); 87 | 88 | /** 89 | * \brief Fit the model. 90 | */ 91 | bool computeModel( int debug_verbosity_level = 0 ); 92 | }; 93 | 94 | } // namespace sac 95 | } // namespace opengv 96 | 97 | #include "implementation/Lmeds.hpp" 98 | 99 | #endif /* OPENGV_SAC_LMEDS_HPP_ */ 100 | -------------------------------------------------------------------------------- /include/opengv/sac/Ransac.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Authors: Laurent Kneip & Paul Furgale * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | //Note: has been derived from ROS 32 | 33 | /** 34 | * \file Ransac.hpp 35 | * \brief Implementation of the Ransac algorithm as outlined in [15]. 36 | */ 37 | 38 | #ifndef OPENGV_SAC_RANSAC_HPP_ 39 | #define OPENGV_SAC_RANSAC_HPP_ 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | /** 46 | * \brief The namespace of this library. 47 | */ 48 | namespace opengv 49 | { 50 | /** 51 | * \brief The namespace for the sample consensus methods. 52 | */ 53 | namespace sac 54 | { 55 | 56 | /** 57 | * The Ransac sample consensus method, as outlined in [15]. 58 | */ 59 | template 60 | class Ransac : public SampleConsensus 61 | { 62 | public: 63 | /** A child of SampleConsensusProblem */ 64 | typedef PROBLEM_T problem_t; 65 | /** The model we trying to fit */ 66 | typedef typename problem_t::model_t model_t; 67 | 68 | using SampleConsensus::max_iterations_; 69 | using SampleConsensus::threshold_; 70 | using SampleConsensus::iterations_; 71 | using SampleConsensus::sac_model_; 72 | using SampleConsensus::model_; 73 | using SampleConsensus::model_coefficients_; 74 | using SampleConsensus::inliers_; 75 | using SampleConsensus::probability_; 76 | 77 | /** 78 | * \brief Constructor. 79 | */ 80 | Ransac( 81 | int maxIterations = 1000, 82 | double threshold = 1.0, 83 | double probability = 0.99 ); 84 | /** 85 | * \brief Destructor. 86 | */ 87 | virtual ~Ransac(); 88 | 89 | /** 90 | * \brief Fit the model. 91 | */ 92 | bool computeModel( int debug_verbosity_level = 0 ); 93 | }; 94 | 95 | } // namespace sac 96 | } // namespace opengv 97 | 98 | #include "implementation/Ransac.hpp" 99 | 100 | #endif /* OPENGV_SAC_RANSAC_HPP_ */ 101 | -------------------------------------------------------------------------------- /include/opengv/sac/implementation/MultiSampleConsensus.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Authors: Laurent Kneip & Paul Furgale * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | //Note: has been derived from ROS 32 | 33 | template 34 | opengv::sac::MultiSampleConsensus

::MultiSampleConsensus( 35 | int maxIterations, 36 | double threshold, 37 | double probability) : 38 | max_iterations_(maxIterations), 39 | threshold_(threshold), 40 | probability_(probability) 41 | {} 42 | 43 | template 44 | opengv::sac::MultiSampleConsensus

::~MultiSampleConsensus() 45 | {} 46 | -------------------------------------------------------------------------------- /include/opengv/sac/implementation/SampleConsensus.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Authors: Laurent Kneip & Paul Furgale * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | //Note: has been derived from ROS 32 | 33 | template 34 | opengv::sac::SampleConsensus

::SampleConsensus( 35 | int maxIterations, double threshold, double probability) : 36 | max_iterations_(maxIterations), 37 | threshold_(threshold), 38 | probability_(probability) 39 | {} 40 | 41 | template 42 | opengv::sac::SampleConsensus

::~SampleConsensus(){} 43 | -------------------------------------------------------------------------------- /include/opengv/triangulation/methods.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file triangulation/methods.hpp 33 | * \brief Some triangulation methods. Not exhaustive. 34 | */ 35 | 36 | #ifndef OPENGV_TRIANGULATION_METHODS_HPP_ 37 | #define OPENGV_TRIANGULATION_METHODS_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | /** 45 | * \brief The namespace of this library. 46 | */ 47 | namespace opengv 48 | { 49 | /** 50 | * \brief The namespace for the triangulation methods. 51 | */ 52 | namespace triangulation 53 | { 54 | 55 | /** 56 | * \brief Compute the position of a 3D point seen from two viewpoints. Linear 57 | * Method. 58 | * 59 | * \param[in] adapter Visitor holding bearing-vector correspondences, plus the 60 | * relative transformation. 61 | * \param[in] index The index of the correspondence being triangulated. 62 | * \return The 3D point expressed in the first viewpoint. 63 | */ 64 | point_t triangulate( 65 | const relative_pose::RelativeAdapterBase & adapter, size_t index ); 66 | 67 | /** 68 | * \brief Compute the position of a 3D point seen from two viewpoints. Fast 69 | * non-linear approximation (closed-form). 70 | * 71 | * \param[in] adapter Visitor holding bearing-vector correspondences, plus the 72 | * relative transformation. 73 | * \param[in] index The index of the correspondence being triangulated. 74 | * \return The 3D point expressed in the first viewpoint. 75 | */ 76 | point_t triangulate2( 77 | const relative_pose::RelativeAdapterBase & adapter, size_t index ); 78 | 79 | } 80 | 81 | } 82 | 83 | #endif /* OPENGV_TRIANGULATION_METHODS_HPP_ */ 84 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | opengv 5 | 6 | 7 | Laurent Kneip 8 | FreeBSD 9 | 10 | http://ros.org/wiki/opengv 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /matlab/benchmark_absolute_pose.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 1; 12 | % let's only get 6 points, and generate new ones in each iteration 13 | pt_number = 6; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 5000 iterations per noise level 17 | iterations = 5000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'p3p_kneip'; 'p3p_gao'; 'epnp'; 'abs_nonlin_central'; 'upnp'; 'upnp' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1, 2, 3]; [1, 2, 3]; [1, 2, 3, 4, 5, 6]; [1, 2, 3, 4, 5, 6]; [1, 2, 3, 4, 5, 6]; [1, 2, 3] }; 23 | % The name of the algorithms on the plots 24 | names = { 'P3P (Kneip)'; 'P3P (Gao)'; 'EPnP'; 'nonlinear optimization'; 'UPnP'; 'UPnP (minimal)' }; 25 | 26 | % The maximum noise to analyze 27 | max_noise = 5.0; 28 | % The step in between different noise levels 29 | noise_step = 0.1; 30 | 31 | %% Run the benchmark 32 | 33 | %prepare the overall result arrays 34 | number_noise_levels = max_noise / noise_step + 1; 35 | num_algorithms = size(algorithms,1); 36 | mean_position_errors = zeros(num_algorithms,number_noise_levels); 37 | mean_rotation_errors = zeros(num_algorithms,number_noise_levels); 38 | median_position_errors = zeros(num_algorithms,number_noise_levels); 39 | median_rotation_errors = zeros(num_algorithms,number_noise_levels); 40 | noise_levels = zeros(1,number_noise_levels); 41 | 42 | %Run the experiment 43 | for n=1:number_noise_levels 44 | 45 | noise = (n - 1) * noise_step; 46 | noise_levels(1,n) = noise; 47 | display(['Analyzing noise level: ' num2str(noise)]) 48 | 49 | position_errors = zeros(num_algorithms,iterations); 50 | rotation_errors = zeros(num_algorithms,iterations); 51 | 52 | counter = 0; 53 | 54 | for i=1:iterations 55 | 56 | % generate experiment 57 | [points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction); 58 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 59 | T_perturbed = [R_perturbed,t_perturbed]; 60 | T_gt = [R,t]; 61 | 62 | % run all algorithms 63 | for a=1:num_algorithms 64 | T = opengv(algorithms{a},indices{a},points,v,T_perturbed); 65 | [position_error, rotation_error] = evaluateTransformationError( T_gt, T ); 66 | position_errors(a,i) = position_error; 67 | rotation_errors(a,i) = rotation_error; 68 | end 69 | 70 | counter = counter + 1; 71 | if counter == 100 72 | counter = 0; 73 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 74 | end 75 | end 76 | 77 | %Now compute the mean and median value of the error for each algorithm 78 | for a=1:num_algorithms 79 | mean_position_errors(a,n) = mean(position_errors(a,:)); 80 | median_position_errors(a,n) = median(position_errors(a,:)); 81 | mean_rotation_errors(a,n) = mean(rotation_errors(a,:)); 82 | median_rotation_errors(a,n) = median(rotation_errors(a,:)); 83 | end 84 | 85 | end 86 | 87 | %% Plot the results 88 | 89 | figure(1) 90 | plot(noise_levels',mean_rotation_errors','LineWidth',2) 91 | legend(names,'Location','NorthWest') 92 | xlabel('noise level [pix]') 93 | ylabel('mean rot. error [rad]') 94 | grid on 95 | 96 | figure(2) 97 | plot(noise_levels',median_rotation_errors','LineWidth',2) 98 | legend(names,'Location','NorthWest') 99 | xlabel('noise level [pix]') 100 | ylabel('median rot. error [rad]') 101 | grid on 102 | 103 | figure(3) 104 | plot(noise_levels',mean_position_errors','LineWidth',2) 105 | legend(names,'Location','NorthWest') 106 | xlabel('noise level [pix]') 107 | ylabel('mean pos. error [m]') 108 | grid on 109 | 110 | figure(4) 111 | plot(noise_levels',median_position_errors','LineWidth',2) 112 | legend(names,'Location','NorthWest') 113 | xlabel('noise level [pix]') 114 | ylabel('median pos. error [m]') 115 | grid on -------------------------------------------------------------------------------- /matlab/benchmark_absolute_pose_execution_times.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 1; 12 | % let's only get 6 points, and generate new ones in each iteration 13 | pt_number = 50; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 iterations 17 | iterations = 1000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'p2p'; 'p3p_kneip'; 'p3p_gao'; 'epnp'; 'upnp' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1, 2]; [1, 2, 3]; [1, 2, 3]; [1:1:50]; [1:1:50] }; 23 | % The name of the algorithms on the plots 24 | names = { 'P2P'; 'P3P (Kneip)'; 'P3P (Gao)'; 'EPnP (50pts)'; 'UPnP (50pts)'}; 25 | 26 | % The noise in this experiment 27 | noise = 1.0; 28 | 29 | %% Run the benchmark 30 | 31 | %prepare the overall result array 32 | num_algorithms = size(algorithms,1); 33 | execution_times = zeros(num_algorithms,iterations); 34 | counter = 0; 35 | 36 | for i=1:iterations 37 | 38 | % generate experiment 39 | [points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction); 40 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 41 | T_perturbed = [R_perturbed,t_perturbed]; 42 | 43 | % run all algorithms 44 | for a=1:num_algorithms 45 | tic; 46 | T = opengv_donotuse(algorithms{a},indices{a},points,v,T_perturbed); 47 | execution_times(a,i) = toc/20.0; 48 | end 49 | 50 | counter = counter + 1; 51 | if counter == 100 52 | counter = 0; 53 | display(['Iteration ' num2str(i) ' of ' num2str(iterations)]); 54 | end 55 | end 56 | 57 | %% Plot the results 58 | 59 | bins = [0.000001:0.000001:0.00001]; 60 | hist(execution_times',bins) 61 | legend(names,'Location','NorthWest') 62 | xlabel('execution times [s]') 63 | grid on 64 | 65 | mean(execution_times') 66 | -------------------------------------------------------------------------------- /matlab/benchmark_absolute_pose_noncentral_execution_timing.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 4; 12 | % let's only get 6 points, and generate new ones in each iteration 13 | pt_number = 50; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 iterations 17 | iterations = 1000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'gp3p'; 'gpnp'; 'upnp'}; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1, 2, 3]; [1:1:50]; [1:1:50] }; 23 | % The name of the algorithms on the plots 24 | names = { 'GP3P'; 'GPnP (50pts)'; 'UPnP (50pts)' }; 25 | 26 | % The noise in this experiment 27 | noise = 1.0; 28 | 29 | %% Run the benchmark 30 | 31 | %prepare the overall result arrays 32 | num_algorithms = size(algorithms,1); 33 | execution_times = zeros(num_algorithms,iterations); 34 | counter = 0; 35 | 36 | for i=1:iterations 37 | 38 | % generate experiment 39 | [points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction); 40 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 41 | T_perturbed = [R_perturbed,t_perturbed]; 42 | 43 | for a=1:num_algorithms 44 | tic 45 | T = opengv_donotuse(algorithms{a},indices{a},points,v,T_perturbed); 46 | execution_times(a,i) = toc/20.0; 47 | end 48 | 49 | counter = counter + 1; 50 | if counter == 100 51 | counter = 0; 52 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 53 | end 54 | end 55 | 56 | %% Plot the results 57 | 58 | hist(execution_times') 59 | legend(names,'Location','NorthWest') 60 | xlabel('execution times [s]') 61 | grid on 62 | 63 | mean(execution_times') 64 | -------------------------------------------------------------------------------- /matlab/benchmark_relative_pose.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 1; 12 | % Getting 10 points, and testing all algorithms with the respective number of points 13 | pt_number = 10; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 5000 tests per noise level 17 | iterations = 5000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'fivept_stewenius'; 'fivept_nister'; 'fivept_kneip'; 'sevenpt'; 'eightpt'; 'eigensolver'; 'rel_nonlin_central' }; 21 | % Some parameter that tells us what the result means 22 | returns = [ 1, 1, 0, 1, 1, 0, 2 ]; % 1means essential matrix(ces) needing decomposition, %0 means rotation matrix(ces), %2 means transformation matrix 23 | % This defines the number of points used for every algorithm 24 | indices = { [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5, 6, 7]; [1, 2, 3, 4, 5, 6, 7, 8]; [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]; [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] }; 25 | % The name of the algorithms in the final plots 26 | names = { '5pt (Stewenius)'; '5pt (Nister)'; '5pt (Kneip)'; '7pt'; '8pt'; 'eigensolver (10pts)'; 'nonlin. opt. (10pts)' }; 27 | 28 | % The maximum noise to analyze 29 | max_noise = 5.0; 30 | % The step in between different noise levels 31 | noise_step = 0.1; 32 | 33 | %% Run the benchmark 34 | 35 | %prepare the overall result arrays 36 | number_noise_levels = max_noise / noise_step + 1; 37 | num_algorithms = size(algorithms,1); 38 | mean_rotation_errors = zeros(num_algorithms,number_noise_levels); 39 | median_rotation_errors = zeros(num_algorithms,number_noise_levels); 40 | noise_levels = zeros(1,number_noise_levels); 41 | 42 | %Run the experiment 43 | for n=1:number_noise_levels 44 | 45 | noise = (n - 1) * noise_step; 46 | noise_levels(1,n) = noise; 47 | display(['Analyzing noise level: ' num2str(noise)]) 48 | 49 | rotation_errors = zeros(num_algorithms,iterations); 50 | 51 | counter = 0; 52 | 53 | validIterations = 0; 54 | 55 | for i=1:iterations 56 | 57 | % generate experiment 58 | [v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction); 59 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 60 | T_perturbed = [R_perturbed,t_perturbed]; 61 | R_gt = R; 62 | 63 | % run all algorithms 64 | allValid = 1; 65 | 66 | for a=1:num_algorithms 67 | Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed); 68 | 69 | if ~isempty(Out) 70 | 71 | if returns(1,a) == 1 72 | temp = transformEssentials(Out); 73 | Out = temp; 74 | end 75 | if returns(1,a) == 2 76 | temp = Out(:,1:3); 77 | Out = temp; 78 | end 79 | 80 | rotation_errors(a,validIterations+1) = evaluateRotationError( R_gt, Out ); 81 | 82 | else 83 | 84 | allValid = 0; 85 | break; 86 | 87 | end 88 | end 89 | 90 | if allValid == 1 91 | validIterations = validIterations +1; 92 | end 93 | 94 | counter = counter + 1; 95 | if counter == 100 96 | counter = 0; 97 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 98 | end 99 | end 100 | 101 | %Now compute the mean and median value of the error for each algorithm 102 | for a=1:num_algorithms 103 | mean_rotation_errors(a,n) = mean(rotation_errors(a,1:validIterations)); 104 | median_rotation_errors(a,n) = median(rotation_errors(a,1:validIterations)); 105 | end 106 | 107 | end 108 | 109 | %% Plot the results 110 | 111 | figure(1) 112 | plot(noise_levels,mean_rotation_errors,'LineWidth',2) 113 | legend(names,'Location','NorthWest') 114 | xlabel('noise level [pix]') 115 | ylabel('mean rot. error [rad]') 116 | grid on 117 | 118 | figure(2) 119 | plot(noise_levels,median_rotation_errors,'LineWidth',2) 120 | legend(names,'Location','NorthWest') 121 | xlabel('noise level [pix]') 122 | ylabel('median rot. error [rad]') 123 | grid on 124 | -------------------------------------------------------------------------------- /matlab/benchmark_relative_pose_execution_times.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 1; 12 | % Getting 10 points, and testing all algorithms with the respective number of points 13 | pt_number = 10; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 iterations 17 | iterations = 1000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'twopt';'fivept_stewenius'; 'fivept_nister'; 'fivept_kneip'; 'sevenpt'; 'eightpt'; 'eigensolver' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1,2]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5]; [1, 2, 3, 4, 5, 6, 7]; [1, 2, 3, 4, 5, 6, 7, 8]; [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] }; 23 | % The name of the algorithms in the final plots 24 | names = { '2pt';'5pt (Stewenius)'; '5pt (Nister)'; '5pt (Kneip)'; '7pt'; '8pt'; 'eigensolver' }; 25 | 26 | % noise in this experiment 27 | noise = 1.0; 28 | 29 | %% Run the benchmark 30 | 31 | %prepare the overall result arrays 32 | num_algorithms = size(algorithms,1); 33 | execution_times = zeros(num_algorithms,iterations); 34 | counter = 0; 35 | 36 | for i=1:iterations 37 | 38 | % generate experiment 39 | [v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction); 40 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 41 | T_perturbed = [R_perturbed,t_perturbed]; 42 | 43 | for a=1:num_algorithms 44 | tic 45 | Out = opengv_donotuse(algorithms{a},indices{a},v1,v2,T_perturbed); 46 | execution_times(a,i) = toc/20.0; 47 | end 48 | 49 | counter = counter + 1; 50 | if counter == 100 51 | counter = 0; 52 | display(['Iteration ' num2str(i) ' of ' num2str(iterations)]); 53 | end 54 | end 55 | 56 | %% Plot the results 57 | 58 | hist(execution_times') 59 | legend(names,'Location','NorthWest') 60 | xlabel('execution times [s]') 61 | grid on 62 | 63 | mean(execution_times') 64 | -------------------------------------------------------------------------------- /matlab/benchmark_relative_pose_noncentral.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % noncentral case 11 | cam_number = 4; 12 | % Getting 10 points, and testing all algorithms with the respective number of points 13 | pt_number = 50; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 5000 tests per noise level 17 | iterations = 5000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'seventeenpt'; 'rel_nonlin_noncentral'; 'rel_nonlin_noncentral' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1:1:17]; [1:1:17]; [1:1:50] }; 23 | % The name of the algorithms in the final plots 24 | names = { '17pt'; 'nonlin. opt. (17pts)'; 'nonlin. opt. (50pts)' }; 25 | 26 | % The maximum noise to analyze 27 | max_noise = 5.0; 28 | % The step in between different noise levels 29 | noise_step = 0.1; 30 | 31 | %% Run the benchmark 32 | 33 | %prepare the overall result arrays 34 | number_noise_levels = max_noise / noise_step + 1; 35 | num_algorithms = size(algorithms,1); 36 | mean_rotation_errors = zeros(num_algorithms,number_noise_levels); 37 | median_rotation_errors = zeros(num_algorithms,number_noise_levels); 38 | mean_position_errors = zeros(num_algorithms,number_noise_levels); 39 | median_position_errors = zeros(num_algorithms,number_noise_levels); 40 | noise_levels = zeros(1,number_noise_levels); 41 | 42 | %Run the experiment 43 | for n=1:number_noise_levels 44 | 45 | noise = (n - 1) * noise_step; 46 | noise_levels(1,n) = noise; 47 | display(['Analyzing noise level: ' num2str(noise)]) 48 | 49 | rotation_errors = zeros(num_algorithms,iterations); 50 | position_errors = zeros(num_algorithms,iterations); 51 | 52 | counter = 0; 53 | 54 | for i=1:iterations 55 | 56 | % generate experiment 57 | [v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction); 58 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 59 | T_perturbed = [R_perturbed,t_perturbed]; 60 | T_gt = [R,t]; 61 | 62 | for a=1:num_algorithms 63 | Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed); 64 | [position_error, rotation_error] = evaluateTransformationError( T_gt, Out ); 65 | position_errors(a,i) = position_error; 66 | rotation_errors(a,i) = rotation_error; 67 | end 68 | 69 | counter = counter + 1; 70 | if counter == 100 71 | counter = 0; 72 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 73 | end 74 | end 75 | 76 | %Now compute the mean and median value of the error for each algorithm 77 | for a=1:num_algorithms 78 | mean_rotation_errors(a,n) = mean(rotation_errors(a,:)); 79 | median_rotation_errors(a,n) = median(rotation_errors(a,:)); 80 | mean_position_errors(a,n) = mean(position_errors(a,:)); 81 | median_position_errors(a,n) = median(position_errors(a,:)); 82 | end 83 | 84 | end 85 | 86 | %% Plot the results 87 | 88 | figure(1) 89 | plot(noise_levels,mean_rotation_errors,'LineWidth',2) 90 | legend(names,'Location','NorthWest') 91 | xlabel('noise level [pix]') 92 | ylabel('mean rot. error [rad]') 93 | grid on 94 | 95 | figure(2) 96 | plot(noise_levels,median_rotation_errors,'LineWidth',2) 97 | legend(names,'Location','NorthWest') 98 | xlabel('noise level [pix]') 99 | ylabel('median rot. error [rad]') 100 | grid on 101 | 102 | figure(3) 103 | plot(noise_levels,mean_position_errors,'LineWidth',2) 104 | legend(names,'Location','NorthWest') 105 | xlabel('noise level [pix]') 106 | ylabel('mean pos. error [m]') 107 | grid on 108 | 109 | figure(4) 110 | plot(noise_levels,median_position_errors,'LineWidth',2) 111 | legend(names,'Location','NorthWest') 112 | xlabel('noise level [pix]') 113 | ylabel('median pos. error [m]') 114 | grid on -------------------------------------------------------------------------------- /matlab/benchmark_relative_pose_noncentral2.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % noncentral case 11 | cam_number = 4; 12 | % Getting 17 points, and testing all algorithms with the respective number of points 13 | pt_number = 17; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 tests per noise level 17 | iterations = 1000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'sixpt'; 'ge'; 'ge'; 'seventeenpt'; 'rel_nonlin_noncentral' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1:1:6]; [1:1:8]; [1:1:17]; [1:1:17]; [1:1:17] }; 23 | % The name of the algorithms in the final plots 24 | names = { '6pt'; 'ge (8pt)'; 'ge (17pt)'; '17pt'; 'nonlin. opt. (17pt)' }; 25 | 26 | % The maximum noise to analyze 27 | max_noise = 5.0; 28 | % The step in between different noise levels 29 | noise_step = 0.1; 30 | 31 | %% Run the benchmark 32 | 33 | %prepare the overall result arrays 34 | number_noise_levels = max_noise / noise_step + 1; 35 | num_algorithms = size(algorithms,1); 36 | mean_rotation_errors = zeros(num_algorithms,number_noise_levels); 37 | median_rotation_errors = zeros(num_algorithms,number_noise_levels); 38 | noise_levels = zeros(1,number_noise_levels); 39 | 40 | %Run the experiment 41 | for n=1:number_noise_levels 42 | 43 | noise = (n - 1) * noise_step; 44 | noise_levels(1,n) = noise; 45 | display(['Analyzing noise level: ' num2str(noise)]) 46 | 47 | rotation_errors = zeros(num_algorithms,iterations); 48 | 49 | counter = 0; 50 | 51 | for i=1:iterations 52 | 53 | % generate experiment 54 | [v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction); 55 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 56 | T_perturbed = [R_perturbed,t_perturbed]; 57 | T_init = [eye(3),zeros(3,1)]; 58 | T_gt = [R,t]; 59 | 60 | for a=1:num_algorithms 61 | 62 | if strcmp(algorithms{a},'ge') 63 | Out = opengv(algorithms{a},indices{a},v1,v2,T_init); 64 | else 65 | Out = opengv(algorithms{a},indices{a},v1,v2,T_perturbed); 66 | end 67 | 68 | if a > 3 %if a bigger than 4, we obtain entire transformation, and need to "cut" the rotation 69 | temp = Out(:,1:3); 70 | Out = temp; 71 | end 72 | 73 | rotation_error = evaluateRotationError( R, Out ); 74 | rotation_errors(a,i) = rotation_error; 75 | end 76 | 77 | counter = counter + 1; 78 | if counter == 100 79 | counter = 0; 80 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 81 | end 82 | end 83 | 84 | %Now compute the mean and median value of the error for each algorithm 85 | for a=1:num_algorithms 86 | mean_rotation_errors(a,n) = mean(rotation_errors(a,:)); 87 | median_rotation_errors(a,n) = median(rotation_errors(a,:)); 88 | end 89 | 90 | end 91 | 92 | %% Plot the results 93 | 94 | figure(1) 95 | plot(noise_levels,mean_rotation_errors,'LineWidth',2) 96 | legend(names,'Location','NorthWest') 97 | xlabel('noise level [pix]') 98 | ylabel('mean rot. error [rad]') 99 | grid on 100 | 101 | figure(2) 102 | plot(noise_levels,median_rotation_errors,'LineWidth',2) 103 | legend(names,'Location','NorthWest') 104 | xlabel('noise level [pix]') 105 | ylabel('median rot. error [rad]') 106 | grid on -------------------------------------------------------------------------------- /matlab/benchmark_relative_pose_noncentral_execution_times.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 4; 12 | % Getting 10 points, and testing all algorithms with the respective number of points 13 | pt_number = 50; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 iterations 17 | iterations = 1000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'seventeenpt' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1:1:17] }; 23 | % The name of the algorithms in the final plots 24 | names = { '17pt' }; 25 | 26 | % The noise in this experiment 27 | noise = 1.0; 28 | 29 | %% Run the benchmark 30 | 31 | %prepare the overall result arrays 32 | num_algorithms = size(algorithms,1); 33 | execution_times = zeros(num_algorithms,iterations); 34 | counter = 0; 35 | 36 | for i=1:iterations 37 | 38 | % generate experiment 39 | [v1,v2,t,R] = create2D2DExperiment(pt_number,cam_number,noise,outlier_fraction); 40 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 41 | T_perturbed = [R_perturbed,t_perturbed]; 42 | 43 | for a=1:num_algorithms 44 | tic 45 | Out = opengv_donotuse(algorithms{a},indices{a},v1,v2,T_perturbed); 46 | execution_times(a,i) = toc/20.0; 47 | end 48 | 49 | counter = counter + 1; 50 | if counter == 100 51 | counter = 0; 52 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 53 | end 54 | end 55 | 56 | %% Plot the results 57 | 58 | hist(execution_times') 59 | legend(names,'Location','NorthWest') 60 | xlabel('execution times [s]') 61 | grid on 62 | 63 | mean(execution_times') 64 | -------------------------------------------------------------------------------- /matlab/benchmark_relative_pose_noncentral_execution_times2.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % central case -> only one camera 11 | cam_number = 4; 12 | % Getting 17 points, and testing all algorithms with the respective number of points 13 | pt_number = 17; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 iterations 17 | iterations = 1000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'sixpt'; 'ge'; 'seventeenpt' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1:1:6]; [1:1:8]; [1:1:17] }; 23 | % The name of the algorithms in the final plots 24 | names = { '6pt';'ge (8pt)'; '17pt' }; 25 | 26 | % The noise in this experiment 27 | noise = 0.5; 28 | 29 | %% Run the benchmark 30 | 31 | %prepare the overall result arrays 32 | num_algorithms = size(algorithms,1); 33 | execution_times = zeros(num_algorithms,iterations); 34 | counter = 0; 35 | 36 | for i=1:iterations 37 | 38 | % generate experiment 39 | [v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction); 40 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 41 | T_perturbed = [R_perturbed,t_perturbed]; 42 | T_init = [eye(3) zeros(3,1)]; 43 | 44 | for a=1:num_algorithms 45 | tic 46 | Out = opengv_donotuse(algorithms{a},indices{a},v1,v2,T_init); 47 | execution_times(a,i) = toc/20.0; 48 | end 49 | 50 | counter = counter + 1; 51 | if counter == 1 52 | counter = 0; 53 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 54 | end 55 | end 56 | 57 | %% Plot results 58 | 59 | hist(log10(execution_times)') 60 | 61 | legend(names,'Location','NorthWest') 62 | xlabel('execution time [s]') 63 | ylabel('occurence') 64 | grid on 65 | 66 | %% print the mean and median execution time on the console 67 | 68 | display( 'mean execution times:' ) 69 | display(['sixpt: ' num2str(mean(execution_times(1,:)'))] ); 70 | display(['ge: ' num2str(mean(execution_times(2,:)'))] ); 71 | display(['seventeenpt: ' num2str(mean(execution_times(3,:)'))] ); 72 | 73 | %% Plot the results 74 | % 75 | % [y1,x1] = hist(execution_times(1,:)); 76 | % [y2,x2] = hist(execution_times(2,:)); 77 | % [y3,x3] = hist(execution_times(3,:)); 78 | % 79 | % y1 = y1 / (x1(1,2) - x1(1,1)); 80 | % y2 = y2 / (x2(1,2) - x2(1,1)); 81 | % y3 = y3 / (x3(1,2) - x3(1,1)); 82 | % 83 | % figure(2) 84 | % hold on 85 | % plot(x1,y1,'b'); 86 | % plot(x2,y2,'g'); 87 | % plot(x3,y3,'r'); 88 | % legend(names,'Location','NorthWest') 89 | % xlabel('execution time [s]') 90 | % ylabel('probability') 91 | % grid on 92 | -------------------------------------------------------------------------------- /matlab/helpers/addNoise.m: -------------------------------------------------------------------------------- 1 | function v_noisy = addNoise(v_clean,focal_length,pixel_noise) 2 | 3 | %find good vector in normal plane based on good conditioning 4 | inPlaneVector1 = zeros(3,1); 5 | 6 | if v_clean(1,1) > v_clean(2,1) && v_clean(1,1) > v_clean(3,1) 7 | inPlaneVector1(2,1) = 1.0; 8 | inPlaneVector1(3,1) = 0.0; 9 | inPlaneVector1(1,1) = 1.0/v_clean(1,1) * (-v_clean(2,1)); 10 | else 11 | if v_clean(2,1) > v_clean(3,1) && v_clean(2,1) > v_clean(1,1) 12 | inPlaneVector1(3,1) = 1.0; 13 | inPlaneVector1(1,1) = 0.0; 14 | inPlaneVector1(2,1) = 1.0/v_clean(2,1) * (-v_clean(3,1)); 15 | else 16 | %v_clean(3,1) > v_clean(1,1) && v_clean(3,1) > v_clean(2,1) 17 | inPlaneVector1(1,1) = 1.0; 18 | inPlaneVector1(2,1) = 0.0; 19 | inPlaneVector1(3,1) = 1.0/v_clean(3,1) * (-v_clean(1,1)); 20 | end 21 | end 22 | 23 | %normalize the in-plane vector 24 | inPlaneVector1 = inPlaneVector1 / norm(inPlaneVector1); 25 | inPlaneVector2 = cross(v_clean,inPlaneVector1); 26 | 27 | noiseX = pixel_noise * (rand-0.5)*2.0;% / sqrt(2); 28 | noiseY = pixel_noise * (rand-0.5)*2.0;% / sqrt(2); 29 | 30 | v_noisy = focal_length * v_clean + noiseX * inPlaneVector1 + noiseY * inPlaneVector2; 31 | 32 | v_noisy_norm = norm(v_noisy); 33 | v_noisy = v_noisy ./ v_noisy_norm; 34 | 35 | end -------------------------------------------------------------------------------- /matlab/helpers/cayley2rot.m: -------------------------------------------------------------------------------- 1 | function R = cayley2rot(v) 2 | 3 | cayley0 = v(1,1); 4 | cayley1 = v(2,1); 5 | cayley2 = v(3,1); 6 | 7 | R = zeros(3,3); 8 | scale = 1+cayley0^2+cayley1^2+cayley2^2; 9 | 10 | R(1,1) = 1+cayley0^2-cayley1^2-cayley2^2; 11 | R(1,2) = 2*(cayley0*cayley1-cayley2); 12 | R(1,3) = 2*(cayley0*cayley2+cayley1); 13 | R(2,1) = 2*(cayley0*cayley1+cayley2); 14 | R(2,2) = 1-cayley0^2+cayley1^2-cayley2^2; 15 | R(2,3) = 2*(cayley1*cayley2-cayley0); 16 | R(3,1) = 2*(cayley0*cayley2-cayley1); 17 | R(3,2) = 2*(cayley1*cayley2+cayley0); 18 | R(3,3) = 1-cayley0^2-cayley1^2+cayley2^2; 19 | 20 | R = (1/scale) * R; -------------------------------------------------------------------------------- /matlab/helpers/create2D2DOmniExperiment.m: -------------------------------------------------------------------------------- 1 | function [v1, v2, t, R ] = create2D2DOmniExperiment( pt_number, cam_number, noise, outlier_fraction ) 2 | 3 | %% generate the camera system 4 | 5 | cam_distance = 1.0; 6 | 7 | %% set a regular camera system with 2 or 4 cameras here 8 | if cam_number == 2 9 | cam_offsets = [ cam_distance -cam_distance; 0.0 0.0; 0.0 0.0 ]; 10 | else 11 | cam_number = 4; % only two or 4 supported for this experiment 12 | cam_offsets = [ cam_distance 0.0 -cam_distance 0.0; 0.0 cam_distance 0.0 -cam_distance; 0.0 0.0 0.0 0.0 ]; 13 | end 14 | 15 | %% generate random view-points 16 | 17 | max_parallax = 2.0; 18 | max_rotation = 0.5; 19 | 20 | position1 = zeros(3,1); 21 | rotation1 = eye(3); 22 | 23 | position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1)); 24 | rotation2 = generateBoundedR(max_rotation); 25 | 26 | %% Generate random point-cloud 27 | 28 | avg_depth_over_cam_distance = 10.0; 29 | maxDepth = 5.0; 30 | 31 | normalizedPoints = 2.0*(rand(3,pt_number)-repmat(0.5,3,pt_number)); 32 | points = maxDepth * normalizedPoints; 33 | 34 | %% Now create the correspondences by looping through the cameras 35 | 36 | focal_length = 800.0; 37 | 38 | v1 = zeros(6,pt_number); 39 | v2 = zeros(6,pt_number); 40 | cam_correspondence = 1; 41 | cam_correspondences = zeros(1,pt_number); 42 | 43 | for i=1:pt_number 44 | 45 | cam_offset = cam_offsets(:,cam_correspondence); 46 | %cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3); 47 | 48 | %special: shift the point in the first frame along current camera axis, which guarantees homogeneous distribution 49 | temp = points(:,i) + avg_depth_over_cam_distance * cam_offset; 50 | points(:,i) = temp; 51 | 52 | body_point1 = rotation1' * (points(:,i)-position1); 53 | body_point2 = rotation2' * (points(:,i)-position2); 54 | 55 | % we actually omit the can rotation here by unrotating the bearing 56 | % vectors already 57 | bearingVector1 = body_point1 - cam_offset; 58 | bearingVector2 = body_point2 - cam_offset; 59 | bearingVector1_norm = norm(bearingVector1); 60 | bearingVector2_norm = norm(bearingVector2); 61 | bearingVector1 = bearingVector1/bearingVector1_norm; 62 | bearingVector2 = bearingVector2/bearingVector2_norm; 63 | 64 | % add noise to the bearing vectors here 65 | bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise); 66 | bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise); 67 | 68 | % store the normalized bearing vectors along with the cameras they are 69 | % being seen (we create correspondences that always originate from the 70 | % same camera, you can change this if you want) 71 | bearingVector1_norm = norm(bearingVector1_noisy); 72 | bearingVector2_norm = norm(bearingVector2_noisy); 73 | 74 | v1(:,i) = [bearingVector1_noisy./bearingVector1_norm; cam_offset]; 75 | v2(:,i) = [bearingVector2_noisy./bearingVector2_norm; cam_offset]; 76 | 77 | % change the camera correspondence 78 | cam_correspondences(1,i) = cam_correspondence; 79 | cam_correspondence = cam_correspondence + 1; 80 | if cam_correspondence > cam_number 81 | cam_correspondence = 1; 82 | end 83 | end 84 | 85 | %% Add outliers 86 | number_outliers = floor(outlier_fraction*pt_number); 87 | 88 | if number_outliers > 0 89 | for i=1:number_outliers 90 | 91 | cam_correspondence = cam_correspondences(1,i); 92 | 93 | cam_offset = cam_offsets(:,cam_correspondence); 94 | %cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3); 95 | 96 | %generate random point 97 | normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1)); 98 | point = maxDepth * normalizedPoint + avg_depth_over_cam_distance * cam_offset; 99 | 100 | 101 | body_point2 = rotation2' * (point-position2); 102 | 103 | % store the point (no need to add noise) 104 | bearingVector2 = body_point2 - cam_offset; 105 | 106 | % store the normalized bearing vectors along with the cameras they are 107 | % being seen (we create correspondences that always originate from the 108 | % same camera, you can change this if you want) 109 | bearingVector2_norm = norm(bearingVector2); 110 | 111 | v2(:,i) = [bearingVector2./bearingVector2_norm; cam_offset]; 112 | end 113 | end 114 | 115 | %% compute relative translation and rotation 116 | 117 | R = rotation1' * rotation2; 118 | t = rotation1' * (position2 - position1); -------------------------------------------------------------------------------- /matlab/helpers/create2D3DExperiment.m: -------------------------------------------------------------------------------- 1 | function [points, v, t, R ] = create2D3DExperiment( pt_number, cam_number, noise, outlier_fraction ) 2 | 3 | %% generate the camera system 4 | 5 | avg_cam_distance = 0.5; 6 | cam_offsets = zeros(3,cam_number); 7 | %cam_rotations = zeros(3,cam_number*3); 8 | 9 | if cam_number == 1 10 | cam_offsets = zeros(3,1); 11 | %cam_rotations = eye(3); 12 | else 13 | for i=1:cam_number 14 | cam_offsets(:,i) = avg_cam_distance * generateRandomR() * [1.0; 0.0; 0.0]; 15 | %cam_rotations(:,(i-1)*3+1:(i-1)*3+3) = generateRandomR(); 16 | end 17 | end 18 | 19 | %% generate random view-points 20 | 21 | max_parallax = 2.0; 22 | max_rotation = 0.5; 23 | 24 | position = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1)); 25 | rotation = generateBoundedR(max_rotation); 26 | 27 | %% Generate random point-cloud 28 | 29 | minDepth = 4.0; 30 | maxDepth = 8.0; 31 | 32 | normalizedPoints = 2.0*(rand(3,pt_number)-repmat(0.5,3,pt_number)); 33 | norms = sqrt(sum(normalizedPoints.*normalizedPoints)); 34 | directions = normalizedPoints./repmat(norms,3,1); 35 | points = (maxDepth-minDepth) * normalizedPoints + minDepth * directions; 36 | 37 | %% Now create the correspondences by looping through the cameras 38 | 39 | focal_length = 800.0; 40 | 41 | v = zeros(6,pt_number); 42 | cam_correspondence = 1; 43 | cam_correspondences = zeros(1,pt_number); 44 | 45 | for i=1:pt_number 46 | 47 | cam_offset = cam_offsets(:,cam_correspondence); 48 | %cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3); 49 | 50 | body_point = rotation' * (points(:,i)-position); 51 | 52 | % we actually omit the can rotation here by unrotating the bearing 53 | % vectors already 54 | bearingVector = body_point - cam_offset; 55 | bearingVector_norm = norm(bearingVector); 56 | bearingVector = bearingVector/bearingVector_norm; 57 | 58 | % add noise to the bearing vectors here 59 | bearingVector_noisy = addNoise(bearingVector,focal_length,noise); 60 | 61 | % store the normalized bearing vectors along with the cameras they are 62 | % being seen (we create correspondences that always originate from the 63 | % same camera, you can change this if you want) 64 | bearingVector_norm = norm(bearingVector_noisy); 65 | 66 | v(:,i) = [bearingVector_noisy./bearingVector_norm; cam_offset]; 67 | 68 | % change the camera correspondence 69 | cam_correspondences(1,i) = cam_correspondence; 70 | cam_correspondence = cam_correspondence + 1; 71 | if cam_correspondence > cam_number 72 | cam_correspondence = 1; 73 | end 74 | end 75 | 76 | %% Add outliers 77 | number_outliers = floor(outlier_fraction*pt_number); 78 | 79 | if number_outliers > 0 80 | for i=1:number_outliers 81 | 82 | cam_correspondence = cam_correspondences(1,i); 83 | 84 | cam_offset = cam_offsets(:,cam_correspondence); 85 | %cam_rotation = cam_rotations(:,(cam_correspondence-1)*3+1:(cam_correspondence-1)*3+3); 86 | 87 | %generate random point 88 | normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1)); 89 | norm1 = sqrt(sum(normalizedPoint.*normalizedPoint)); 90 | direction = normalizedPoint./norm1; 91 | point = (maxDepth-minDepth) * normalizedPoint + minDepth * direction; 92 | 93 | body_point = rotation' * (point-position); 94 | 95 | % store the point (no need to add noise) 96 | bearingVector = body_point - cam_offset; 97 | 98 | % store the normalized bearing vectors along with the cameras they are 99 | % being seen (we create correspondences that always originate from the 100 | % same camera, you can change this if you want) 101 | bearingVector_norm = norm(bearingVector); 102 | 103 | v(:,i) = [bearingVector./bearingVector_norm; cam_offset]; 104 | end 105 | end 106 | 107 | %% copy over the position and orientation 108 | 109 | t = position; 110 | R = rotation; 111 | 112 | %% cut the cam offsets in the single camera (e.g. central case) 113 | 114 | if cam_number == 1 115 | v = v(1:3,:); 116 | end -------------------------------------------------------------------------------- /matlab/helpers/createMulti2D2DExperiment.m: -------------------------------------------------------------------------------- 1 | function [v1, v2, cam_offsets, t, R ] = createMulti2D2DExperiment( pt_per_cam, cam_number, noise, outlier_fraction ) 2 | 3 | %% generate the camera system 4 | 5 | avg_cam_distance = 0.5; 6 | cam_offsets = zeros(3,cam_number); 7 | %cam_rotations = zeros(3,cam_number*3); 8 | 9 | if cam_number == 1 10 | cam_offsets = zeros(3,1); 11 | %cam_rotations = eye(3); 12 | else 13 | for i=1:cam_number 14 | cam_offsets(:,i) = avg_cam_distance * generateRandomR() * [1.0; 0.0; 0.0]; 15 | %cam_rotations(:,(i-1)*3+1:(i-1)*3+3) = generateRandomR(); 16 | end 17 | end 18 | 19 | %% generate random view-points 20 | 21 | max_parallax = 2.0; 22 | max_rotation = 0.5; 23 | 24 | position1 = zeros(3,1); 25 | rotation1 = eye(3); 26 | 27 | position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1)); 28 | rotation2 = generateBoundedR(max_rotation); 29 | 30 | %% Generate random point-clouds 31 | 32 | minDepth = 4.0; 33 | maxDepth = 8.0; 34 | 35 | p = cell([cam_number 1]); 36 | 37 | for cam=1:cam_number 38 | normalizedPoints = 2.0*(rand(3,pt_per_cam)-repmat(0.5,3,pt_per_cam)); 39 | norms = sqrt(sum(normalizedPoints.*normalizedPoints)); 40 | directions = normalizedPoints./repmat(norms,3,1); 41 | p{cam,1} = (maxDepth-minDepth) * normalizedPoints + minDepth * directions; 42 | end 43 | 44 | %% Now create the correspondences by looping through the cameras 45 | 46 | focal_length = 800.0; 47 | v1 = cell([cam_number 1]); 48 | v2 = cell([cam_number 1]); 49 | 50 | for cam=1:cam_number 51 | 52 | v1{cam,1} = zeros(3,pt_per_cam); 53 | v2{cam,1} = zeros(3,pt_per_cam); 54 | 55 | for i=1:pt_per_cam 56 | 57 | cam_offset = cam_offsets(:,cam); 58 | %cam_rotation = cam_rotations(:,(cam-1)*3+1:(cam-1)*3+3); 59 | 60 | body_point1 = rotation1' * (p{cam,1}(:,i)-position1); 61 | body_point2 = rotation2' * (p{cam,1}(:,i)-position2); 62 | 63 | % we actually omit the cam rotation here by unrotating the bearing 64 | % vectors already 65 | bearingVector1 = body_point1 - cam_offset; 66 | bearingVector2 = body_point2 - cam_offset; 67 | bearingVector1_norm = norm(bearingVector1); 68 | bearingVector2_norm = norm(bearingVector2); 69 | bearingVector1 = bearingVector1/bearingVector1_norm; 70 | bearingVector2 = bearingVector2/bearingVector2_norm; 71 | 72 | % add noise to the bearing vectors here 73 | bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise); 74 | bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise); 75 | 76 | % store the normalized bearing vectors along with the cameras they are 77 | % being seen (we create correspondences that always originate from the 78 | % same camera, you should not change this in this experiment!) 79 | bearingVector1_norm = norm(bearingVector1_noisy); 80 | bearingVector2_norm = norm(bearingVector2_noisy); 81 | 82 | v1{cam,1}(:,i) = bearingVector1_noisy./bearingVector1_norm; 83 | v2{cam,1}(:,i) = bearingVector2_noisy./bearingVector2_norm; 84 | end 85 | end 86 | 87 | %% Add outliers 88 | outliers_per_cam = floor(outlier_fraction*pt_per_cam); 89 | 90 | if outliers_per_cam > 0 91 | for cam=1:cam_number 92 | 93 | for i=1:outliers_per_cam 94 | 95 | cam_offset = cam_offsets(:,cam); 96 | %cam_rotation = cam_rotations(:,(cam-1)*3+1:(cam-1)*3+3); 97 | 98 | %generate random point 99 | normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1)); 100 | norm1 = sqrt(sum(normalizedPoint.*normalizedPoint)); 101 | direction = normalizedPoint./norm1; 102 | point = (maxDepth-minDepth) * normalizedPoint + minDepth * direction; 103 | 104 | body_point2 = rotation2' * (point-position2); 105 | 106 | % store the point (no need to add noise) 107 | bearingVector2 = body_point2 - cam_offset; 108 | bearingVector2_norm = norm(bearingVector2); 109 | 110 | v2{cam,1}(:,i) = bearingVector2./bearingVector2_norm; 111 | end 112 | 113 | end 114 | end 115 | 116 | %% compute relative translation and rotation 117 | 118 | R = rotation1' * rotation2; 119 | t = rotation1' * (position2 - position1); -------------------------------------------------------------------------------- /matlab/helpers/createMulti2D2DOmniExperiment.m: -------------------------------------------------------------------------------- 1 | function [v1, v2, cam_offsets, t, R ] = createMulti2D2DOmniExperiment( pt_per_cam, cam_number, noise, outlier_fraction ) 2 | 3 | %% generate the camera system 4 | 5 | cam_distance = 1.0; 6 | 7 | %% set a regular camera system with 2 or 4 cameras here 8 | if cam_number == 2 9 | cam_offsets = [ cam_distance -cam_distance; 0.0 0.0; 0.0 0.0 ]; 10 | else 11 | cam_number = 4; % only two or 4 supported for this experiment 12 | cam_offsets = [ cam_distance 0.0 -cam_distance 0.0; 0.0 cam_distance 0.0 -cam_distance; 0.0 0.0 0.0 0.0 ]; 13 | end 14 | 15 | %% generate random view-points 16 | 17 | max_parallax = 2.0; 18 | max_rotation = 0.5; 19 | 20 | position1 = zeros(3,1); 21 | rotation1 = eye(3); 22 | 23 | position2 = max_parallax * 2.0 * (rand(3,1) - repmat(0.5,3,1)); 24 | rotation2 = generateBoundedR(max_rotation); 25 | 26 | %% Generate random point-cloud 27 | 28 | avg_depth_over_cam_distance = 10.0; 29 | maxDepth = 5.0; 30 | 31 | p = cell([cam_number 1]); 32 | 33 | for cam=1:cam_number 34 | 35 | normalizedPoints = 2.0*(rand(3,pt_per_cam)-repmat(0.5,3,pt_per_cam)); 36 | p{cam,1} = maxDepth * normalizedPoints; 37 | 38 | end 39 | 40 | %% Now create the correspondences by looping through the cameras 41 | 42 | focal_length = 800.0; 43 | v1 = cell([cam_number 1]); 44 | v2 = cell([cam_number 1]); 45 | 46 | for cam=1:cam_number 47 | 48 | v1{cam,1} = zeros(3,pt_per_cam); 49 | v2{cam,1} = zeros(3,pt_per_cam); 50 | 51 | for i=1:pt_per_cam 52 | 53 | cam_offset = cam_offsets(:,cam); 54 | 55 | %special: shift the point in the first frame along current camera axis, which guarantees homogeneous distribution 56 | temp = p{cam,1}(:,i) + avg_depth_over_cam_distance * cam_offset; 57 | p{cam,1}(:,i) = temp; 58 | 59 | body_point1 = rotation1' * (p{cam,1}(:,i)-position1); 60 | body_point2 = rotation2' * (p{cam,1}(:,i)-position2); 61 | 62 | % we actually omit the can rotation here by unrotating the bearing 63 | % vectors already 64 | bearingVector1 = body_point1 - cam_offset; 65 | bearingVector2 = body_point2 - cam_offset; 66 | bearingVector1_norm = norm(bearingVector1); 67 | bearingVector2_norm = norm(bearingVector2); 68 | bearingVector1 = bearingVector1/bearingVector1_norm; 69 | bearingVector2 = bearingVector2/bearingVector2_norm; 70 | 71 | % add noise to the bearing vectors here 72 | bearingVector1_noisy = addNoise(bearingVector1,focal_length,noise); 73 | bearingVector2_noisy = addNoise(bearingVector2,focal_length,noise); 74 | 75 | % store the normalized bearing vectors along with the cameras they are 76 | % being seen (we create correspondences that always originate from the 77 | % same camera, you can change this if you want) 78 | bearingVector1_norm = norm(bearingVector1_noisy); 79 | bearingVector2_norm = norm(bearingVector2_noisy); 80 | 81 | v1{cam,1}(:,i) = [bearingVector1_noisy./bearingVector1_norm]; 82 | v2{cam,1}(:,i) = [bearingVector2_noisy./bearingVector2_norm]; 83 | end 84 | end 85 | 86 | %% Add outliers 87 | outliers_per_cam = floor(outlier_fraction*pt_per_cam); 88 | 89 | if outliers_per_cam > 0 90 | for cam=1:cam_number 91 | 92 | for i=1:outliers_per_cam 93 | 94 | cam_offset = cam_offsets(:,cam); 95 | 96 | %generate random point 97 | normalizedPoint = 2.0*(rand(3,1)-repmat(0.5,3,1)); 98 | point = maxDepth * normalizedPoint + avg_depth_over_cam_distance * cam_offset; 99 | 100 | 101 | body_point2 = rotation2' * (point-position2); 102 | 103 | % store the point (no need to add noise) 104 | bearingVector2 = body_point2 - cam_offset; 105 | 106 | % store the normalized bearing vectors along with the cameras they are 107 | % being seen (we create correspondences that always originate from the 108 | % same camera, you can change this if you want) 109 | bearingVector2_norm = norm(bearingVector2); 110 | 111 | v2{cam,1}(:,i) = [bearingVector2./bearingVector2_norm]; 112 | end 113 | 114 | end 115 | end 116 | 117 | %% compute relative translation and rotation 118 | 119 | R = rotation1' * rotation2; 120 | t = rotation1' * (position2 - position1); -------------------------------------------------------------------------------- /matlab/helpers/evaluateRotationError.m: -------------------------------------------------------------------------------- 1 | function rotation_error = evaluateRotationError(R_gt,R) 2 | 3 | temp = size(size(R)); 4 | numberSolutions = 1; 5 | if temp(1,2) == 3 6 | temp2 = size(R); 7 | numberSolutions = temp2(1,3); 8 | end 9 | 10 | if numberSolutions == 1 11 | 12 | %rotation_error = norm(rodrigues(R_gt'*R)); 13 | rotation_error = norm( rodrigues(R_gt) - rodrigues(R) ); 14 | 15 | else 16 | 17 | rotation_errors = zeros(1,numberSolutions); 18 | 19 | index = 0; 20 | 21 | for i=1:numberSolutions 22 | 23 | %Check if there is any Nan 24 | if ~isnan(R(1,1,i)) 25 | index = index + 1; 26 | %rotation_errors(1,index) = norm(rodrigues(R_gt'*R(:,:,i))); 27 | rotation_errors(1,index) = norm( rodrigues(R_gt) - rodrigues(R(:,:,i)) ); 28 | end 29 | end 30 | 31 | %find the smallest error (we are the most "nice" to algorithms returning multiple solutions, 32 | %and do the disambiguation by hand) 33 | [~,minIndex] = min(rotation_errors(1,1:index)); 34 | rotation_error = rotation_errors(1,minIndex); 35 | 36 | end 37 | 38 | end 39 | -------------------------------------------------------------------------------- /matlab/helpers/evaluateTransformationError.m: -------------------------------------------------------------------------------- 1 | function [position_error,rotation_error] = evaluateTransformationError(T_gt,T) 2 | 3 | temp = size(size(T)); 4 | numberSolutions = 1; 5 | if temp(1,2) == 3 6 | temp2 = size(T); 7 | numberSolutions = temp2(1,3); 8 | end 9 | 10 | if numberSolutions == 1 11 | 12 | position_error = norm(T_gt(:,4)-T(:,4)); 13 | rotation_error = norm(rodrigues(T_gt(:,1:3)'*T(:,1:3))); 14 | 15 | else 16 | 17 | position_errors = zeros(1,numberSolutions); 18 | rotation_errors = zeros(1,numberSolutions); 19 | 20 | index = 0; 21 | 22 | for i=1:numberSolutions 23 | 24 | %Check if there is any Nan 25 | if ~isnan(T(1,1,i)) 26 | index = index + 1; 27 | position_errors(1,index) = norm(T_gt(:,4)-T(:,4,i)); 28 | rotation_errors(1,index) = norm(rodrigues(T_gt(:,1:3)'*T(:,1:3,i))); 29 | end 30 | end 31 | 32 | %find the smallest error (we are the most "nice" to algorithms returning multiple solutions, 33 | %and do the disambiguation by hand) 34 | [~,minIndex] = min(position_errors(1,1:index)); 35 | position_error = position_errors(1,minIndex); 36 | rotation_error = rotation_errors(1,minIndex); 37 | 38 | end 39 | 40 | end -------------------------------------------------------------------------------- /matlab/helpers/generateBoundedR.m: -------------------------------------------------------------------------------- 1 | function R = generateBoundedR( bound ) 2 | rpy = bound*2.0*(rand(3,1)-repmat(0.5,3,1)); 3 | 4 | R1 = zeros(3,3); 5 | R1(1,1) = 1.0; 6 | R1(2,2) = cos(rpy(1,1)); 7 | R1(2,3) = -sin(rpy(1,1)); 8 | R1(3,2) = -R1(2,3); 9 | R1(3,3) = R1(2,2); 10 | 11 | R2 = zeros(3,3); 12 | R2(1,1) = cos(rpy(2,1)); 13 | R2(1,3) = sin(rpy(2,1)); 14 | R2(2,2) = 1.0; 15 | R2(3,1) = -R2(1,3); 16 | R2(3,3) = R2(1,1); 17 | 18 | R3 = zeros(3,3); 19 | R3(1,1) = cos(rpy(3,1)); 20 | R3(1,2) = -sin(rpy(3,1)); 21 | R3(2,1) =-R3(1,2); 22 | R3(2,2) = R3(1,1); 23 | R3(3,3) = 1.0; 24 | 25 | R = R3 * R2 * R1; 26 | end -------------------------------------------------------------------------------- /matlab/helpers/generateRandomR.m: -------------------------------------------------------------------------------- 1 | function R = generateRandomR() 2 | rpy = pi()*2.0*(rand(3,1)-repmat(0.5,3,1)); 3 | rpy(2,1) = 0.5*rpy(2,1); 4 | 5 | R1 = zeros(3,3); 6 | R1(1,1) = 1.0; 7 | R1(2,2) = cos(rpy(1,1)); 8 | R1(2,3) = -sin(rpy(1,1)); 9 | R1(3,2) = -R1(2,3); 10 | R1(3,3) = R1(2,2); 11 | 12 | R2 = zeros(3,3); 13 | R2(1,1) = cos(rpy(2,1)); 14 | R2(1,3) = sin(rpy(2,1)); 15 | R2(2,2) = 1.0; 16 | R2(3,1) = -R2(1,3); 17 | R2(3,3) = R2(1,1); 18 | 19 | R3 = zeros(3,3); 20 | R3(1,1) = cos(rpy(3,1)); 21 | R3(1,2) = -sin(rpy(3,1)); 22 | R3(2,1) =-R3(1,2); 23 | R3(2,2) = R3(1,1); 24 | R3(3,3) = 1.0; 25 | 26 | R = R3 * R2 * R1; 27 | end -------------------------------------------------------------------------------- /matlab/helpers/perturb.m: -------------------------------------------------------------------------------- 1 | function [t_perturbed,R_perturbed] = perturb(t,R,amplitude) 2 | 3 | t_perturbed = t; 4 | r = rodrigues(R); 5 | 6 | for i=1:3 7 | t_perturbed(i,1) = t_perturbed(i,1) + (rand-0.5)*2.0*amplitude; 8 | r(i,1) = r(i,1) + (rand-0.5)*2.0*amplitude; 9 | end 10 | 11 | R_perturbed = rodrigues(r); 12 | end -------------------------------------------------------------------------------- /matlab/helpers/rot2cayley.m: -------------------------------------------------------------------------------- 1 | function v = rot2cayley(R) 2 | 3 | C1 = R-eye(3); 4 | C2 = R+eye(3); 5 | C = C1 * inv(C2); 6 | 7 | v = zeros(3,1); 8 | v(1,1) = -C(2,3); 9 | v(2,1) = C(1,3); 10 | v(3,1) = -C(1,2); 11 | 12 | end -------------------------------------------------------------------------------- /matlab/helpers/transformEssentials.m: -------------------------------------------------------------------------------- 1 | function Rs = transformEssentials(Es) 2 | 3 | temp = size(size(Es)); 4 | numberSolutions = 1; 5 | if temp(1,2) == 3 6 | temp2 = size(Es); 7 | numberSolutions = temp2(1,3); 8 | end 9 | 10 | if numberSolutions == 1 11 | 12 | Rs = zeros(3,3,2); 13 | [U,~,V] = svd(Es); 14 | W = [0 -1 0; 1 0 0; 0 0 1]; 15 | Rs(1:3,1:3) = U * W * V'; 16 | Rs(1:3,4:6) = U * W' * V'; 17 | 18 | if( det(Rs(1:3,1:3)) < 0 ) 19 | Rs(1:3,1:3) = -Rs(1:3,1:3); 20 | end 21 | if( det(Rs(1:3,4:6)) < 0 ) 22 | Rs(1:3,4:6) = -Rs(1:3,4:6); 23 | end 24 | 25 | else 26 | 27 | Rs_temp = zeros(3,3,2*numberSolutions); 28 | index = 0; 29 | 30 | for i=1:numberSolutions 31 | 32 | %Check if there is any Nan 33 | if ~isnan(Es(1,1,i)) 34 | [U,~,V] = svd(Es(:,:,i)); 35 | W = [0 -1 0; 1 0 0; 0 0 1]; 36 | index = index + 1; 37 | Rs_temp( :,:, index ) = U * W * V'; 38 | if(det(Rs_temp( :,:, index )) < 0) 39 | Rs_temp( :,:, index ) = -Rs_temp( :,:, index ); 40 | end 41 | index = index + 1; 42 | Rs_temp( :,:, index ) = U * W' * V'; 43 | if(det(Rs_temp( :,:, index )) < 0) 44 | Rs_temp( :,:, index ) = -Rs_temp( :,:, index ); 45 | end 46 | end 47 | end 48 | 49 | Rs = Rs_temp(:,:,1:index); 50 | 51 | end 52 | 53 | end 54 | -------------------------------------------------------------------------------- /matlab/make_mex.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Generate MEX files by compiling from Matlab 3 | # This scripts intends to automate the compilation process 4 | # by following the instructions provided in 5 | # http://laurentkneip.github.io/opengv/page_installation.html#sec_installation_5 6 | # We assume the installation configuration is standard 7 | # so that every dependency can be found in an automated way 8 | 9 | # We assume a launcher command is available: matlab 10 | # Add OpenGV library directory to the path 11 | export LD_LIBRARY_PATH=../build/lib:$LD_LIBRARY_PATH 12 | 13 | # Find path to Eigen library (assumes CMake has cached EIGEN_INCLUDE_DIRS) 14 | # See https://stackoverflow.com/questions/8474753/how-to-get-a-cmake-variable-from-the-command-line 15 | EigenPath=$(cmake -L ../build | grep EIGEN_INCLUDE_DIRS | cut -d "=" -f2) 16 | 17 | # Call Matlab with the compilation command 18 | matlab -nodisplay -nosplash -nodesktop \ 19 | -r "mex -I../include -I${EigenPath} -L../build/lib -lopengv opengv.cpp -cxx, mex -I../include -I${EigenPath} -L../build/lib -lopengv opengv_donotuse.cpp -cxx, exit" 20 | 21 | 22 | -------------------------------------------------------------------------------- /matlab/plot_arun_error.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % noncentral case 11 | cam_number = 4; 12 | % Getting 17 points, and testing all algorithms with the respective number of points 13 | pt_number = 8; 14 | % noise test, so no outliers 15 | outlier_fraction = 0.0; 16 | % repeat 1000 tests per noise level 17 | iterations = 10000; 18 | 19 | % The algorithms we want to test 20 | algorithms = { 'ge2' }; 21 | % This defines the number of points used for every algorithm 22 | indices = { [1:1:8] }; 23 | % The name of the algorithms in the final plots 24 | names = { 'arun (8pt)' }; 25 | 26 | % The maximum noise to analyze 27 | noise = 0.5; 28 | 29 | %% Run the benchmark 30 | 31 | %Run the experiment 32 | 33 | rotation_errors = zeros(1,iterations); 34 | 35 | counter = 0; 36 | 37 | for i=1:iterations 38 | 39 | % generate experiment 40 | [v1,v2,t,R] = create2D2DOmniExperiment(pt_number,cam_number,noise,outlier_fraction); 41 | [t_perturbed,R_perturbed] = perturb(t,R,0.01); 42 | T_perturbed = [R_perturbed,t_perturbed]; 43 | T_init = [eye(3),zeros(3,1)]; 44 | T_gt = [R,t]; 45 | 46 | Out = opengv(algorithms{1},indices{1},v1,v2,T_perturbed); 47 | 48 | rotation_error = evaluateRotationError( R, Out(1:3,1:3) ); 49 | rotation_errors(1,i) = rotation_error; 50 | 51 | 52 | counter = counter + 1; 53 | if counter == 100 54 | counter = 0; 55 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(noise level ' num2str(noise) ')']); 56 | end 57 | end 58 | 59 | %% Plot the results 60 | hist(rad2deg(rotation_errors)) 61 | xlabel('rotation error [deg]') 62 | ylabel('occurence') 63 | grid on -------------------------------------------------------------------------------- /matlab/plot_expected_iterations.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % The algorithms we want to test 11 | algorithms = [ 6; 8; 17 ]; 12 | % The name of the algorithms in the final plots 13 | names = { '6pt'; 'ge (8pt)'; '17pt'}; 14 | 15 | % The main experiment parameters 16 | min_outlier_fraction = 0.05;%0.05; 17 | max_outlier_fraction = 0.25; 18 | outlier_fraction_step = 0.025; 19 | 20 | p = 0.99; 21 | 22 | %% Run the benchmark 23 | 24 | %prepare the overall result arrays 25 | number_outlier_fraction_levels = round((max_outlier_fraction - min_outlier_fraction) / outlier_fraction_step + 1); 26 | num_algorithms = size(algorithms,1); 27 | expected_number_iterations = zeros(num_algorithms,number_outlier_fraction_levels); 28 | outlier_fraction_levels = zeros(1,number_outlier_fraction_levels); 29 | 30 | %Run the experiment 31 | for n=1:number_outlier_fraction_levels 32 | 33 | outlier_fraction = (n - 1) * outlier_fraction_step + min_outlier_fraction; 34 | outlier_fraction_levels(1,n) = outlier_fraction; 35 | display(['Analyzing outlier fraction level: ' num2str(outlier_fraction)]) 36 | 37 | %Now compute the mean and median value of the error for each algorithm 38 | for a=1:num_algorithms 39 | expected_number_iterations(a,n) = log(1-p)/log(1-(1-outlier_fraction)^(algorithms(a,1))); 40 | end 41 | 42 | end 43 | 44 | %% Plot the results 45 | 46 | figure(1) 47 | plot(outlier_fraction_levels,expected_number_iterations,'LineWidth',2) 48 | legend(names,'Location','NorthWest') 49 | xlabel('outlier fraction') 50 | ylabel('expected number iterations') 51 | grid on -------------------------------------------------------------------------------- /matlab/ransac_experiment.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % noncentral case 11 | cam_number = 4; 12 | % set maximum and minimum number of points per cam 13 | pt_number_per_cam = 50; 14 | % set maximum and minimum number of outliers 15 | min_outlier_fraction = 0.1; 16 | max_outlier_fraction = 0.2; 17 | % repeat 10000 iterations 18 | iterations = 1000; 19 | 20 | % The name of the algorithms in the final plots 21 | names = { 'Homogeneous'; 'Vanilla' }; 22 | 23 | % The noise in this experiment 24 | noise = 0.5; 25 | 26 | %% Run the benchmark 27 | 28 | %prepare the overall result arrays 29 | ransac_iterations = zeros(2,iterations); 30 | 31 | %Run the RANSAC with homogeneous sampling 32 | counter = 0; 33 | 34 | for i=1:iterations 35 | 36 | %generate random outlier fraction 37 | outlier_fraction = rand() * (max_outlier_fraction - min_outlier_fraction) + min_outlier_fraction; 38 | 39 | % generate experiment 40 | [v1,v2,cam_offsets,t,R] = createMulti2D2DExperiment(pt_number_per_cam,cam_number,noise,outlier_fraction); 41 | Out = opengv_experimental1( v1{1,1}, v1{2,1}, v1{3,1}, v1{4,1}, v2{1,1}, v2{2,1}, v2{3,1}, v2{4,1}, cam_offsets, 2 ); 42 | ransac_iterations(1,i) = Out(1,5); 43 | 44 | counter = counter + 1; 45 | if counter == 100 46 | counter = 0; 47 | display(['Homogeneous sampling: Iteration ' num2str(i) ' of ' num2str(iterations)]); 48 | end 49 | end 50 | 51 | %Run the RANSAC with vanilla sampling 52 | counter = 0; 53 | 54 | for i=1:iterations 55 | 56 | %generate random outlier fraction 57 | outlier_fraction = rand() * (max_outlier_fraction - min_outlier_fraction) + min_outlier_fraction; 58 | 59 | % generate experiment 60 | [v1,v2,t,R] = create2D2DExperiment(pt_number_per_cam*cam_number,cam_number,noise,outlier_fraction); 61 | Out = opengv_experimental2( v1, v2, 2 ); 62 | ransac_iterations(2,i) = Out(1,5); 63 | 64 | counter = counter + 1; 65 | if counter == 100 66 | counter = 0; 67 | display(['Vanilla sampling: Iteration ' num2str(i) ' of ' num2str(iterations)]); 68 | end 69 | end 70 | 71 | %% Plot the results 72 | 73 | figure(1) 74 | hist(ransac_iterations') 75 | [Y,X] = hist(ransac_iterations') 76 | legend(names,'Location','NorthEast') 77 | xlabel('number of iterations') 78 | grid on -------------------------------------------------------------------------------- /matlab/ransac_experiment2.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | rng shuffle 9 | 10 | %% Configure the benchmark 11 | 12 | % noncentral case 13 | cam_number = 4; 14 | % Getting 10 points, and testing all algorithms with the respective number of points 15 | pt_per_cam = 20; 16 | % outlier test, so constant noise 17 | noise = 0.5; 18 | % repeat 100 tests per outlier-ratio 19 | iterations = 50; 20 | 21 | % The algorithms we want to test 22 | algorithms = [ 0; 1; 2 ]; 23 | % The name of the algorithms in the final plots 24 | names = { '6pt'; 'ge (8pt)'; '17pt'}; 25 | 26 | % The main experiment parameters 27 | min_outlier_fraction = 0.05; 28 | max_outlier_fraction = 0.45; 29 | outlier_fraction_step = 0.05; 30 | 31 | %% Run the benchmark 32 | 33 | %prepare the overall result arrays 34 | number_outlier_fraction_levels = round((max_outlier_fraction - min_outlier_fraction) / outlier_fraction_step + 1); 35 | num_algorithms = size(algorithms,1); 36 | 37 | %Run the experiment 38 | for n=1:number_outlier_fraction_levels 39 | 40 | outlier_fraction = (n - 1) * outlier_fraction_step + min_outlier_fraction; 41 | display(['Analyzing outlier fraction level: ' num2str(outlier_fraction)]) 42 | 43 | clear number_iterations 44 | clear execution_times 45 | 46 | counter = 0; 47 | 48 | temp_file_name1 = ['number_iterations_' num2str(outlier_fraction) '.mat']; 49 | temp_file_name2 = ['execution_times_' num2str(outlier_fraction) '.mat']; 50 | 51 | if exist(temp_file_name1,'file') > 0 52 | display(['number_iterations_' num2str(outlier_fraction) '.mat exists already']) 53 | load(temp_file_name1) 54 | load(temp_file_name2) 55 | startingIteration = size(number_iterations,2) + 1; 56 | display(['starting at ' num2str(startingIteration)]) 57 | else 58 | startingIteration = 1; 59 | end 60 | 61 | if startingIteration <= iterations 62 | for i=startingIteration:iterations 63 | 64 | % generate experiment 65 | [v1,v2,cam_offsets,t,R] = createMulti2D2DOmniExperiment(pt_per_cam,cam_number,noise,outlier_fraction); 66 | 67 | for a=1:num_algorithms 68 | 69 | if strcmp(names{a,1},'6pt') && outlier_fraction > 0.25 70 | Out = zeros(4,5); 71 | time = 10000000.0; 72 | else 73 | tic 74 | Out = opengv_experimental1( v1{1,1}, v1{2,1}, v1{3,1}, v1{4,1}, v2{1,1}, v2{2,1}, v2{3,1}, v2{4,1}, cam_offsets, algorithms(a,1) ); 75 | time = toc; 76 | end 77 | 78 | number_iterations(a,i) = Out(1,5); 79 | execution_times(a,i) = time; 80 | end 81 | 82 | save(temp_file_name1,'number_iterations'); 83 | save(temp_file_name2,'execution_times'); 84 | 85 | counter = counter + 1; 86 | if counter == 1 87 | counter = 0; 88 | display(['Iteration ' num2str(i) ' of ' num2str(iterations) '(outlier_fraction level ' num2str(outlier_fraction) ')']); 89 | end 90 | end 91 | end 92 | end -------------------------------------------------------------------------------- /matlab/ransac_experiment3.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | %% Configure the benchmark 9 | 10 | % The name of the algorithms in the final plots 11 | names = { '6pt'; 'ge (8pt)'; '17pt'}; 12 | 13 | % The main experiment parameters 14 | min_outlier_fraction = 0.05; 15 | max_outlier_fraction = 0.45; 16 | outlier_fraction_step = 0.05; 17 | 18 | %% Run the benchmark 19 | 20 | %prepare the overall result arrays 21 | number_outlier_fraction_levels = round((max_outlier_fraction - min_outlier_fraction) / outlier_fraction_step + 1); 22 | num_algorithms = size(names,1); 23 | mean_number_iterations = zeros(num_algorithms,number_outlier_fraction_levels); 24 | mean_execution_times = zeros(num_algorithms,number_outlier_fraction_levels); 25 | outlier_fraction_levels = zeros(1,number_outlier_fraction_levels); 26 | 27 | %Run the experiment 28 | for n=1:number_outlier_fraction_levels 29 | 30 | outlier_fraction = (n - 1) * outlier_fraction_step + min_outlier_fraction; 31 | outlier_fraction_levels(1,n) = outlier_fraction; 32 | display(['Analyzing outlier fraction level: ' num2str(outlier_fraction)]) 33 | 34 | clear number_iterations 35 | clear execution_times 36 | temp_file_name1 = ['number_iterations_' num2str(outlier_fraction) '.mat']; 37 | temp_file_name2 = ['execution_times_' num2str(outlier_fraction) '.mat']; 38 | load(temp_file_name1) 39 | load(temp_file_name2) 40 | 41 | %Now compute the mean and median value of the error for each algorithm 42 | for a=1:num_algorithms 43 | mean_number_iterations(a,n) = mean(number_iterations(a,:)); 44 | mean_execution_times(a,n) = mean(execution_times(a,:)); 45 | end 46 | end 47 | 48 | %% Plot the results 49 | 50 | figure(1) 51 | plot(outlier_fraction_levels,mean_number_iterations,'LineWidth',2) 52 | legend(names,'Location','NorthWest') 53 | xlabel('outlier fraction') 54 | ylabel('mean number iterations') 55 | axis([0.05 0.25 0 1500]) 56 | grid on 57 | 58 | figure(2) 59 | hold on 60 | plot(outlier_fraction_levels(1,1:6),mean_execution_times(1,1:6),'LineWidth',2) 61 | plot(outlier_fraction_levels,mean_execution_times(2:3,:),'LineWidth',2) 62 | legend(names,'Location','NorthWest') 63 | xlabel('outlier fraction') 64 | ylabel('mean execution time [s]') 65 | axis([0.05 0.45 0 40]) 66 | grid on -------------------------------------------------------------------------------- /matlab/ransac_test.m: -------------------------------------------------------------------------------- 1 | %% Reset everything 2 | 3 | clear all; 4 | clc; 5 | close all; 6 | addpath('helpers'); 7 | 8 | % central case -> only one camera 9 | cam_number = 1; 10 | % let's only get 6 points, and generate new ones in each iteration 11 | pt_number = 100; 12 | % noise test, so no outliers 13 | outlier_fraction = 0.1; 14 | 15 | noise = 0.0; 16 | [points,v,t,R] = create2D3DExperiment(pt_number,cam_number,noise,outlier_fraction); 17 | [X, inliers] = opengv('p3p_kneip_ransac',points,v); -------------------------------------------------------------------------------- /modules/Config.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include("${CMAKE_CURRENT_LIST_DIR}/@targets_export_name@.cmake") 4 | check_required_components("@PROJECT_NAME@") 5 | -------------------------------------------------------------------------------- /modules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_subdirectory(pybind11) 3 | 4 | include_directories(${PYTHON_INCLUDE_DIRS}) 5 | 6 | 7 | pybind11_add_module(pyopengv pyopengv.cpp) 8 | target_link_libraries(pyopengv PRIVATE opengv) 9 | 10 | 11 | # Find where to install python libs. 12 | execute_process(COMMAND 13 | ${PYTHON_EXECUTABLE} -c "import distutils.sysconfig; print('/'.join(distutils.sysconfig.get_python_lib().split('/')[-3:]))" 14 | OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) 15 | 16 | set(PYTHON_INSTALL_DIR 17 | "${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}" 18 | CACHE PATH "Path where to install pyopengv") 19 | 20 | install(TARGETS pyopengv DESTINATION "${PYTHON_INSTALL_DIR}") 21 | 22 | message(python executable ${PYTHON_EXECUTABLE}) -------------------------------------------------------------------------------- /python/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __TYPES_H__ 2 | #define __TYPES_H__ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | 11 | namespace pyopengv { 12 | 13 | namespace py = pybind11; 14 | 15 | typedef py::array_t pyarray_d; 16 | 17 | template 18 | py::array_t py_array_from_data(const T *data, size_t shape0) { 19 | py::array_t res({shape0}); 20 | std::copy(data, data + shape0, res.mutable_data()); 21 | return res; 22 | } 23 | 24 | template 25 | py::array_t py_array_from_data(const T *data, size_t shape0, size_t shape1) { 26 | py::array_t res({shape0, shape1}); 27 | std::copy(data, data + shape0 * shape1, res.mutable_data()); 28 | return res; 29 | } 30 | 31 | template 32 | py::array_t py_array_from_vector(const std::vector &v) { 33 | const T *data = v.size() ? &v[0] : NULL; 34 | return py_array_from_data(data, v.size()); 35 | } 36 | 37 | 38 | } 39 | 40 | #endif // __TYPES_H__ 41 | -------------------------------------------------------------------------------- /src/absolute_pose/modules/gpnp1/code.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | 35 | void 36 | opengv::absolute_pose::modules::gpnp1::compute( 37 | Eigen::Matrix & groebnerMatrix ) 38 | { 39 | sPolynomial3(groebnerMatrix); 40 | 41 | sPolynomial4(groebnerMatrix); 42 | groebnerRow3_0_f(groebnerMatrix,4); 43 | 44 | } 45 | -------------------------------------------------------------------------------- /src/absolute_pose/modules/gpnp1/reductors.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | 35 | void 36 | opengv::absolute_pose::modules::gpnp1::groebnerRow3_0_f( Eigen::Matrix & groebnerMatrix, int targetRow ) 37 | { 38 | double factor = groebnerMatrix(targetRow,1) / groebnerMatrix(3,1); 39 | groebnerMatrix(targetRow,1) = 0.0; 40 | groebnerMatrix(targetRow,2) -= factor * groebnerMatrix(3,2); 41 | } 42 | 43 | -------------------------------------------------------------------------------- /src/absolute_pose/modules/gpnp1/spolynomials.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | 35 | void 36 | opengv::absolute_pose::modules::gpnp1::sPolynomial3( Eigen::Matrix & groebnerMatrix ) 37 | { 38 | groebnerMatrix(3,1) = (groebnerMatrix(0,1)/(groebnerMatrix(0,0))-groebnerMatrix(1,1)/(groebnerMatrix(1,0))); 39 | groebnerMatrix(3,2) = (groebnerMatrix(0,2)/(groebnerMatrix(0,0))-groebnerMatrix(1,2)/(groebnerMatrix(1,0))); 40 | } 41 | 42 | void 43 | opengv::absolute_pose::modules::gpnp1::sPolynomial4( Eigen::Matrix & groebnerMatrix ) 44 | { 45 | groebnerMatrix(4,1) = (groebnerMatrix(1,1)/(groebnerMatrix(1,0))-groebnerMatrix(2,1)/(groebnerMatrix(2,0))); 46 | groebnerMatrix(4,2) = (groebnerMatrix(1,2)/(groebnerMatrix(1,0))-groebnerMatrix(2,2)/(groebnerMatrix(2,0))); 47 | } 48 | 49 | -------------------------------------------------------------------------------- /src/absolute_pose/modules/gpnp2/code.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | 35 | void 36 | opengv::absolute_pose::modules::gpnp2::compute( 37 | Eigen::Matrix & groebnerMatrix ) 38 | { 39 | sPolynomial5(groebnerMatrix); 40 | 41 | sPolynomial6(groebnerMatrix); 42 | groebnerRow5_00_f(groebnerMatrix,6); 43 | 44 | sPolynomial7(groebnerMatrix); 45 | groebnerRow5_00_f(groebnerMatrix,7); 46 | groebnerRow6_00_f(groebnerMatrix,7); 47 | 48 | sPolynomial8(groebnerMatrix); 49 | groebnerRow7_10_f(groebnerMatrix,8); 50 | groebnerRow6_00_f(groebnerMatrix,8); 51 | groebnerRow7_00_f(groebnerMatrix,8); 52 | 53 | sPolynomial9(groebnerMatrix); 54 | groebnerRow7_00_f(groebnerMatrix,9); 55 | groebnerRow8_00_f(groebnerMatrix,9); 56 | 57 | } 58 | -------------------------------------------------------------------------------- /src/absolute_pose/modules/gpnp2/reductors.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | 35 | void 36 | opengv::absolute_pose::modules::gpnp2::groebnerRow5_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ) 37 | { 38 | double factor = groebnerMatrix(targetRow,1) / groebnerMatrix(5,1); 39 | groebnerMatrix(targetRow,1) = 0.0; 40 | groebnerMatrix(targetRow,2) -= factor * groebnerMatrix(5,2); 41 | groebnerMatrix(targetRow,3) -= factor * groebnerMatrix(5,3); 42 | groebnerMatrix(targetRow,4) -= factor * groebnerMatrix(5,4); 43 | groebnerMatrix(targetRow,5) -= factor * groebnerMatrix(5,5); 44 | } 45 | 46 | void 47 | opengv::absolute_pose::modules::gpnp2::groebnerRow6_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ) 48 | { 49 | double factor = groebnerMatrix(targetRow,2) / groebnerMatrix(6,2); 50 | groebnerMatrix(targetRow,2) = 0.0; 51 | groebnerMatrix(targetRow,3) -= factor * groebnerMatrix(6,3); 52 | groebnerMatrix(targetRow,4) -= factor * groebnerMatrix(6,4); 53 | groebnerMatrix(targetRow,5) -= factor * groebnerMatrix(6,5); 54 | } 55 | 56 | void 57 | opengv::absolute_pose::modules::gpnp2::groebnerRow7_10_f( Eigen::Matrix & groebnerMatrix, int targetRow ) 58 | { 59 | double factor = groebnerMatrix(targetRow,1) / groebnerMatrix(7,3); 60 | groebnerMatrix(targetRow,1) = 0.0; 61 | groebnerMatrix(targetRow,2) -= factor * groebnerMatrix(7,4); 62 | groebnerMatrix(targetRow,4) -= factor * groebnerMatrix(7,5); 63 | } 64 | 65 | void 66 | opengv::absolute_pose::modules::gpnp2::groebnerRow7_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ) 67 | { 68 | double factor = groebnerMatrix(targetRow,3) / groebnerMatrix(7,3); 69 | groebnerMatrix(targetRow,3) = 0.0; 70 | groebnerMatrix(targetRow,4) -= factor * groebnerMatrix(7,4); 71 | groebnerMatrix(targetRow,5) -= factor * groebnerMatrix(7,5); 72 | } 73 | 74 | void 75 | opengv::absolute_pose::modules::gpnp2::groebnerRow8_00_f( Eigen::Matrix & groebnerMatrix, int targetRow ) 76 | { 77 | double factor = groebnerMatrix(targetRow,4) / groebnerMatrix(8,4); 78 | groebnerMatrix(targetRow,4) = 0.0; 79 | groebnerMatrix(targetRow,5) -= factor * groebnerMatrix(8,5); 80 | } 81 | 82 | -------------------------------------------------------------------------------- /src/math/arun.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | opengv::rotation_t 35 | opengv::math::arun( const Eigen::MatrixXd & Hcross ) 36 | { 37 | //decompose matrix H to obtain rotation 38 | Eigen::JacobiSVD< Eigen::MatrixXd > SVDcross( 39 | Hcross, 40 | Eigen::ComputeFullU | Eigen::ComputeFullV ); 41 | 42 | Eigen::Matrix3d V = SVDcross.matrixV(); 43 | Eigen::Matrix3d U = SVDcross.matrixU(); 44 | rotation_t R = V * U.transpose(); 45 | 46 | //modify the result in case the rotation has determinant=-1 47 | if( R.determinant() < 0 ) 48 | { 49 | Eigen::Matrix3d V_prime; 50 | V_prime.col(0) = V.col(0); 51 | V_prime.col(1) = V.col(1); 52 | V_prime.col(2) = -V.col(2); 53 | R = V_prime * U.transpose(); 54 | } 55 | 56 | return R; 57 | } 58 | 59 | opengv::transformation_t 60 | opengv::math::arun_complete( 61 | const points_t & p1, 62 | const points_t & p2 ) 63 | { 64 | assert(p1.size() == p2.size()); 65 | 66 | //derive the centroid of the two point-clouds 67 | point_t pointsCenter1 = Eigen::Vector3d::Zero(); 68 | point_t pointsCenter2 = Eigen::Vector3d::Zero(); 69 | 70 | for( size_t i = 0; i < p1.size(); i++ ) 71 | { 72 | pointsCenter1 += p1[i]; 73 | pointsCenter2 += p2[i]; 74 | } 75 | 76 | pointsCenter1 = pointsCenter1 / p1.size(); 77 | pointsCenter2 = pointsCenter2 / p2.size(); 78 | 79 | //compute the matrix H = sum(f'*f^{T}) 80 | Eigen::MatrixXd Hcross(3,3); 81 | Hcross = Eigen::Matrix3d::Zero(); 82 | 83 | for( size_t i = 0; i < p1.size(); i++ ) 84 | { 85 | Eigen::Vector3d f = p1[i] - pointsCenter1; 86 | Eigen::Vector3d fprime = p2[i] - pointsCenter2; 87 | Hcross += fprime * f.transpose(); 88 | } 89 | 90 | //decompose this matrix (SVD) to obtain rotation 91 | rotation_t rotation = arun(Hcross); 92 | translation_t translation = pointsCenter1 - rotation*pointsCenter2; 93 | transformation_t solution; 94 | solution.block<3,3>(0,0) = rotation; 95 | solution.col(3) = translation; 96 | 97 | return solution; 98 | } 99 | -------------------------------------------------------------------------------- /src/math/cayley.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | opengv::rotation_t 35 | opengv::math::cayley2rot( const cayley_t & cayley) 36 | { 37 | rotation_t R; 38 | double scale = 1+pow(cayley[0],2)+pow(cayley[1],2)+pow(cayley[2],2); 39 | 40 | R(0,0) = 1+pow(cayley[0],2)-pow(cayley[1],2)-pow(cayley[2],2); 41 | R(0,1) = 2*(cayley[0]*cayley[1]-cayley[2]); 42 | R(0,2) = 2*(cayley[0]*cayley[2]+cayley[1]); 43 | R(1,0) = 2*(cayley[0]*cayley[1]+cayley[2]); 44 | R(1,1) = 1-pow(cayley[0],2)+pow(cayley[1],2)-pow(cayley[2],2); 45 | R(1,2) = 2*(cayley[1]*cayley[2]-cayley[0]); 46 | R(2,0) = 2*(cayley[0]*cayley[2]-cayley[1]); 47 | R(2,1) = 2*(cayley[1]*cayley[2]+cayley[0]); 48 | R(2,2) = 1-pow(cayley[0],2)-pow(cayley[1],2)+pow(cayley[2],2); 49 | 50 | R = (1/scale) * R; 51 | return R; 52 | } 53 | 54 | opengv::rotation_t 55 | opengv::math::cayley2rot_reduced( const cayley_t & cayley) 56 | { 57 | rotation_t R; 58 | 59 | R(0,0) = 1+pow(cayley[0],2)-pow(cayley[1],2)-pow(cayley[2],2); 60 | R(0,1) = 2*(cayley[0]*cayley[1]-cayley[2]); 61 | R(0,2) = 2*(cayley[0]*cayley[2]+cayley[1]); 62 | R(1,0) = 2*(cayley[0]*cayley[1]+cayley[2]); 63 | R(1,1) = 1-pow(cayley[0],2)+pow(cayley[1],2)-pow(cayley[2],2); 64 | R(1,2) = 2*(cayley[1]*cayley[2]-cayley[0]); 65 | R(2,0) = 2*(cayley[0]*cayley[2]-cayley[1]); 66 | R(2,1) = 2*(cayley[1]*cayley[2]+cayley[0]); 67 | R(2,2) = 1-pow(cayley[0],2)-pow(cayley[1],2)+pow(cayley[2],2); 68 | 69 | return R; 70 | } 71 | 72 | opengv::cayley_t 73 | opengv::math::rot2cayley( const rotation_t & R ) 74 | { 75 | Eigen::Matrix3d C1; 76 | Eigen::Matrix3d C2; 77 | Eigen::Matrix3d C; 78 | C1 = R-Eigen::Matrix3d::Identity(); 79 | C2 = R+Eigen::Matrix3d::Identity(); 80 | C = C1 * C2.inverse(); 81 | 82 | cayley_t cayley; 83 | cayley[0] = -C(1,2); 84 | cayley[1] = C(0,2); 85 | cayley[2] = -C(0,1); 86 | 87 | return cayley; 88 | } 89 | -------------------------------------------------------------------------------- /src/point_cloud/MAPointCloud.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | opengv::point_cloud::MAPointCloud::MAPointCloud( 35 | const double * points1, 36 | const double * points2, 37 | int numberPoints1, 38 | int numberPoints2 ) : 39 | PointCloudAdapterBase(), 40 | _points1(points1), 41 | _points2(points2), 42 | _numberPoints1(numberPoints1), 43 | _numberPoints2(numberPoints2) 44 | {} 45 | 46 | opengv::point_cloud::MAPointCloud::~MAPointCloud() 47 | {} 48 | 49 | opengv::point_t 50 | opengv::point_cloud::MAPointCloud:: 51 | getPoint1( size_t index ) const 52 | { 53 | point_t point; 54 | assert( index < _numberPoints1 ); 55 | point[0] = _points1[ 3 * index]; 56 | point[1] = _points1[ 3 * index + 1]; 57 | point[2] = _points1[ 3 * index + 2]; 58 | return point; 59 | } 60 | 61 | opengv::point_t 62 | opengv::point_cloud::MAPointCloud:: 63 | getPoint2( size_t index ) const 64 | { 65 | assert(index < _numberPoints2); 66 | point_t point; 67 | point[0] = _points2[ index * 3 ]; 68 | point[1] = _points2[ index * 3 + 1 ]; 69 | point[2] = _points2[ index * 3 + 2 ]; 70 | return point; 71 | } 72 | 73 | double 74 | opengv::point_cloud::MAPointCloud:: 75 | getWeight( size_t index ) const 76 | { 77 | return 1.0; 78 | } 79 | 80 | size_t 81 | opengv::point_cloud::MAPointCloud:: 82 | getNumberCorrespondences() const 83 | { 84 | return _numberPoints2; 85 | } 86 | -------------------------------------------------------------------------------- /src/point_cloud/PointCloudAdapter.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | 34 | opengv::point_cloud::PointCloudAdapter::PointCloudAdapter( 35 | const points_t & points1, 36 | const points_t & points2 ) : 37 | PointCloudAdapterBase(), 38 | _points1(points1), 39 | _points2(points2) 40 | {} 41 | 42 | opengv::point_cloud::PointCloudAdapter::PointCloudAdapter( 43 | const points_t & points1, 44 | const points_t & points2, 45 | const rotation_t & R12 ) : 46 | PointCloudAdapterBase(R12), 47 | _points1(points1), 48 | _points2(points2) 49 | {} 50 | 51 | opengv::point_cloud::PointCloudAdapter::PointCloudAdapter( 52 | const points_t & points1, 53 | const points_t & points2, 54 | const translation_t & t12, 55 | const rotation_t & R12 ) : 56 | PointCloudAdapterBase(t12,R12), 57 | _points1(points1), 58 | _points2(points2) 59 | {} 60 | 61 | opengv::point_cloud::PointCloudAdapter::~PointCloudAdapter() 62 | {} 63 | 64 | opengv::point_t 65 | opengv::point_cloud::PointCloudAdapter:: 66 | getPoint1( size_t index ) const 67 | { 68 | assert(index < _points1.size()); 69 | return _points1[index]; 70 | } 71 | 72 | opengv::point_t 73 | opengv::point_cloud::PointCloudAdapter:: 74 | getPoint2( size_t index ) const 75 | { 76 | assert(index < _points2.size()); 77 | return _points2[index]; 78 | } 79 | 80 | double 81 | opengv::point_cloud::PointCloudAdapter:: 82 | getWeight( size_t index ) const 83 | { 84 | return 1.0; 85 | } 86 | 87 | size_t 88 | opengv::point_cloud::PointCloudAdapter:: 89 | getNumberCorrespondences() const 90 | { 91 | return _points2.size(); 92 | } 93 | -------------------------------------------------------------------------------- /src/sac_problems/point_cloud/PointCloudSacProblem.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | #include 34 | 35 | bool 36 | opengv::sac_problems:: 37 | point_cloud::PointCloudSacProblem::computeModelCoefficients( 38 | const std::vector &indices, 39 | model_t & outModel) const 40 | { 41 | outModel = opengv::point_cloud::threept_arun(_adapter,indices); 42 | return true; 43 | } 44 | 45 | void 46 | opengv::sac_problems:: 47 | point_cloud::PointCloudSacProblem::getSelectedDistancesToModel( 48 | const model_t & model, 49 | const std::vector & indices, 50 | std::vector & scores) const 51 | { 52 | Eigen::Matrix p_hom; 53 | p_hom[3] = 1.0; 54 | 55 | for( size_t i = 0; i < indices.size(); i++ ) 56 | { 57 | p_hom.block<3,1>(0,0) = _adapter.getPoint2(indices[i]); 58 | point_t transformedPoint = model * p_hom; 59 | translation_t error = _adapter.getPoint1(indices[i]) - transformedPoint; 60 | scores.push_back(error.norm()); 61 | } 62 | } 63 | 64 | void 65 | opengv::sac_problems:: 66 | point_cloud::PointCloudSacProblem::optimizeModelCoefficients( 67 | const std::vector & inliers, 68 | const model_t & model, 69 | model_t & optimized_model) 70 | { 71 | optimized_model = opengv::point_cloud::threept_arun(_adapter,inliers); 72 | } 73 | 74 | int 75 | opengv::sac_problems:: 76 | point_cloud::PointCloudSacProblem::getSampleSize() const 77 | { 78 | int sampleSize = 3; 79 | return sampleSize; 80 | } 81 | -------------------------------------------------------------------------------- /src/sac_problems/relative_pose/RotationOnlySacProblem.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | bool 39 | opengv::sac_problems:: 40 | relative_pose::RotationOnlySacProblem::computeModelCoefficients( 41 | const std::vector &indices, 42 | model_t & outModel) const 43 | { 44 | outModel = opengv::relative_pose::twopt_rotationOnly(_adapter,indices); 45 | 46 | return true; 47 | } 48 | 49 | void 50 | opengv::sac_problems:: 51 | relative_pose::RotationOnlySacProblem::getSelectedDistancesToModel( 52 | const model_t & model, 53 | const std::vector & indices, 54 | std::vector & scores) const 55 | { 56 | for( size_t i = 0; i < indices.size(); i++ ) 57 | { 58 | bearingVector_t f1 = _adapter.getBearingVector1(indices[i]); 59 | bearingVector_t f2 = _adapter.getBearingVector2(indices[i]); 60 | 61 | //unrotate bearing-vector f2 62 | bearingVector_t f2_unrotated = model * f2; 63 | 64 | //bearing-vector based outlier criterium (select threshold accordingly): 65 | //1-(f1'*f2) = 1-cos(alpha) \in [0:2] 66 | double error = 1.0 - (f1.transpose() * f2_unrotated); 67 | scores.push_back(error); 68 | } 69 | } 70 | 71 | void 72 | opengv::sac_problems:: 73 | relative_pose::RotationOnlySacProblem::optimizeModelCoefficients( 74 | const std::vector & inliers, 75 | const model_t & model, 76 | model_t & optimized_model) 77 | { 78 | optimized_model = opengv::relative_pose::rotationOnly(_adapter,inliers); 79 | } 80 | 81 | int 82 | opengv::sac_problems:: 83 | relative_pose::RotationOnlySacProblem::getSampleSize() const 84 | { 85 | int sampleSize = 2; 86 | return sampleSize; 87 | } 88 | -------------------------------------------------------------------------------- /test/random_generators.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | #ifndef OPENGV_RANDOM_GENERATORS_HPP_ 32 | #define OPENGV_RANDOM_GENERATORS_HPP_ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | namespace opengv 39 | { 40 | 41 | void initializeRandomSeed(); 42 | Eigen::Vector3d generateRandomPoint( double maximumDepth, double minimumDepth ); 43 | Eigen::Vector3d generateRandomPointPlane(); 44 | Eigen::Vector3d addNoise( double noiseLevel, Eigen::Vector3d cleanPoint ); 45 | Eigen::Vector3d generateRandomTranslation( double maximumParallax ); 46 | Eigen::Vector3d generateRandomDirectionTranslation( double parallax ); 47 | Eigen::Matrix3d generateRandomRotation( double maxAngle ); 48 | Eigen::Matrix3d generateRandomRotation(); 49 | 50 | } 51 | 52 | #endif /* OPENGV_RANDOM_GENERATORS_HPP_ */ 53 | -------------------------------------------------------------------------------- /test/time_measurement.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | #include "time_measurement.hpp" 32 | 33 | #ifdef WIN32 34 | 35 | #include 36 | 37 | void 38 | gettimeofday( timeval * timeofday, int dummy) 39 | { 40 | struct timeb time; 41 | ftime(&time); 42 | timeofday->tv_sec = (int) time.time; 43 | timeofday->tv_usec = 1000 * (int) time.millitm; 44 | } 45 | 46 | #endif 47 | 48 | timeval 49 | timeval_minus( const struct timeval &t1, const struct timeval &t2 ) 50 | { 51 | timeval ret; 52 | ret.tv_sec = t1.tv_sec - t2.tv_sec; 53 | if( t1.tv_usec < t2.tv_usec ) 54 | { 55 | ret.tv_sec--; 56 | ret.tv_usec = t1.tv_usec - t2.tv_usec + 1000000; 57 | } 58 | else 59 | ret.tv_usec = t1.tv_usec - t2.tv_usec; 60 | 61 | return ret; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /test/time_measurement.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. 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 * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * 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 ANU OR THE 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 * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | #ifndef OPENGV_TIME_MEASUREMENT_HPP_ 32 | #define OPENGV_TIME_MEASUREMENT_HPP_ 33 | 34 | #include 35 | #include 36 | 37 | #ifdef WIN32 38 | struct timeval { 39 | int tv_sec; 40 | int tv_usec; 41 | }; 42 | 43 | void gettimeofday( timeval * timeofday, int dummy); 44 | #else 45 | #include 46 | #endif 47 | 48 | #define TIMETODOUBLE(x) ( x.tv_sec + x.tv_usec * 1e-6 ) 49 | 50 | timeval 51 | timeval_minus( const struct timeval &t1, const struct timeval &t2 ); 52 | 53 | #endif /* OPENGV_TIME_MEASUREMENT_HPP_ */ 54 | --------------------------------------------------------------------------------