├── .idea ├── .name ├── MRO.iml ├── misc.xml ├── modules.xml ├── vcs.xml └── workspace.xml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── data └── config.yaml ├── demo.sh ├── ral ├── .DS_Store ├── ._.DS_Store ├── ._CMakeLists.txt ├── ._README.md ├── ._l1_irls.cpp ├── ._l1_irls.hpp ├── ._test.cpp ├── CMakeLists.txt ├── README.md ├── anders │ ├── ._rotavg.m │ ├── ._test_avg.m │ ├── ORB_pose.mat │ ├── rotavg.m │ ├── rotdist.m │ ├── roterr.m │ └── test_avg.m ├── cmake │ ├── ._FindSuiteSparse.cmake │ ├── ._UpdateCacheVariable.cmake │ ├── FindSuiteSparse.cmake │ └── UpdateCacheVariable.cmake ├── data │ ├── ._ravg_input.txt │ └── ravg_input.txt ├── l1_irls.cpp ├── l1_irls.hpp ├── rotavg_anders.zip └── test.cpp ├── src ├── CMakeLists.txt ├── Camera.cpp ├── Camera.hpp ├── Converter.cpp ├── Converter.hpp ├── Frame.cpp ├── Frame.hpp ├── IRotAvg.cpp ├── ORBExtractor.cpp ├── ORBExtractor.hpp ├── ORBVocabulary.cpp ├── ORBVocabulary.hpp ├── Pose.cpp ├── Pose.hpp ├── SequenceLoader.cpp ├── SequenceLoader.hpp ├── View.cpp ├── View.hpp ├── ViewDatabase.cpp ├── ViewDatabase.hpp ├── ViewGraph.cpp └── ViewGraph.hpp └── third_party ├── ._DBoW2 ├── DBoW2 ├── ._CMakeLists.txt ├── ._DBoW2 ├── ._DUtils ├── ._LICENSE.txt ├── ._README.txt ├── CMakeLists.txt ├── DBoW2 │ ├── ._BowVector.cpp │ ├── ._BowVector.h │ ├── ._FClass.h │ ├── ._FORB.cpp │ ├── ._FORB.h │ ├── ._FeatureVector.cpp │ ├── ._FeatureVector.h │ ├── ._ScoringObject.cpp │ ├── ._ScoringObject.h │ ├── ._TemplatedVocabulary.h │ ├── BowVector.cpp │ ├── BowVector.h │ ├── FClass.h │ ├── FORB.cpp │ ├── FORB.h │ ├── FeatureVector.cpp │ ├── FeatureVector.h │ ├── ScoringObject.cpp │ ├── ScoringObject.h │ └── TemplatedVocabulary.h ├── DUtils │ ├── ._Random.cpp │ ├── ._Random.h │ ├── ._Timestamp.cpp │ ├── ._Timestamp.h │ ├── Random.cpp │ ├── Random.h │ ├── Timestamp.cpp │ └── Timestamp.h ├── LICENSE.txt └── README.txt └── opengv ├── .idea ├── misc.xml ├── modules.xml ├── opengv.iml └── workspace.xml ├── 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.mexa64 ├── 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 /.idea/.name: -------------------------------------------------------------------------------- 1 | irotavg -------------------------------------------------------------------------------- /.idea/MRO.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | cmake_minimum_required (VERSION 3.0) 4 | project (irotavg) 5 | 6 | set(default_build_type "Release") 7 | 8 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 9 | message(STATUS "Setting build type to '${default_build_type}' as none was specified.") 10 | set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE 11 | STRING "Choose the type of build." FORCE) 12 | # Set the possible values of build type for cmake-gui 13 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS 14 | "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 15 | endif() 16 | 17 | 18 | set (CMAKE_CXX_STANDARD 11) 19 | 20 | # Make CMake aware of the cmake folder for local FindXXX scripts, 21 | # append rather than set in case the user has passed their own 22 | # additional paths via -D. 23 | list(APPEND CMAKE_MODULE_PATH "${irotavg_SOURCE_DIR}/ral/cmake") 24 | include(UpdateCacheVariable) 25 | 26 | message( "irotavg_SOURCE_DIR: ${irotavg_SOURCE_DIR}" ) 27 | 28 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 29 | 30 | add_subdirectory(src) 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MRO 2 | 3 | Author: Chee-Kheng Chng 4 | 5 | Given monocular images, MRO estimates their absolute orientations. 6 | It is 7 | 1) robust against pure rotation motion (equipped with the relative rotation estimator from openGV (https://laurentkneip.github.io/opengv/)) 8 | 2) capable of loop closing 9 | 3) a constant time rotation avergaing solver (due to its incremental nature). 10 | 11 | It is an extension to IRotAvg from https://github.com/ajparra/iRotAvg. 12 | 13 | ## License 14 | 15 | MRO is released under a GPLv3 license. 16 | 17 | For a closed-source version of MRO (e.g., for commercial purposes), please contact the author. 18 | 19 | For an academic use of MRO, please cite 20 | [C.-K. Chng, Á. Parra, T.-J. Chin, Y. Latif: Monocular Rotational Odometry with Incremental Rotation Averaging and Loop Closure, DICTA 2020](https://arxiv.org/pdf/2010.01872.pdf) 21 | 22 | 23 | ## Dependencies 24 | Tested on: 25 | - Eigen 3.3.4 26 | sudo apt install libeigen3-dev 27 | 28 | - SuiteSparse 29 | sudo apt-get install libsuitesparse-dev 30 | 31 | - opencv 4.0.0 32 | https://www.pyimagesearch.com/2018/05/28/ubuntu-18-04-how-to-install-opencv/ provides a good guide. 33 | 34 | - Boost (Filesystem) 1.6.5 35 | sudo apt-get install libboost-all-dev 36 | 37 | - opengv 1.0.0 (refer to the [official site](https://laurentkneip.github.io/opengv/) for potential installation FAQs) 38 | - cd third_party/opengv 39 | - mkdir build 40 | - cd build 41 | - cmake .. 42 | - make 43 | - cmake -DCMAKE_INSTALL_PREFIX=/path/to/local_install -P cmake_install.cmake 44 | 45 | ## Compilation 46 | Finally, to install MRO 47 | 48 | *first edit line 8 and 10 in /src/CMakelist.txt to appropriate directories* 49 | 50 | Then, 51 | 52 | - cd /dir/to/MRO 53 | - mkdir build 54 | - cd build 55 | - cmake .. 56 | - make 57 | 58 | (binary is compiled inside src) 59 | 60 | ## Demo 61 | The demo.sh bash file in written to run KITTI seq00. The sequence (and the entire dataset) can be downloaded [here](http://www.cvlibs.net/datasets/kitti/eval_odometry.php). 62 | 63 | Edit the I/O paths appropriately. 64 | 65 | ORB feature extractor used in MRO takes *config.yaml* and *ORBvoc.txt* as inputs, both are provided in /data. The current config is tuned according to the camera parameters of KITTI sequence00. It needs to be adjusted accordingly when running on other datasets. 66 | 67 | Run ./demo.sh 68 | 69 | ## Hyperparameters 70 | As seen in demo.sh 71 | 72 | - MIN_SET : minimal set to run ransac for opengv's relative rotation estimator, [original paper](https://www.cv-foundation.org/openaccess/content_iccv_2013/papers/Kneip_Direct_Optimization_of_2013_ICCV_paper.pdf) proposes 10. 73 | 74 | - winsize : the number of neighbouring window of frames that MRO performs feature matching on. 75 | 76 | - img_width: image width 77 | 78 | - img_height: image height 79 | 80 | In /src/IRotAvg.cpp 81 | - rotavg_win_size : number of nodes that the underlying incremental rotation averaging solver takes in during optimisation 82 | 83 | - vg_min_matches : minimum number of feature matches to be considered a pair of frames that are viewing the same scene. 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /data/config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 7.188560000000e+02 9 | Camera.fy: 7.188560000000e+02 10 | Camera.cx: 6.071928000000e+02 11 | Camera.cy: 1.852157000000e+02 12 | 13 | Camera.k1: 0 14 | Camera.k2: 0 15 | Camera.p1: 0 16 | Camera.p2: 0 17 | # Camera.k3: 0.0 18 | 19 | # Camera frames per second 20 | Camera.fps: 22.0 21 | #Camera.fps: 4.4 22 | 23 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 24 | Camera.RGB: 1 25 | 26 | #-------------------------------------------------------------------------------------------- 27 | # ORB Parameters 28 | #-------------------------------------------------------------------------------------------- 29 | 30 | # ORB Extractor: Number of features per image 31 | ORBextractor.nFeatures: 4000 32 | 33 | # ORB Extractor: Scale factor between levels in the scale pyramid 34 | ORBextractor.scaleFactor: 1.2 35 | #ORBextractor.scaleFactor: 1.2872 36 | #ORBextractor.scaleFactor: 2 37 | 38 | # ORB Extractor: Number of levels in the scale pyramid ; Ali uses 8 39 | ORBextractor.nLevels: 8 40 | 41 | # ORB Extractor: Fast threshold 42 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 43 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 44 | # You can lower these values if your images have low contrast 45 | ORBextractor.iniThFAST: 20 46 | ORBextractor.minThFAST: 7 47 | 48 | #-------------------------------------------------------------------------------------------- 49 | # Viewer Parameters 50 | #-------------------------------------------------------------------------------------------- 51 | Viewer.KeyFrameSize: 0.1 52 | Viewer.KeyFrameLineWidth: 1 53 | Viewer.GraphLineWidth: 1 54 | Viewer.PointSize:2 55 | Viewer.CameraSize: 0.15 56 | Viewer.CameraLineWidth: 2 57 | Viewer.ViewpointX: 0 58 | Viewer.ViewpointY: -10 59 | Viewer.ViewpointZ: -0.1 60 | Viewer.ViewpointF: 2000 61 | 62 | -------------------------------------------------------------------------------- /demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cd '/path/to/MRO/build/src' 3 | 4 | orb_filename="/path/to/MRO/data/ORBvoc.txt" 5 | 6 | MIN_SET=10 7 | winsize=4 8 | img_width=1241 9 | img_height=376 10 | 11 | config_filename="/path/to/MRO/data/config.yaml" 12 | 13 | sequence_path="/path/to/KITTI/data_odometry_gray/dataset/sequences/00/image_0" 14 | 15 | output_path="/path/to/MRO/outputs" 16 | 17 | sel_id_output_path="${output_path}/view_graph_sel_id.h5" 18 | 19 | conn_output_path="${output_path}/view_graph_connections.h5" 20 | 21 | tij_conn_output_path="${output_path}/view_graph_tij_connections.h5" 22 | 23 | view_conn_output_path="${output_path}/view_graph_views.h5" 24 | 25 | poses_conn_output_path="${output_path}/view_graph_poses.h5" 26 | 27 | ./irotavg $orb_filename $config_filename $sequence_path $sel_id_output_path $conn_output_path $tij_conn_output_path $view_conn_output_path $poses_conn_output_path $MIN_SET $winsize $img_width $img_height 28 | 29 | -------------------------------------------------------------------------------- /ral/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/.DS_Store -------------------------------------------------------------------------------- /ral/._.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/._.DS_Store -------------------------------------------------------------------------------- /ral/._CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/._CMakeLists.txt -------------------------------------------------------------------------------- /ral/._README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/._README.md -------------------------------------------------------------------------------- /ral/._l1_irls.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/._l1_irls.cpp -------------------------------------------------------------------------------- /ral/._l1_irls.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/._l1_irls.hpp -------------------------------------------------------------------------------- /ral/._test.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/._test.cpp -------------------------------------------------------------------------------- /ral/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of iRotAvg. 3 | # 4 | # Created by Alvaro Parra on 19/3/19. 5 | # Copyright © 2019 Alvaro Parra (The University of Adelaide) 7 | # For more information see 8 | # 9 | # This work was supported by Maptek (http://maptek.com) and the 10 | # ARC Linkage Project LP140100946. 11 | # 12 | # iRotAvg is free software: you can redistribute it and/or modify 13 | # it under the terms of the GNU General Public License as published by 14 | # the Free Software Foundation, either version 3 of the License, or 15 | # (at your option) any later version. 16 | # 17 | # iRotAvg is distributed in the hope that it will be useful, 18 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | # GNU General Public License for more details. 21 | # 22 | # You should have received a copy of the GNU General Public License 23 | # along with iRotAvg. If not, see . 24 | # 25 | 26 | 27 | cmake_minimum_required (VERSION 3.0) 28 | project (RAL) 29 | 30 | set(default_build_type "Release") 31 | 32 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 33 | message(STATUS "Setting build type to '${default_build_type}' as none was specified.") 34 | set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE 35 | STRING "Choose the type of build." FORCE) 36 | # Set the possible values of build type for cmake-gui 37 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS 38 | "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 39 | endif() 40 | 41 | 42 | set (CMAKE_CXX_STANDARD 11) 43 | 44 | 45 | # Make CMake aware of the cmake folder for local FindXXX scripts, 46 | # append rather than set in case the user has passed their own 47 | # additional paths via -D. 48 | list(APPEND CMAKE_MODULE_PATH "${RAL_SOURCE_DIR}/cmake") 49 | include(UpdateCacheVariable) 50 | 51 | 52 | 53 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 54 | 55 | 56 | find_package(SuiteSparse) 57 | 58 | if (SUITESPARSE_FOUND) 59 | # On Ubuntu the system install of SuiteSparse (v3.4.0) up to at least 60 | # Ubuntu 13.10 cannot be used to link shared libraries. 61 | if (BUILD_SHARED_LIBS AND 62 | SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION) 63 | 64 | message(FATAL_ERROR "You are attempting to build Ceres as a shared " 65 | "library on Ubuntu using a system package install of SuiteSparse " 66 | "3.4.0. This package is broken and does not support the " 67 | "construction of shared libraries (you can still build Ceres as " 68 | " a static library). If you wish to build a shared version of Ceres " 69 | "you should uninstall the system install of SuiteSparse " 70 | "(libsuitesparse-dev) and perform a source install of SuiteSparse " 71 | "(we recommend that you use the latest version), " 72 | "see http://ceres-solver.org/building.html for more information.") 73 | endif (BUILD_SHARED_LIBS AND 74 | SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION) 75 | 76 | message("-- Found SuiteSparse ${SUITESPARSE_VERSION}, " 77 | "building with SuiteSparse.") 78 | else (SUITESPARSE_FOUND) 79 | message(FATAL_ERROR "Unable to find SuiteSparse!") 80 | endif (SUITESPARSE_FOUND) 81 | 82 | 83 | link_directories( ${SUITESPARSE_LIBRARY_DIR_HINTS} ) 84 | 85 | 86 | add_executable( l1_irls test.cpp l1_irls.cpp ) 87 | 88 | #message(" SuiteSparse INCLUDE ${SUITESPARSE_INCLUDE_DIRS}, " 89 | #"building with SuiteSparse.") 90 | 91 | 92 | 93 | target_include_directories( l1_irls PUBLIC 94 | ${Eigen3_INCLUDE_DIRS} 95 | ${SUITESPARSE_INCLUDE_DIRS} 96 | ) 97 | 98 | 99 | target_link_libraries (l1_irls Eigen3::Eigen ${SUITESPARSE_LIBRARIES} umfpack) 100 | -------------------------------------------------------------------------------- /ral/README.md: -------------------------------------------------------------------------------- 1 | 2 | # RAL 3 | 4 | Author: [Alvaro Parra](http://alvaroparra.com) 5 | 6 | Implementation of rotation averaging algorithms for [iRotAvg](https://github.com/ajparra/iRotAvg/). This implementations allow to fix an arbitrary number of rotations. Instead of conventionally fixing one rotation only (usually the first rotation as the identity), fixing multiple rotations allows to: 7 | - Incorporate known absolute rotations to rotation averaging. 8 | - Efficiently combine multiple rotations averaging problems, e.g., addressing a (large) problem by incrementally optimising over the relative rotations or by chunks. 9 | 10 | 11 | This work was supported by [Maptek](http://maptek.com) and the ARC Linkage Project LP140100946. 12 | 13 |

14 | 15 | Maptek 16 |       17 | 18 | The University of Adelaide 19 | 20 |

21 | 22 | 23 | ## Related Publication: 24 | 25 | [Á. Parra, T.-J. Chin, A. Eriksson, I. Reid: Visual SLAM: Why bundle adjust?, ICRA 2019](https://cs.adelaide.edu.au/~aparra/publication/parra19_icra/) 26 | 27 | 28 | ## License 29 | 30 | iRotAvg is released under a GPLv3 license. 31 | 32 | For a closed-source version of iRotAvg (e.g., for commercial purposes), please [contact the author](https://cs.adelaide.edu.au/~aparra/#contact). 33 | 34 | For an academic use of iRotAvg, please cite 35 | [Á. Parra, T.-J. Chin, A. Eriksson, I. Reid: Why bundle adjust?, ICRA 2019](https://cs.adelaide.edu.au/~aparra/publication/parra19_icra/). 36 | 37 | 38 | ## Dependencies 39 | 40 | - SuiteSparse 41 | - Eigen 42 | 43 | In Mac: 44 | 45 | - `brew install eigen` 46 | - `brew install suite-sparse` 47 | 48 | In Ubuntu: 49 | - `sudo apt install libeigen3-dev` 50 | - `sudo apt-get install libsuitesparse-dev` 51 | 52 | 53 | ## Compilation 54 | 55 | - `mkdir build` 56 | - `cd build` 57 | - `cmake ..` 58 | - `make` 59 | 60 | ## Execution 61 | To see usage simply execute 62 | ``` 63 | ./l1_irls 64 | ``` 65 | 66 | An example is provided in the folder data. To test the method run: 67 | ``` 68 | ./l1_irls ../data/ravg_input.txt 69 | ``` 70 | -------------------------------------------------------------------------------- /ral/anders/._rotavg.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/anders/._rotavg.m -------------------------------------------------------------------------------- /ral/anders/._test_avg.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/anders/._test_avg.m -------------------------------------------------------------------------------- /ral/anders/ORB_pose.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/anders/ORB_pose.mat -------------------------------------------------------------------------------- /ral/anders/rotavg.m: -------------------------------------------------------------------------------- 1 | function [RotsAvg, iter, objVals] = rotavg(Rots, R) 2 | % rotation averaging 3 | 4 | objVals = []; 5 | 6 | R0 = [Rots{:}]; 7 | Y0 = R0'*R0; 8 | Y = Y0; 9 | 10 | m = length(Rots); 11 | 12 | % Solve rot avg from R and Y0 13 | 14 | oldObj = -trace(R*Y); 15 | numOfIters = 10000; 16 | 17 | for iter = 1:numOfIters % add convergence criteria 18 | % select k 19 | %k = randsample(1:m, 1); 20 | for k = 1:m 21 | % B: eliminating the kth row and column from Y 22 | B = Y; 23 | B(3*k-2:3*k,:) = []; % eliminating kth row 24 | B(:,3*k-2:3*k) = []; % eliminating kth col 25 | 26 | % W: eliminating the kth col and all but the kth row from R 27 | W = R(3*k-2:3*k, :); % the kth row from R 28 | W(:,3*k-2:3*k) = []; % eliminating kth col 29 | W = W'; % make W a column vector 30 | 31 | % Obtain S 32 | S = B*W*pinv( sqrtm(W'*B*W) ); 33 | 34 | Y = [eye(3) S'; S B]; 35 | 36 | % reorder 37 | order = [4:3*k 1:3 (3*k+1):3*m]; 38 | Y = Y(order,order); 39 | end 40 | 41 | % eval soluiton 42 | obj = -trace(R*Y); 43 | 44 | objVals(end+1)=obj; 45 | 46 | fprintf('obj %f oldObj %f\n', obj, oldObj); 47 | 48 | if (oldObj-obj)/max(abs(oldObj),1) <= 1e-8 49 | break 50 | end 51 | oldObj = obj; 52 | end 53 | % 54 | % Obtain Ropt 55 | RotsAvg = cell(1,m); 56 | for i = 1:m 57 | RotsAvg{i} = Y(1:3, 3*i-2:3*i); 58 | 59 | if det(RotsAvg{i})<0 60 | RotsAvg{i} = -RotsAvg{i}; 61 | end 62 | end 63 | end -------------------------------------------------------------------------------- /ral/anders/rotdist.m: -------------------------------------------------------------------------------- 1 | function [ theta ] = rotdist( S, R) 2 | %Angular distance between two rotation matrices. 3 | 4 | n = norm(S-R,'fro'); 5 | theta = 2*asin(n/(2*sqrt(2))); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /ral/anders/roterr.m: -------------------------------------------------------------------------------- 1 | function [R] = roterr(err) 2 | % rotation matrix of err ang error about a random axis of rotation 3 | 4 | R = vrrotvec2mat([2*rand(1,3)-1, err*randn()] ); 5 | end -------------------------------------------------------------------------------- /ral/anders/test_avg.m: -------------------------------------------------------------------------------- 1 | 2 | % make relative rotations from GT 3 | sampleIdx = 1:1:450; 4 | ORB_pose = load('ORB_pose'); 5 | 6 | CvORB_sampled = ORB_pose.CvORB(:,:,sampleIdx); 7 | 8 | rotsGT = cell(1,length(CvORB_sampled)); 9 | k = 1; 10 | for i=sampleIdx 11 | rotsGT{k} = ORB_pose.CvORB(:,:,i)'; 12 | k = k+1; 13 | end 14 | 15 | % make block rotation matrix 16 | m = size(rotsGT,2); 17 | R = cell(m,m); 18 | for i=1:m 19 | Ri = rotsGT{i}; 20 | R{i,i} = zeros(3,3); 21 | for j=(i+1):m 22 | Rj = rotsGT{j}; 23 | R{i,j} = Ri'*Rj; 24 | %R{i,j} = Rj*Ri'; 25 | 26 | R{j,i} = R{i,j}'; 27 | end 28 | end 29 | 30 | % run rot avg 31 | rotsGT = rotsGT(1:m); 32 | rots_init = cellfun(@(x) roterr(.1*pi/180)*x, rotsGT, 'UniformOutput', 0); 33 | 34 | [ravg, iter, objVals] = rotavg(rots_init, cell2mat(R)); 35 | %[ravg] = rotavg_l1irls(R, rots_init); 36 | 37 | 38 | avgErr = zeros(1,m); 39 | initErr = zeros(1,m); 40 | for i=1:m 41 | avgErr(i) = rotdist(ravg{i}, rotsGT{i}); 42 | initErr(i) = rotdist(rots_init{i}, rotsGT{i}); 43 | end 44 | 45 | figure, plot(avgErr.*180/pi, 'linewidth', 2) 46 | hold on 47 | plot(initErr.*180/pi) 48 | legend('avg', 'init ') 49 | -------------------------------------------------------------------------------- /ral/cmake/._FindSuiteSparse.cmake: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/cmake/._FindSuiteSparse.cmake -------------------------------------------------------------------------------- /ral/cmake/._UpdateCacheVariable.cmake: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/cmake/._UpdateCacheVariable.cmake -------------------------------------------------------------------------------- /ral/cmake/UpdateCacheVariable.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of iRotAvg. 3 | # This file is based on the file UpdateCacheVariable.cmake 4 | # from Ceres Solver (released under the New BSD license, 5 | # see its oroginal header below). 6 | # 7 | # Created by Alvaro Parra on 19/3/19. 8 | # Copyright © 2019 Alvaro Parra (The University of Adelaide) 10 | # For more information see 11 | # 12 | # This work was supported by Maptek (http://maptek.com) and the 13 | # ARC Linkage Project LP140100946. 14 | # 15 | # iRotAvg is free software: you can redistribute it and/or modify 16 | # it under the terms of the GNU General Public License as published by 17 | # the Free Software Foundation, either version 3 of the License, or 18 | # (at your option) any later version. 19 | # 20 | # iRotAvg is distributed in the hope that it will be useful, 21 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | # GNU General Public License for more details. 24 | # 25 | # You should have received a copy of the GNU General Public License 26 | # along with iRotAvg. If not, see . 27 | # 28 | 29 | # 30 | ############################################################################ 31 | # Ceres Solver - A fast non-linear least squares minimizer 32 | # Copyright 2015 Google Inc. All rights reserved. 33 | # http://ceres-solver.org/ 34 | # 35 | # Redistribution and use in source and binary forms, with or without 36 | # modification, are permitted provided that the following conditions are met: 37 | # 38 | # * Redistributions of source code must retain the above copyright notice, 39 | # this list of conditions and the following disclaimer. 40 | # * Redistributions in binary form must reproduce the above copyright notice, 41 | # this list of conditions and the following disclaimer in the documentation 42 | # and/or other materials provided with the distribution. 43 | # * Neither the name of Google Inc. nor the names of its contributors may be 44 | # used to endorse or promote products derived from this software without 45 | # specific prior written permission. 46 | # 47 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 48 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 49 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 50 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 51 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 52 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 53 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 54 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 55 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 56 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 57 | # POSSIBILITY OF SUCH DAMAGE. 58 | # 59 | # Author: alexs.mac@gmail.com (Alex Stewart) 60 | 61 | # By default, there is no easy way in CMake to set the value of a cache 62 | # variable without reinitialising it, which involves resetting its 63 | # associated help string. This is particularly annoying for CMake options 64 | # where they need to programmatically updated. 65 | # 66 | # This function automates this process by getting the current help string 67 | # for the cache variable to update, then reinitialising it with the new 68 | # value, but with the original help string. 69 | function(UPDATE_CACHE_VARIABLE VAR_NAME VALUE) 70 | get_property(IS_DEFINED_IN_CACHE CACHE ${VAR_NAME} PROPERTY VALUE SET) 71 | if (NOT IS_DEFINED_IN_CACHE) 72 | message(FATAL_ERROR "Specified variable to update in cache: " 73 | "${VAR_NAME} has not been set in the cache.") 74 | endif() 75 | get_property(HELP_STRING CACHE ${VAR_NAME} PROPERTY HELPSTRING) 76 | get_property(VAR_TYPE CACHE ${VAR_NAME} PROPERTY TYPE) 77 | set(${VAR_NAME} ${VALUE} CACHE ${VAR_TYPE} "${HELP_STRING}" FORCE) 78 | endfunction() 79 | 80 | -------------------------------------------------------------------------------- /ral/data/._ravg_input.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/data/._ravg_input.txt -------------------------------------------------------------------------------- /ral/rotavg_anders.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/ral/rotavg_anders.zip -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/ral/cmake") 2 | include(UpdateCacheVariable) 3 | 4 | 5 | find_package(Boost COMPONENTS filesystem REQUIRED) 6 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 7 | 8 | find_package(OpenCV 4 PATHS "../release/lib/cmake/opencv4" REQUIRED) 9 | 10 | find_package(opengv PATHS "../MRO/third_party/opengv/local_install/lib/cmake/opengv-1.0" REQUIRED) 11 | 12 | 13 | message(${OpenCV_LIBRARIES}) 14 | message(${OpenCV_INCLUDE_DIRS}) 15 | 16 | find_package(h5cpp REQUIRED) 17 | find_package(SuiteSparse) 18 | 19 | add_definitions (-march=native) 20 | 21 | if (SUITESPARSE_FOUND) 22 | # On Ubuntu the system install of SuiteSparse (v3.4.0) up to at least 23 | # Ubuntu 13.10 cannot be used to link shared libraries. 24 | if (BUILD_SHARED_LIBS AND 25 | SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION) 26 | 27 | message(FATAL_ERROR "You are attempting to build Ceres as a shared " 28 | "library on Ubuntu using a system package install of SuiteSparse " 29 | "3.4.0. This package is broken and does not support the " 30 | "construction of shared libraries (you can still build Ceres as " 31 | " a static library). If you wish to build a shared version of Ceres " 32 | "you should uninstall the system install of SuiteSparse " 33 | "(libsuitesparse-dev) and perform a source install of SuiteSparse " 34 | "(we recommend that you use the latest version), " 35 | "see http://ceres-solver.org/building.html for more information.") 36 | endif (BUILD_SHARED_LIBS AND 37 | SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION) 38 | 39 | message("-- Found SuiteSparse ${SUITESPARSE_VERSION}, " 40 | "building with SuiteSparse.") 41 | else (SUITESPARSE_FOUND) 42 | message(FATAL_ERROR "Unable to find SuiteSparse!") 43 | endif (SUITESPARSE_FOUND) 44 | 45 | 46 | link_directories( ${SUITESPARSE_LIBRARY_DIR_HINTS} ) 47 | 48 | 49 | add_executable( irotavg IRotAvg 50 | ${PROJECT_SOURCE_DIR}/ral/l1_irls 51 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DBoW2/FeatureVector 52 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DBoW2/BowVector 53 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DBoW2/ScoringObject 54 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DBoW2/FORB 55 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DUtils/Timestamp 56 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DUtils/Random 57 | ORBVocabulary 58 | ViewGraph 59 | SequenceLoader 60 | Frame 61 | View 62 | Camera 63 | ORBExtractor 64 | Pose 65 | Converter 66 | ViewDatabase 67 | ) 68 | 69 | target_include_directories( irotavg PUBLIC 70 | ${Eigen3_INCLUDE_DIRS} 71 | ${SUITESPARSE_INCLUDE_DIRS} 72 | ${OpenCV_INCLUDE_DIRS} 73 | ${PROJECT_SOURCE_DIR}/third_party/DBoW2/DBoW2/ 74 | ${PROJECT_SOURCE_DIR}/ral/ 75 | ) 76 | 77 | target_link_libraries (irotavg 78 | Eigen3::Eigen 79 | ${SUITESPARSE_LIBRARIES} umfpack 80 | ${OpenCV_LIBRARIES} 81 | ${Boost_FILESYSTEM_LIBRARY} 82 | -lboost_system 83 | h5cpp 84 | opengv) 85 | 86 | -------------------------------------------------------------------------------- /src/Camera.cpp: -------------------------------------------------------------------------------- 1 | #include "Camera.hpp" 2 | 3 | using namespace irotavg; 4 | 5 | void Camera::init(CameraParameters intrinsics, const cv::Mat im) 6 | { 7 | m_camera_parameters = std::move(intrinsics); 8 | 9 | const CameraParameters::Dist_type &dist_coef = m_camera_parameters.dist_coef(); 10 | 11 | // uncalibrated case 12 | if(dist_coef(0) != 0.0) 13 | { 14 | cv::Mat mat(4,2,CV_64F); 15 | mat.at(0,0) = 0.0; mat.at(0,1) = 0.0; 16 | mat.at(1,0) = im.cols; mat.at(1,1) = 0.0; 17 | mat.at(2,0) = 0.0; mat.at(2,1) = im.rows; 18 | mat.at(3,0) = im.cols; mat.at(3,1) = im.rows; 19 | 20 | const CameraParameters::Intrinsic_type &K = m_camera_parameters.intrinsic(); 21 | 22 | // Undistort corners 23 | mat = mat.reshape(2); 24 | cv::undistortPoints(mat, mat, K, dist_coef, cv::Mat(), K); 25 | mat = mat.reshape(1); 26 | 27 | m_minX = std::min(mat.at(0,0), mat.at(2,0)); 28 | m_maxX = std::max(mat.at(1,0), mat.at(3,0)); 29 | m_minY = std::min(mat.at(0,1), mat.at(1,1)); 30 | m_maxY = std::max(mat.at(2,1), mat.at(3,1)); 31 | } 32 | else 33 | { 34 | m_minX = 0.0f; 35 | m_maxX = im.cols; 36 | m_minY = 0.0f; 37 | m_maxY = im.rows; 38 | } 39 | 40 | m_gridCellWidthInv = (FRAME_GRID_COLS)/(m_maxX-m_minX); 41 | m_gridCellHeightInv = (FRAME_GRID_ROWS)/(m_maxY-m_minY); 42 | } 43 | -------------------------------------------------------------------------------- /src/Camera.hpp: -------------------------------------------------------------------------------- 1 | #ifndef Camera_hpp 2 | #define Camera_hpp 3 | 4 | #include 5 | 6 | #define FRAME_GRID_ROWS 48 7 | #define FRAME_GRID_COLS 64 8 | 9 | // singleton 10 | //https://stackoverflow.com/questions/1008019/c-singleton-design-pattern 11 | 12 | 13 | namespace irotavg 14 | { 15 | 16 | class CameraParameters 17 | { 18 | public: 19 | typedef cv::Matx33d Intrinsic_type; 20 | typedef cv::Vec4d Dist_type; 21 | 22 | CameraParameters(): 23 | m_intrinsic( Intrinsic_type::eye() ), 24 | m_dist_coef( Dist_type::all(0) ) 25 | {} 26 | 27 | CameraParameters(Intrinsic_type K): 28 | m_intrinsic(std::move(K)) 29 | {} 30 | 31 | CameraParameters(Intrinsic_type K, Dist_type dist_coef): 32 | m_intrinsic(std::move(K)), 33 | m_dist_coef(std::move(dist_coef)) 34 | {} 35 | 36 | double f() const { return m_intrinsic(0,0); } 37 | cv::Point2d pp() const { return cv::Point2d(m_intrinsic(0,2), m_intrinsic(1,2)); } 38 | const Intrinsic_type &intrinsic() const { return m_intrinsic; } 39 | const Dist_type &dist_coef() const { return m_dist_coef; } 40 | 41 | private: 42 | 43 | Intrinsic_type m_intrinsic; // intrinsic matrix 44 | Dist_type m_dist_coef; // [k1 k2 p1 p2] 45 | }; 46 | 47 | 48 | 49 | // Camera -- singleton class 50 | class Camera 51 | { 52 | public: 53 | Camera(Camera const&) = delete; 54 | 55 | void operator=(Camera const&) = delete; 56 | 57 | const double &minX() const { return m_minX; } 58 | const double &minY() const { return m_minY; } 59 | const double &maxX() const { return m_maxX; } 60 | const double &maxY() const { return m_maxY; } 61 | const double &gridCellWidthInv() const { return m_gridCellWidthInv; } 62 | const double &gridCellHeightInv() const { return m_gridCellHeightInv; } 63 | 64 | 65 | static Camera& instance() 66 | { 67 | static Camera instance; 68 | return instance; 69 | } 70 | 71 | const CameraParameters &cameraParameters() const { return m_camera_parameters; } 72 | 73 | 74 | void init(CameraParameters intrinsics, const cv::Mat im); 75 | 76 | 77 | 78 | private: 79 | 80 | Camera() {} 81 | 82 | double m_minX, m_maxX, m_minY, m_maxY; 83 | double m_gridCellWidthInv; 84 | double m_gridCellHeightInv; 85 | CameraParameters m_camera_parameters; 86 | }; 87 | 88 | } 89 | 90 | #endif /* Camera_hpp */ 91 | -------------------------------------------------------------------------------- /src/Converter.cpp: -------------------------------------------------------------------------------- 1 | #include "Converter.hpp" 2 | 3 | using namespace irotavg; 4 | 5 | //// TODO: move to an static fucntion in some util class 6 | std::vector Converter::descriptorsMatToVector(const cv::Mat &descriptors) 7 | { 8 | std::vector out; 9 | const int n = descriptors.rows; 10 | out.reserve(n); 11 | for (int j=0; j 6 | #include 7 | 8 | namespace irotavg 9 | { 10 | namespace Converter 11 | { 12 | std::vector descriptorsMatToVector(const cv::Mat &descriptors); 13 | } 14 | } 15 | 16 | 17 | #endif /* Converter_hpp */ 18 | -------------------------------------------------------------------------------- /src/Frame.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef Frame_hpp 4 | #define Frame_hpp 5 | 6 | #include 7 | #include "opencv2/opencv.hpp" 8 | 9 | #include 10 | #include 11 | #include "ORBExtractor.hpp" 12 | #include "Camera.hpp" 13 | 14 | // BoW 15 | #include "BowVector.h" 16 | #include "FeatureVector.h" 17 | 18 | #include "ORBVocabulary.hpp" 19 | 20 | 21 | //--------------------------------------------------- 22 | namespace irotavg 23 | { 24 | 25 | class Frame 26 | { 27 | #define USE_ORF_FLAG true 28 | 29 | public: 30 | 31 | Frame(int id, const std::string path, ORB_SLAM2::ORBextractor &orbExtractor) : 32 | USE_ORB(USE_ORF_FLAG), // 33 | m_id(id), 34 | m_path(path), 35 | m_orb_extractor(orbExtractor) 36 | { 37 | findFeatures(); 38 | undistortKeypoints(); 39 | assignFeaturesToGrid(); 40 | computeBoW(); 41 | } 42 | 43 | std::vector &keypoints() { return m_keypoints; } 44 | std::vector &undistortedKeypoints() { return m_undistorted_keypoints; } 45 | 46 | cv::Mat &descriptors() { return m_descriptors; } 47 | 48 | int id() const { return m_id; } 49 | 50 | cv::Mat getImage(); 51 | 52 | 53 | bool posInGrid(const cv::KeyPoint &kp, int &x, int &y); 54 | 55 | void assignFeaturesToGrid(); 56 | 57 | std::vector getFeaturesInArea(double x, double y, double r, 58 | int minLevel=-1, int maxLevel=-1) const; 59 | 60 | 61 | const DBoW2::FeatureVector &bow_features() const { return m_bow_features; } 62 | 63 | const DBoW2::BowVector &bow() const { return m_bow; } 64 | 65 | 66 | private: 67 | void findFeatures(); 68 | 69 | void undistortKeypoints(); 70 | 71 | void computeBoW(); 72 | 73 | const bool USE_ORB; 74 | 75 | const int m_id; 76 | const std::string m_path; 77 | ORB_SLAM2::ORBextractor &m_orb_extractor; 78 | //static cv::Ptr detector (cv::ORB::create() ); 79 | 80 | std::vector m_keypoints; 81 | std::vector m_undistorted_keypoints; 82 | cv::Mat m_descriptors; 83 | 84 | // Keypoints are assigned to cells in a grid 85 | std::vector m_grid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 86 | 87 | // ----------------------------------------------------------- 88 | // BoW members 89 | DBoW2::BowVector m_bow; 90 | DBoW2::FeatureVector m_bow_features; 91 | }; 92 | 93 | } 94 | 95 | #endif /* Frame_hpp */ 96 | -------------------------------------------------------------------------------- /src/ORBExtractor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ORBEXTRACTOR_H 2 | #define ORBEXTRACTOR_H 3 | 4 | #include 5 | #include 6 | //#include 7 | 8 | 9 | namespace ORB_SLAM2 10 | { 11 | 12 | class ExtractorNode 13 | { 14 | public: 15 | ExtractorNode():bNoMore(false){} 16 | 17 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 18 | 19 | std::vector vKeys; 20 | cv::Point2i UL, UR, BL, BR; 21 | std::list::iterator lit; 22 | bool bNoMore; 23 | }; 24 | 25 | class ORBextractor 26 | { 27 | public: 28 | 29 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 30 | 31 | 32 | 33 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 34 | int iniThFAST, int minThFAST); 35 | 36 | 37 | ~ORBextractor(){} 38 | 39 | // Compute the ORB features and descriptors on an image. 40 | // ORB are dispersed on the image using an octree. 41 | // Mask is ignored in the current implementation. 42 | void operator()( cv::InputArray image, cv::InputArray mask, 43 | std::vector& keypoints, 44 | cv::OutputArray descriptors); 45 | 46 | int inline GetLevels(){ 47 | return nlevels;} 48 | 49 | float inline GetScaleFactor(){ 50 | return scaleFactor;} 51 | 52 | std::vector inline GetScaleFactors(){ 53 | return mvScaleFactor; 54 | } 55 | 56 | std::vector inline GetInverseScaleFactors(){ 57 | return mvInvScaleFactor; 58 | } 59 | 60 | std::vector inline GetScaleSigmaSquares(){ 61 | return mvLevelSigma2; 62 | } 63 | 64 | std::vector inline GetInverseScaleSigmaSquares(){ 65 | return mvInvLevelSigma2; 66 | } 67 | 68 | std::vector mvImagePyramid; 69 | 70 | protected: 71 | 72 | void ComputePyramid(cv::Mat image); 73 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 74 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 75 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 76 | 77 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 78 | std::vector pattern; 79 | 80 | int nfeatures; 81 | double scaleFactor; 82 | int nlevels; 83 | int iniThFAST; 84 | int minThFAST; 85 | 86 | std::vector mnFeaturesPerLevel; 87 | 88 | std::vector umax; 89 | 90 | std::vector mvScaleFactor; 91 | std::vector mvInvScaleFactor; 92 | std::vector mvLevelSigma2; 93 | std::vector mvInvLevelSigma2; 94 | }; 95 | 96 | } //namespace ORB_SLAM 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /src/ORBVocabulary.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "ORBVocabulary.hpp" 4 | 5 | using namespace irotavg; 6 | 7 | void ORBVocabulary::load(const std::string &filename) 8 | { 9 | //Load ORB Vocabulary 10 | std::cout << "\nLoading ORB Vocabulary from " << filename << 11 | "\nThis could take a while..." << std::endl; 12 | 13 | if( !m_vocabulary.loadFromTextFile(filename) ) 14 | { 15 | std::cerr << "Wrong path to vocabulary. " << std::endl; 16 | std::cerr << "Failed to open: " << filename << std::endl; 17 | exit(-1); 18 | } 19 | std::cout << "Vocabulary loaded!\n" << std::endl; 20 | } 21 | -------------------------------------------------------------------------------- /src/ORBVocabulary.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef ORBVocabulary_hpp 4 | #define ORBVocabulary_hpp 5 | 6 | #include 7 | #include 8 | 9 | // ORB Vocabulary 10 | #include"FORB.h" 11 | #include"TemplatedVocabulary.h" 12 | // We assume the vocabulary tree has 6 levels, change the 4 otherwise 13 | #define ORB_VOCAB_LEVELS 4 14 | 15 | 16 | 17 | namespace irotavg 18 | { 19 | 20 | // ORBVocabulary -- singleton class 21 | class ORBVocabulary 22 | { 23 | public: 24 | 25 | typedef DBoW2::TemplatedVocabulary Vocabulary; 26 | 27 | ORBVocabulary(ORBVocabulary const&) = delete; 28 | 29 | void operator=(ORBVocabulary const&) = delete; 30 | 31 | static ORBVocabulary& instance() 32 | { 33 | static ORBVocabulary instance; 34 | return instance; 35 | } 36 | 37 | const Vocabulary &vocabulary() const { return m_vocabulary; } 38 | 39 | void load(const std::string &filename); 40 | 41 | 42 | private: 43 | 44 | ORBVocabulary(){} 45 | 46 | std::string m_vocabulary_filename; 47 | Vocabulary m_vocabulary; 48 | }; 49 | } 50 | 51 | 52 | #endif /* ORBVocabulary_hpp */ 53 | -------------------------------------------------------------------------------- /src/Pose.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Pose.hpp" 3 | 4 | -------------------------------------------------------------------------------- /src/Pose.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef Pose_hpp 3 | #define Pose_hpp 4 | 5 | #include 6 | #include 7 | 8 | namespace irotavg 9 | { 10 | 11 | class Pose 12 | { 13 | 14 | public: 15 | typedef cv::Matx33d Mat3; 16 | typedef cv::Vec3d Vec3; 17 | typedef cv::Vec4d Vec4; 18 | 19 | Pose(): m_R(Mat3::eye()), m_t(cv::Vec3d(0,0,0)) {} 20 | 21 | Pose(Mat3 R, Vec3 t): m_R(R), m_t(t) {} 22 | 23 | void setR(Mat3 R) { m_R = std::move(R); } 24 | 25 | void setT(Vec3 t) { m_t = std::move(t); } 26 | 27 | const Mat3 &R() const { return m_R; } 28 | 29 | const Vec3 &t() const { return m_t; } 30 | 31 | private: 32 | 33 | Mat3 m_R; 34 | Vec3 m_t; 35 | }; 36 | 37 | } 38 | 39 | #endif /* Pose_hpp */ 40 | -------------------------------------------------------------------------------- /src/SequenceLoader.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "SequenceLoader.hpp" 3 | 4 | using namespace irotavg; 5 | 6 | SequenceLoader::SequenceLoader(std::string path, std::string im_ext, int timestamp_offset) 7 | { 8 | //im_ext -- image extension, includes the dot Ex: ".png" 9 | //timestamp_offset -- offset of the timestamp in the filenae 10 | 11 | namespace fs = ::boost::filesystem; 12 | pair_id_path frame_pair; 13 | int timestamp; 14 | fs::path p(path); 15 | for (auto it = fs::directory_iterator(p); it != fs::directory_iterator(); it++) 16 | { 17 | if(fs::is_regular_file(*it) && it->path().extension().string() == im_ext) 18 | { 19 | timestamp = std::stoi(it->path().stem().string().substr(timestamp_offset)); 20 | frame_pair = std::make_pair(timestamp, it->path()); 21 | m_frames.push_back(frame_pair); 22 | } 23 | } 24 | // sort by the timestamp 25 | std::sort(m_frames.begin(), m_frames.end()); 26 | } 27 | -------------------------------------------------------------------------------- /src/SequenceLoader.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef SequenceLoader_hpp 4 | #define SequenceLoader_hpp 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | namespace irotavg 12 | { 13 | class SequenceLoader 14 | { 15 | public: 16 | 17 | typedef std::pair pair_id_path; 18 | 19 | SequenceLoader(std::string path, std::string im_ext, int timestamp_offset=0); 20 | 21 | std::vector::const_iterator begin() const 22 | { 23 | return m_frames.begin(); 24 | } 25 | 26 | std::vector::const_iterator end() const 27 | { 28 | return m_frames.end(); 29 | } 30 | 31 | private: 32 | std::vector m_frames; 33 | }; 34 | } 35 | 36 | #endif /* SequenceLoader_hpp */ 37 | -------------------------------------------------------------------------------- /src/View.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "View.hpp" 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/View.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef View_hpp 4 | #define View_hpp 5 | 6 | #include 7 | #include 8 | #include "Frame.hpp" 9 | #include "Pose.hpp" 10 | 11 | #include // std::greater 12 | #include // std::sort 13 | 14 | 15 | namespace irotavg 16 | { 17 | 18 | typedef std::vector FeatureMatches; 19 | 20 | class View 21 | { 22 | 23 | public: 24 | 25 | class ViewConnection 26 | { 27 | public: 28 | ViewConnection(View &v1, View &v2, FeatureMatches matches, Pose rel_pose): 29 | m_v1(v1), m_v2(v2), m_matches(std::move(matches)), m_rel_pose(std::move(rel_pose)) 30 | {} 31 | 32 | FeatureMatches &matches() { return m_matches; } 33 | size_t size() const { return m_matches.size(); } 34 | const Pose &pose() const { return m_rel_pose; } 35 | 36 | 37 | private: 38 | View &m_v1; 39 | View &m_v2; 40 | FeatureMatches m_matches; 41 | Pose m_rel_pose; 42 | 43 | }; 44 | 45 | typedef std::map Connections; 46 | 47 | 48 | View(Frame &frame): m_frame(frame) {} 49 | 50 | Frame &frame() { return m_frame; } 51 | 52 | Pose &pose() { return m_pose; } 53 | 54 | Pose &k_pose() { return m_pose; } 55 | 56 | bool isConnectedTo(const View &view) const 57 | { 58 | return (m_connections.count(const_cast(&view))>0); 59 | } 60 | 61 | FeatureMatches &getFeatureMatches(const View &view) 62 | { 63 | //make sure noes are connected! 64 | assert(this->isConnectedTo(view)); 65 | ViewConnection *view_connection = m_connections[const_cast(&view)]; 66 | FeatureMatches &matches = view_connection->matches(); 67 | return matches; 68 | } 69 | 70 | const Connections &connections() const { return m_connections; } 71 | 72 | 73 | static bool connect(View &v1, View &v2, FeatureMatches matches, Pose rel_pose); 74 | 75 | //TODO: implement the desctructor... 76 | 77 | 78 | 79 | std::vector getBestCovisibilityViews(const int n) 80 | { 81 | 82 | typedef std::pair WeightedView; 83 | 84 | std::vector weighted_views; 85 | for (auto connection: m_connections) 86 | { 87 | View *view = connection.first; 88 | ViewConnection *view_connection = connection.second; 89 | 90 | // check there is no null elements in connections!!! 91 | assert(view != NULL); 92 | assert(view_connection != NULL); 93 | 94 | weighted_views.push_back( std::make_pair( (int)view_connection->size(), view) ); 95 | } 96 | 97 | // sort by the number of connections in descending order 98 | std::sort(weighted_views.begin(), weighted_views.end(), 99 | [](const WeightedView &x, const WeightedView &y) //TODO: fix x is becames null??? why??? 100 | { 101 | return x.first > y.first; 102 | } 103 | ); 104 | 105 | std::vector covisibility; 106 | covisibility.reserve(n); 107 | 108 | int i = 0; 109 | for (auto wv: weighted_views) 110 | { 111 | if (i++==n) break; 112 | 113 | covisibility.push_back(wv.second); 114 | } 115 | 116 | return covisibility; 117 | } 118 | 119 | 120 | 121 | private: 122 | 123 | 124 | //int m_id; 125 | Frame m_frame; //make sure Frame is movable! 126 | Pose m_pose; // absolute pose 127 | std::map m_connections; 128 | }; 129 | 130 | } 131 | 132 | 133 | #endif /* View_hpp */ 134 | -------------------------------------------------------------------------------- /src/ViewDatabase.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef ViewDatabase_hpp 3 | #define ViewDatabase_hpp 4 | 5 | #include "ViewDatabase.hpp" 6 | #include 7 | #include "Frame.hpp" 8 | #include "View.hpp" 9 | #include "ORBVocabulary.hpp" 10 | 11 | 12 | namespace irotavg 13 | { 14 | class ViewDatabase 15 | { 16 | public: 17 | 18 | ViewDatabase(ViewDatabase const&) = delete; 19 | 20 | void operator=(ViewDatabase const&) = delete; 21 | 22 | static ViewDatabase& instance() 23 | { 24 | static ViewDatabase instance; 25 | return instance; 26 | } 27 | 28 | void init() 29 | { 30 | const ORBVocabulary &voc = ORBVocabulary::instance(); 31 | m_inverted_file.resize(voc.vocabulary().size()); 32 | } 33 | 34 | 35 | void add(View *view); 36 | void erase(View *view); 37 | 38 | void clear() 39 | { 40 | m_inverted_file.clear(); 41 | init(); 42 | } 43 | 44 | std::list findViewsSharingWords(View &view, std::map &num_of_shared_words); 45 | std::vector detectLoopCandidates(View &view, double min_score); 46 | 47 | 48 | 49 | private: 50 | 51 | ViewDatabase(){} 52 | 53 | //std::string m_vocabulary_filename; 54 | 55 | // Inverted file 56 | std::vector< std::list > m_inverted_file; 57 | }; 58 | } 59 | 60 | 61 | #endif /* ViewDatabase_hpp */ 62 | -------------------------------------------------------------------------------- /third_party/._DBoW2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/._DBoW2 -------------------------------------------------------------------------------- /third_party/DBoW2/._CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/._CMakeLists.txt -------------------------------------------------------------------------------- /third_party/DBoW2/._DBoW2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/._DBoW2 -------------------------------------------------------------------------------- /third_party/DBoW2/._DUtils: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/._DUtils -------------------------------------------------------------------------------- /third_party/DBoW2/._LICENSE.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/._LICENSE.txt -------------------------------------------------------------------------------- /third_party/DBoW2/._README.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/._README.txt -------------------------------------------------------------------------------- /third_party/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV 3.0 QUIET) 28 | if(NOT OpenCV_FOUND) 29 | find_package(OpenCV 2.4.3 QUIET) 30 | if(NOT OpenCV_FOUND) 31 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 32 | endif() 33 | endif() 34 | 35 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 36 | 37 | include_directories(${OpenCV_INCLUDE_DIRS}) 38 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 39 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 40 | 41 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._BowVector.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._BowVector.cpp -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._BowVector.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._BowVector.h -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._FClass.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._FClass.h -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._FORB.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._FORB.cpp -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._FORB.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._FORB.h -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._FeatureVector.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._FeatureVector.cpp -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._FeatureVector.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._FeatureVector.h -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._ScoringObject.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._ScoringObject.cpp -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._ScoringObject.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._ScoringObject.h -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/._TemplatedVocabulary.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DBoW2/._TemplatedVocabulary.h -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | //std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /third_party/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /third_party/DBoW2/DUtils/._Random.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DUtils/._Random.cpp -------------------------------------------------------------------------------- /third_party/DBoW2/DUtils/._Random.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DUtils/._Random.h -------------------------------------------------------------------------------- /third_party/DBoW2/DUtils/._Timestamp.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DUtils/._Timestamp.cpp -------------------------------------------------------------------------------- /third_party/DBoW2/DUtils/._Timestamp.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/DBoW2/DUtils/._Timestamp.h -------------------------------------------------------------------------------- /third_party/DBoW2/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /third_party/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. 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 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /third_party/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /third_party/opengv/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | -------------------------------------------------------------------------------- /third_party/opengv/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /third_party/opengv/.idea/opengv.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/Makefile.ros: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /third_party/opengv/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, The Australian National University 12 | contact: kneip.laurent@gmail.com 13 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/absolute_central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/absolute_central.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/absolute_central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/absolute_central.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/absolute_central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/absolute_central.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/absolute_noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/absolute_noncentral.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/absolute_noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/absolute_noncentral.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/absolute_noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/absolute_noncentral.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/central.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/central.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/central.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/multi_viewpoint.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/multi_viewpoint.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/multi_viewpoint.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/multi_viewpoint.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/multi_viewpoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/multi_viewpoint.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/noncentral.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/noncentral.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/noncentral.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/nonoverlapping.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/nonoverlapping.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/nonoverlapping.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/nonoverlapping.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/nonoverlapping.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/nonoverlapping.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/point_cloud.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/point_cloud.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/point_cloud.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/point_cloud.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/point_cloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/point_cloud.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/relative_central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/relative_central.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/relative_central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/relative_central.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/relative_central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/relative_central.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/relative_noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/relative_noncentral.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/relative_noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/relative_noncentral.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/relative_noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/relative_noncentral.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/reprojectionError.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/reprojectionError.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/reprojectionError.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/reprojectionError.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/reprojectionError.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/reprojectionError.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/triangulation_central.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/triangulation_central.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/triangulation_central.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/triangulation_central.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/triangulation_central.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/triangulation_central.png -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/triangulation_noncentral.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/triangulation_noncentral.dia -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/triangulation_noncentral.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/triangulation_noncentral.pdf -------------------------------------------------------------------------------- /third_party/opengv/doc/addons/images/triangulation_noncentral.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/doc/addons/images/triangulation_noncentral.png -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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; -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/matlab/opengv.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ckchng/MRO/6256415d8c4885fc229c57aace7c38738c97257a/third_party/opengv/matlab/opengv.mexa64 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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 -------------------------------------------------------------------------------- /third_party/opengv/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); -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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}) -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | -------------------------------------------------------------------------------- /third_party/opengv/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 | --------------------------------------------------------------------------------