├── backend ├── CMakeLists.txt └── include │ └── ygz │ └── BackendInterface.h ├── Thirdparty ├── fast │ ├── test │ │ ├── data │ │ │ └── test1.png │ │ └── test.cpp │ ├── README.md │ ├── package.xml │ ├── fastConfig.cmake.in │ ├── include │ │ └── fast │ │ │ ├── fast.h │ │ │ └── faster_corner_utilities.h │ └── CMakeLists.txt ├── g2o │ ├── README.txt │ ├── config.h │ ├── config.h.in │ ├── g2o │ │ ├── types │ │ │ ├── dquat2mat.h │ │ │ ├── g2o_types_slam3d_api.h │ │ │ ├── edge_se3_lotsofxyz.h │ │ │ ├── types_slam3d.h │ │ │ ├── isometry3d_gradients.cpp │ │ │ ├── se3_ops.h │ │ │ ├── CMakeLists.txt │ │ │ ├── types_sba.cpp │ │ │ ├── types_sba.h │ │ │ ├── parameter_stereo_camera.h │ │ │ ├── dquat2mat.cpp │ │ │ ├── types_slam3d.cpp │ │ │ ├── se3_ops.hpp │ │ │ ├── edge_pointxyz.cpp │ │ │ ├── edge_se3.h │ │ │ ├── parameter_stereo_camera.cpp │ │ │ ├── edge_se3_offset.h │ │ │ ├── edge_se3_prior.h │ │ │ ├── edge_se3_pointxyz_depth.h │ │ │ ├── parameter_camera.h │ │ │ └── edge_pointxyz.h │ │ ├── core │ │ │ ├── parameter.cpp │ │ │ ├── robust_kernel.cpp │ │ │ ├── base_vertex.hpp │ │ │ ├── parameter.h │ │ │ ├── optimization_algorithm_gauss_newton.h │ │ │ ├── optimization_algorithm.cpp │ │ │ ├── optimization_algorithm_property.h │ │ │ ├── matrix_structure.h │ │ │ ├── creators.h │ │ │ ├── solver.cpp │ │ │ ├── optimization_algorithm_with_hessian.h │ │ │ ├── parameter_container.h │ │ │ ├── robust_kernel.h │ │ │ ├── openmp_mutex.h │ │ │ ├── matrix_operations.h │ │ │ ├── jacobian_workspace.h │ │ │ ├── batch_stats.cpp │ │ │ ├── jacobian_workspace.cpp │ │ │ ├── sparse_block_matrix_test.cpp │ │ │ └── optimization_algorithm_dogleg.h │ │ └── stuff │ │ │ ├── os_specific.h │ │ │ ├── os_specific.c │ │ │ ├── color_macros.h │ │ │ └── property.cpp │ ├── license-bsd.txt │ └── cmake_modules │ │ └── FindEigen3.cmake └── DBoW2 │ ├── DUtils │ ├── config.h │ └── Random.cpp │ ├── README.txt │ ├── CMakeLists.txt │ ├── DBoW2 │ ├── FeatureVector.h │ ├── FClass.h │ ├── FORB.h │ ├── FeatureVector.cpp │ ├── BowVector.h │ ├── ScoringObject.h │ └── BowVector.cpp │ └── LICENSE.txt ├── examples ├── simimu │ └── matlabscripts │ │ ├── hat.m │ │ ├── vee.m │ │ ├── traj_gen.m │ │ ├── imu_sensor.m │ │ ├── t3.m │ │ ├── plotaxis.m │ │ └── generateRP.m ├── CMakeLists.txt ├── Kitti.yaml ├── EurocStereo.yaml ├── EuRoC.yaml ├── EurocStereoVIO.yaml └── EurocStereo.cpp ├── system ├── CMakeLists.txt ├── include │ └── ygz │ │ └── System.h └── src │ └── System.cpp ├── util ├── CMakeLists.txt └── include │ └── ygz │ └── EurocReader.h ├── install_dep.sh ├── common ├── CMakeLists.txt ├── src │ └── IMUData.cpp └── include │ └── ygz │ ├── Feature.h │ ├── IMUData.h │ ├── Camera.h │ ├── NumTypes.h │ └── sophus.hpp ├── cv ├── CMakeLists.txt └── include │ └── ygz │ ├── TrackerLK.h │ ├── Align.h │ └── LKFlow.h ├── generate.sh ├── cmake-modules ├── FindCSparse.cmake ├── FindCholmod.cmake ├── FindEigen3.cmake └── FindG2O.cmake ├── .gitignore ├── LICENSE └── README.md /backend/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( ygz-backend 2 | src/BackendSlidingWindowG2O.cpp 3 | ) -------------------------------------------------------------------------------- /Thirdparty/fast/test/data/test1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaoxiang12/ygz-stereo-inertial/HEAD/Thirdparty/fast/test/data/test1.png -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/hat.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaoxiang12/ygz-stereo-inertial/HEAD/examples/simimu/matlabscripts/hat.m -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/vee.m: -------------------------------------------------------------------------------- 1 | function y = vee( X ) 2 | %vee 实现so3的vee 3 | 4 | y = [X(3,2); X(1,3); X(2,1)]; 5 | 6 | end 7 | 8 | -------------------------------------------------------------------------------- /Thirdparty/fast/README.md: -------------------------------------------------------------------------------- 1 | FAST corner detector by Edward Rosten. 2 | The sourcefiles are from the CVD library: http://www.edwardrosten.com/cvd/ 3 | -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/traj_gen.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaoxiang12/ygz-stereo-inertial/HEAD/examples/simimu/matlabscripts/traj_gen.m -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/imu_sensor.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gaoxiang12/ygz-stereo-inertial/HEAD/examples/simimu/matlabscripts/imu_sensor.m -------------------------------------------------------------------------------- /system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(ygz-system 2 | src/System.cpp 3 | ) 4 | 5 | target_link_libraries( ygz-system 6 | ygz-common ygz-cv ygz-util ygz-backend 7 | ) -------------------------------------------------------------------------------- /util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library( ygz-util 2 | src/EurocReader.cpp 3 | src/Viewer.cpp 4 | ) 5 | 6 | target_link_libraries( ygz-util 7 | ${THIRD_PARTY_LIBS} 8 | ) -------------------------------------------------------------------------------- /install_dep.sh: -------------------------------------------------------------------------------- 1 | sudo apt-get install libopencv-dev libcxsparse3.1.4 libcholmod3.0.6 libsuitesparse-dev libeigen3-dev libqt4-dev qt4-qmake libgoogle-glog-dev libgtest-dev libgflags-dev 2 | 3 | -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/t3.m: -------------------------------------------------------------------------------- 1 | function y = t3( t ) 2 | %t3 3 | % y = [-0.1; -0.6*t; 0.2; 1+2*t; 0; -2]; 4 | % y = [-0.1*t; -0.8*t; 0.2; 3+2*t; 0; -2*t*t]; 5 | y = [sin(t); sin(t); cos(t); 6*sin(t); sin(t); 3*cos(t)]; 6 | end 7 | -------------------------------------------------------------------------------- /Thirdparty/g2o/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original g2o library at: https://github.com/RainerKuemmerle/g2o 3 | All files included in this g2o version are BSD, see license-bsd.txt 4 | -------------------------------------------------------------------------------- /common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(ygz-common 2 | src/IMUData.cpp 3 | src/Settings.cpp 4 | src/Frame.cpp 5 | src/MapPoint.cpp 6 | src/G2OTypes.cpp 7 | ) 8 | 9 | target_link_libraries(ygz-common 10 | ${THIRD_PARTY_LIBS} 11 | ) -------------------------------------------------------------------------------- /cv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(ygz-cv 2 | src/ORBExtractor.cpp 3 | src/ORBMatcher.cpp 4 | src/Tracker.cpp 5 | src/Align.cpp 6 | src/LKFlow.cpp 7 | src/TrackerLK.cpp 8 | ) 9 | 10 | target_link_libraries(ygz-cv ygz-common ${THIRD_PARTY_LIBS}) -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/config.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIG_H 2 | #define CONFIG_H 3 | 4 | #ifdef WIN32 5 | #ifndef DBoW2_EXPORTS 6 | #define DBoW2_EXPORTS 7 | #endif 8 | #ifdef DBoW2_EXPORTS 9 | #define EXPORT __declspec(dllexport) 10 | #else 11 | #define EXPORT __declspec(dllimport) 12 | #endif 13 | #else 14 | #define EXPORT 15 | #endif 16 | 17 | #endif // CONFIG_H 18 | -------------------------------------------------------------------------------- /generate.sh: -------------------------------------------------------------------------------- 1 | cd Thirdparty/DBoW2/ 2 | mkdir build 3 | cd build 4 | cmake .. -DCMAKE_BUILD_TYPE=Release 5 | make -j4 6 | 7 | cd ../../g2o 8 | mkdir build 9 | cd build 10 | cmake .. -DCMAKE_BUILD_TYPE=Release 11 | make -j4 12 | 13 | cd ../../fast 14 | mkdir build 15 | cd build 16 | cmake .. -DCMAKE_BUILD_TYPE=Release 17 | make -j2 18 | 19 | cd ../../.. 20 | mkdir build 21 | cd build 22 | cmake .. 23 | make -j4 24 | -------------------------------------------------------------------------------- /Thirdparty/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 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | /* #undef G2O_OPENMP */ 5 | /* #undef G2O_SHARED_LIBS */ 6 | /* #undef G2O_OPENGL_FOUND */ 7 | 8 | // give a warning if Eigen defaults to row-major matrices. 9 | // We internally assume column-major matrices throughout the code. 10 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 11 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 12 | #endif 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /Thirdparty/g2o/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef G2O_CONFIG_H 2 | #define G2O_CONFIG_H 3 | 4 | #cmakedefine G2O_OPENMP 1 5 | #cmakedefine G2O_SHARED_LIBS 1 6 | #cmakedefine G2O_OPENGL_FOUND 0 7 | 8 | // give a warning if Eigen defaults to row-major matrices. 9 | // We internally assume column-major matrices throughout the code. 10 | #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR 11 | # error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)" 12 | #endif 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(EurocStereo EurocStereo.cpp) 2 | target_link_libraries(EurocStereo 3 | ${YGZ_LIBS} 4 | ${THIRD_PARTY_LIBS} 5 | ) 6 | 7 | add_executable(EurocStereoVIO EurocStereoVIO.cpp) 8 | target_link_libraries(EurocStereoVIO 9 | ${YGZ_LIBS} 10 | ${THIRD_PARTY_LIBS} 11 | ) 12 | 13 | add_executable(Kitti Kitti.cpp) 14 | target_link_libraries(Kitti 15 | ${YGZ_LIBS} 16 | ${THIRD_PARTY_LIBS} 17 | ) 18 | -------------------------------------------------------------------------------- /Thirdparty/fast/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast 4 | 1.0.0 5 | 6 | A ROS wrapper for the FAST corner detector by Edward Rosten. 7 | The sourcefiles are from the CVD library: http://www.edwardrosten.com/cvd/ 8 | 9 | Christian Forster 10 | LGPL 11 | http://www.edwardrosten.com/work/fast.html 12 | catkin 13 | -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/plotaxis.m: -------------------------------------------------------------------------------- 1 | function plotaxis(Twi) 2 | % plot frame axis of pose Twi 3 | % Twi: pose 4 | hold on; xlabel('x');ylabel('y');zlabel('z'); 5 | Rwi = Twi(1:3,1:3); 6 | twi = Twi(1:3,4); 7 | 8 | oi = twi; 9 | axisx = Rwi*[1;0;0]; 10 | plot3([oi(1),oi(1)+axisx(1)],[oi(2),oi(2)+axisx(2)],[oi(3),oi(3)+axisx(3)],'r-'); 11 | axisy = Rwi*[0;1;0]; 12 | plot3([oi(1),oi(1)+axisy(1)],[oi(2),oi(2)+axisy(2)],[oi(3),oi(3)+axisy(3)],'g-'); 13 | axisz = Rwi*[0;0;1]; 14 | plot3([oi(1),oi(1)+axisz(1)],[oi(2),oi(2)+axisz(2)],[oi(3),oi(3)+axisz(3)],'b-'); 15 | 16 | end -------------------------------------------------------------------------------- /Thirdparty/fast/fastConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ####################################################### 2 | # Fast source dir 3 | set( fast_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ####################################################### 6 | # Fast build dir 7 | set( fast_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ####################################################### 10 | set( fast_INCLUDE_DIR "@fast_INCLUDE_DIR@" ) 11 | set( fast_INCLUDE_DIRS "@fast_INCLUDE_DIR@" ) 12 | 13 | set( fast_LIBRARIES "@fast_LIBRARIES@" ) 14 | set( fast_LIBRARY "@fast_LIBRARIES@" ) 15 | 16 | set( fast_LIBRARY_DIR "@fast_LIBRARY_DIR@" ) 17 | set( fast_LIBRARY_DIRS "@fast_LIBRARY_DIR@" ) 18 | -------------------------------------------------------------------------------- /cmake-modules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | ) 14 | 15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 16 | PATHS 17 | /usr/lib 18 | /usr/local/lib 19 | /opt/local/lib 20 | /sw/lib 21 | ) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 25 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 26 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/dquat2mat.h: -------------------------------------------------------------------------------- 1 | #ifndef _DQUAT2MAT_H_ 2 | #define _DQUAT2MAT_H_ 3 | #include 4 | #include "g2o_types_slam3d_api.h" 5 | 6 | namespace g2o { 7 | namespace internal { 8 | 9 | void G2O_TYPES_SLAM3D_API compute_dq_dR ( Eigen::Matrix& dq_dR , const double& r11 , const double& r21 , const double& r31 , const double& r12 , const double& r22 , const double& r32 , const double& r13 , const double& r23 , const double& r33 ); 10 | 11 | void G2O_TYPES_SLAM3D_API compute_dR_dq ( Eigen::Matrix& dR_dq , const double& qx , const double& qy , const double& qz , const double& qw ) ; 12 | } 13 | } 14 | #endif 15 | -------------------------------------------------------------------------------- /common/src/IMUData.cpp: -------------------------------------------------------------------------------- 1 | #include "ygz/Settings.h" 2 | #include "ygz/IMUData.h" 3 | 4 | 5 | namespace ygz { 6 | 7 | // covariance of measurements 8 | Matrix3d IMUData::mfGyrMeasCov = 9 | Matrix3d::Identity() * setting::gyrMeasError2; // sigma_g * sigma_g / dt, ~6e-6*10 10 | Matrix3d IMUData::mfAccMeasCov = Matrix3d::Identity() * setting::accMeasError2; 11 | 12 | // covariance of bias random walk 13 | Matrix3d IMUData::mfGyrBiasRWCov = 14 | Matrix3d::Identity() * setting::gyrBiasRw2; // sigma_gw * sigma_gw * dt, ~2e-12 15 | Matrix3d IMUData::mfAccBiasRWCov = 16 | Matrix3d::Identity() * setting::accBiasRw2; // sigma_aw * sigma_aw * dt, ~4.5e-8 17 | 18 | } -------------------------------------------------------------------------------- /examples/Kitti.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # This is for Kitti 04-12 3 | DatasetDir: /var/dataset/kitti/data_odometry_gray/sequences/08 4 | GroundTruth: /var/dataset/kitti/groundTruthPose/08.txt 5 | 6 | # use vision only 7 | PureVisionMode: true 8 | 9 | # do we need visualization? 10 | UseViewer: true 11 | 12 | # Camera calibration and distortion parameters (OpenCV) 13 | Camera.fx: 707.0912 14 | Camera.fy: 707.0912 15 | Camera.cx: 601.8873 16 | Camera.cy: 183.1104 17 | 18 | Camera.k1: 0.0 19 | Camera.k2: 0.0 20 | Camera.p1: 0.0 21 | Camera.p2: 0.0 22 | 23 | Camera.width: 1241 24 | Camera.height: 376 25 | 26 | # stereo baseline times fx 27 | Camera.bf: 379.8145 28 | 29 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 30 | Camera.RGB: 1 31 | 32 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | **.pyc 16 | CMakeCache.txt 17 | CMakeFiles 18 | Makefile 19 | cmake_install.cmake 20 | CMakeLists.txt.user 21 | install_manifest.txt 22 | /doc/html 23 | /doc/latex 24 | index.html 25 | 26 | *build* 27 | bin/* 28 | lib/ 29 | results/* 30 | cmake-build-debug/* 31 | 32 | *.kdev4 33 | Examples/Monocular/mono_euroc 34 | Examples/Monocular/mono_kitti 35 | Examples/Monocular/mono_tum 36 | Examples/Monocular/mono_euroc_vins 37 | Examples/RGB-D/rgbd_tum 38 | Examples/Stereo/stereo_euroc 39 | Examples/Stereo/stereo_kitti 40 | KeyFrameTrajectory.txt 41 | Examples/Stereo/stereo_kitti 42 | Examples/Stereo/stereo_euroc 43 | Examples/RGB-D/rgbd_tum 44 | *~ 45 | **/KeyFrameTrajectory.txt 46 | **/tmp/ 47 | *.bak 48 | callgrind* 49 | log 50 | 51 | .DS* 52 | .idea* 53 | 54 | debug.txt 55 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/g2o_types_slam3d_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef G2O_TYPES_SLAM3D_API_H 11 | #define G2O_TYPES_SLAM3D_API_H 12 | 13 | #include "./config.h" 14 | 15 | #ifdef _MSC_VER 16 | // We are using a Microsoft compiler: 17 | #ifdef G2O_SHARED_LIBS 18 | #ifdef types_slam3d_EXPORTS 19 | #define G2O_TYPES_SLAM3D_API __declspec(dllexport) 20 | #else 21 | #define G2O_TYPES_SLAM3D_API __declspec(dllimport) 22 | #endif 23 | #else 24 | #define G2O_TYPES_SLAM3D_API 25 | #endif 26 | 27 | #else 28 | // Not Microsoft compiler so set empty definition: 29 | #define G2O_TYPES_SLAM3D_API 30 | #endif 31 | 32 | #endif // G2O_TYPES_SLAM3D_API_H 33 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | # Set optimized building: 5 | if(MSVC_IDE) 6 | add_definitions(-D_CRT_SECURE_NO_WARNINGS) 7 | else(MSVC_IDE) 8 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 10 | endif(MSVC_IDE) 11 | 12 | set(HDRS_DBOW2 13 | DBoW2/BowVector.h 14 | DBoW2/FORB.h 15 | DBoW2/FClass.h 16 | DBoW2/FeatureVector.h 17 | DBoW2/ScoringObject.h 18 | DBoW2/TemplatedVocabulary.h) 19 | set(SRCS_DBOW2 20 | DBoW2/BowVector.cpp 21 | DBoW2/FORB.cpp 22 | DBoW2/FeatureVector.cpp 23 | DBoW2/ScoringObject.cpp) 24 | 25 | set(HDRS_DUTILS 26 | DUtils/Random.h 27 | DUtils/Timestamp.h) 28 | set(SRCS_DUTILS 29 | DUtils/Random.cpp 30 | DUtils/Timestamp.cpp) 31 | 32 | find_package(OpenCV REQUIRED) 33 | 34 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 35 | 36 | include_directories(${OpenCV_INCLUDE_DIRS}) 37 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 38 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 39 | 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2017 半闲居士 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /util/include/ygz/EurocReader.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_EUROC_READER_H 2 | #define YGZ_EUROC_READER_H 3 | 4 | #include "ygz/Settings.h" 5 | #include "ygz/NumTypes.h" 6 | #include "ygz/IMUData.h" 7 | 8 | // 一些用于EuRoC数据集的IO函数 9 | 10 | namespace ygz { 11 | 12 | 13 | // Load the stereo image data 14 | // 输入：左眼图像目录，右眼图像目录，时间戳文件 15 | // 输出：排序后左眼图像文件路径、右眼图像文件路径、时间戳 16 | bool LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes, 17 | vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps); 18 | 19 | // Load the IMU data 20 | bool LoadImus(const string &strImuPath, VecIMU &vImus); 21 | 22 | /** 23 | * Load the ground truth trajectory 24 | * @param [in] trajPath the path to trajectory, in euroc will be xxx/state_groundtruth_estimate0/data.csv 25 | * @param [out] the loaded trajectory 26 | * @return true if succeed 27 | */ 28 | typedef map, Eigen::aligned_allocator> TrajectoryType; 29 | 30 | bool LoadGroundTruthTraj(const string &trajPath, 31 | TrajectoryType &trajectory); 32 | } 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /common/include/ygz/Feature.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_FEATURE_H 2 | #define YGZ_FEATURE_H 3 | 4 | #include "ygz/NumTypes.h" 5 | #include "ygz/Settings.h" 6 | 7 | using namespace std; 8 | 9 | namespace ygz { 10 | 11 | // forward declare 12 | struct MapPoint; 13 | 14 | // Feature 是指一个图像点，参数化中有它的invDepth。没有关联地图点时，mpPoint为空 15 | struct Feature { 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 17 | Vector2f mPixel = Vector2f(0, 0); // the pixel position 18 | shared_ptr mpPoint = nullptr; // the corresponding map point, nullptr if not associated 19 | float mfInvDepth = -1; // inverse depth, invalid if less than zero. 20 | 21 | // data used in ORB 22 | float mScore = 0; // score, maybe harris or other things 23 | float mAngle = 0; // angle of oriented FAST 24 | size_t mLevel = 0; // the pyramid level 25 | uchar mDesc[32] = {0}; // 256 bits of ORB feature (32x8), ignored if using LK flow 26 | 27 | // flags 28 | bool mbOutlier = false; // true if it is an outlier 29 | }; 30 | } 31 | 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /backend/include/ygz/BackendInterface.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_BACKEND_H 2 | #define YGZ_BACKEND_H 3 | 4 | #include 5 | #include 6 | #include "ygz/NumTypes.h" 7 | 8 | /** 9 | * 后端的接口，具体函数请在子类实现 10 | * 后端需要做的事： 11 | * 1. 接受前端给出的Key-frame，有单独线程处理Keyframe-Mappoint的优化 12 | * 2. 管理地图点，向前端提供一个较好的局部地图 13 | * 3. 管理关键帧，删除时间久远的帧 14 | */ 15 | namespace ygz { 16 | 17 | // forward declare 18 | struct Frame; 19 | struct MapPoint; 20 | 21 | // this is the virtual interface class 22 | class BackendInterface { 23 | public: 24 | 25 | BackendInterface() {} 26 | 27 | virtual ~BackendInterface() {} 28 | 29 | // 插入新关键帧 30 | virtual int InsertKeyFrame(shared_ptr new_kf) =0; 31 | 32 | // 查询后端状态 33 | virtual bool IsBusy() =0; 34 | 35 | // 关闭后端 36 | virtual void Shutdown() =0; 37 | 38 | // 获取局部地图（前端追踪用） 39 | virtual std::set> GetLocalMap() =0; 40 | 41 | // 获取所有的关键帧 42 | virtual std::deque> GetAllKF() =0; 43 | 44 | virtual void Reset() =0; 45 | 46 | virtual void CallLocalBA() =0; 47 | 48 | virtual void OptimizeCurrent(shared_ptr current) =0; 49 | 50 | }; 51 | 52 | } 53 | 54 | #endif -------------------------------------------------------------------------------- /Thirdparty/fast/include/fast/fast.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_H 2 | #define FAST_H 3 | 4 | #include 5 | 6 | namespace fast 7 | { 8 | 9 | using ::std::vector; 10 | 11 | struct fast_xy 12 | { 13 | short x, y; 14 | fast_xy(short x_, short y_) : x(x_), y(y_) {} 15 | }; 16 | 17 | typedef unsigned char fast_byte; 18 | 19 | /// SSE2 optimized version of the corner 10 20 | void fast_corner_detect_10_sse2(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 21 | 22 | /// plain C++ version of the corner 10 23 | void fast_corner_detect_10(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 24 | 25 | /// corner score 10 26 | void fast_corner_score_10(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 27 | 28 | /// Nonmax Suppression on a 3x3 Window 29 | void fast_nonmax_3x3(const vector& corners, const vector& scores, vector& nonmax_corners); 30 | 31 | /// NEON optimized version of the corner 9 32 | void fast_corner_detect_9_neon(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 33 | 34 | } // namespace fast 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /common/include/ygz/IMUData.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_IMUDATA_H_ 2 | #define YGZ_IMUDATA_H_ 3 | 4 | #include "ygz/NumTypes.h" 5 | 6 | namespace ygz { 7 | 8 | using namespace Eigen; 9 | 10 | struct IMUData { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | IMUData(const double &gx, const double &gy, const double &gz, 15 | const double &ax, const double &ay, const double &az, 16 | const double &t) : 17 | mfGyro(gx, gy, gz), mfAcce(ax, ay, az), mfTimeStamp(t) {} 18 | 19 | IMUData(const Vector3d gyro, const Vector3d &acce, const double &time) 20 | : mfGyro(gyro), mfAcce(acce), mfTimeStamp(time) {} 21 | 22 | // Raw data of imu 23 | Vector3d mfGyro; //gyr data 24 | Vector3d mfAcce; //acc data 25 | double mfTimeStamp; //timestamp 26 | 27 | // covariance of measurement 28 | static Matrix3d mfGyrMeasCov; // 陀螺仪的协方差阵，是方差组成的对角阵 29 | static Matrix3d mfAccMeasCov; // 加速度计的协方差阵，是方差组成的对角阵 30 | 31 | // covariance of bias random walk, RW stands for random walk 32 | static Matrix3d mfGyrBiasRWCov; // 随机游走的协方差阵 33 | static Matrix3d mfAccBiasRWCov; // 加速度计随机游走的协方差阵 34 | 35 | }; 36 | 37 | typedef std::vector > VecIMU; 38 | } 39 | 40 | #endif // IMUDATA_H 41 | -------------------------------------------------------------------------------- /Thirdparty/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 | #include "../DUtils/config.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Vector of nodes with indexes of local features 23 | class EXPORT FeatureVector: 24 | public std::map > 25 | { 26 | public: 27 | 28 | /** 29 | * Constructor 30 | */ 31 | FeatureVector(void); 32 | 33 | /** 34 | * Destructor 35 | */ 36 | ~FeatureVector(void); 37 | 38 | /** 39 | * Adds a feature to an existing node, or adds a new node with an initial 40 | * feature 41 | * @param id node id to add or to modify 42 | * @param i_feature index of feature to add to the given node 43 | */ 44 | void addFeature(NodeId id, unsigned int i_feature); 45 | 46 | /** 47 | * Sends a string versions of the feature vector through the stream 48 | * @param out stream 49 | * @param v feature vector 50 | */ 51 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 52 | 53 | }; 54 | 55 | } // namespace DBoW2 56 | 57 | #endif 58 | 59 | -------------------------------------------------------------------------------- /common/include/ygz/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_CAMERA_H 2 | #define YGZ_CAMERA_H 3 | 4 | #include "ygz/NumTypes.h" 5 | 6 | namespace ygz { 7 | 8 | // the basic stereo pinhole camera 9 | struct CameraParam { 10 | 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | CameraParam(const float &_fx, const float &_fy, const float &_cx, const float &_cy, const float _bf = 0) 14 | : fx(_fx), fy(_fy), cx(_cx), cy(_cy), bf(_bf) { 15 | K << fx, 0, cx, 0, fy, cy, 0, 0, 1; 16 | fxinv = 1 / fx; 17 | fyinv = 1 / fy; 18 | Kinv = K.inverse(); 19 | f = (fx + fy) * 0.5; 20 | b = bf / f; 21 | } 22 | 23 | // from image pixel to camera point 24 | inline Vector3d Img2Cam(const Vector2f &px) { 25 | return Vector3d( 26 | fxinv * (px[0] - cx), 27 | fyinv * (px[1] - cy), 28 | 1 29 | ); 30 | } 31 | 32 | float fx = 0; 33 | float fy = 0; 34 | float fxinv = 0; 35 | float fyinv = 0; 36 | float cx = 0; 37 | float cy = 0; 38 | float b = 0; // baseline in stereo 39 | float f = 0; // focal length 40 | float bf = 0; // baseline*focal 41 | 42 | Matrix3f K = Matrix3f::Identity(); // intrinsics 43 | Matrix3f Kinv = Matrix3f::Identity(); // inverse K 44 | 45 | }; 46 | 47 | } 48 | 49 | #endif -------------------------------------------------------------------------------- /Thirdparty/g2o/license-bsd.txt: -------------------------------------------------------------------------------- 1 | g2o - General Graph Optimization 2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, 3 | Kurt Konolige, and Wolfram Burgard 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are 8 | met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_se3_lotsofxyz.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_SE3_LOTSOF_XYZ 2 | #define G2O_SE3_LOTSOF_XYZ 3 | 4 | #include "g2o/config.h" 5 | #include "g2o_types_slam3d_api.h" 6 | #include "g2o/core/base_multi_edge.h" 7 | #include "vertex_se3.h" 8 | #include "vertex_pointxyz.h" 9 | 10 | namespace g2o{ 11 | 12 | class G2O_TYPES_SLAM3D_API EdgeSE3LotsOfXYZ : public BaseMultiEdge<-1, VectorXD>{ 13 | 14 | protected: 15 | unsigned int _observedPoints; 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 19 | EdgeSE3LotsOfXYZ(); 20 | 21 | void setDimension(int dimension_){ 22 | _dimension = dimension_; 23 | _information.resize(dimension_, dimension_); 24 | _error.resize(dimension_, 1); 25 | _measurement.resize(dimension_, 1); 26 | } 27 | 28 | void setSize(int vertices){ 29 | resize(vertices); 30 | _observedPoints = vertices-1; 31 | setDimension(_observedPoints*3); 32 | } 33 | 34 | 35 | virtual void computeError(); 36 | 37 | virtual bool read(std::istream& is); 38 | virtual bool write(std::ostream& os) const; 39 | 40 | virtual bool setMeasurementFromState(); 41 | 42 | virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 43 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); 44 | 45 | virtual void linearizeOplus(); 46 | 47 | }; 48 | 49 | } 50 | 51 | 52 | 53 | #endif // G2O_SE3_LOTSOF_XYZ 54 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_slam3d.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef G2O_TYPES_SLAM3D_ 18 | #define G2O_TYPES_SLAM3D_ 19 | 20 | #include "g2o/config.h" 21 | #include "g2o/core/base_vertex.h" 22 | #include "g2o/core/base_binary_edge.h" 23 | #include "g2o/core/hyper_graph_action.h" 24 | 25 | #define THREE_D_TYPES_ANALYTIC_JACOBIAN 26 | 27 | #include "vertex_se3.h" 28 | #include "edge_se3.h" 29 | #include "vertex_pointxyz.h" 30 | 31 | #include "parameter_se3_offset.h" 32 | #include "edge_se3_pointxyz.h" 33 | #include "edge_se3_offset.h" 34 | 35 | #include "parameter_camera.h" 36 | #include "parameter_stereo_camera.h" 37 | #include "edge_se3_pointxyz_disparity.h" 38 | #include "edge_se3_pointxyz_depth.h" 39 | #include "edge_se3_prior.h" 40 | 41 | #include "edge_pointxyz.h" 42 | 43 | #include "edge_se3_lotsofxyz.h" 44 | #endif 45 | -------------------------------------------------------------------------------- /system/include/ygz/System.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_SYSTEM_H 2 | #define YGZ_SYSTEM_H 3 | 4 | #include "ygz/NumTypes.h" 5 | #include "ygz/Tracker.h" 6 | #include "ygz/BackendInterface.h" 7 | #include "ygz/Viewer.h" 8 | 9 | // the interface of full system 10 | 11 | namespace ygz { 12 | 13 | class System { 14 | 15 | public: 16 | System(const string &configPath); 17 | 18 | ~System(); 19 | 20 | /** 21 | * insert a stereo image and imu data 22 | * @param imRectLeft the rectified left image 23 | * @param imRectRight the rectified right image 24 | * @param timestamp 25 | * @param vimu IMU data before this image 26 | * @return the Twb pose of the current stereo image 27 | */ 28 | SE3d AddStereoIMU(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp, 29 | const VecIMU &vimu); 30 | 31 | /** 32 | * insert stereo images 33 | * @param imRectLeft 34 | * @param imRectright 35 | * @param timestamp 36 | * @return 37 | */ 38 | SE3d AddStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectright, const double ×tamp); 39 | 40 | // 设置真实轨迹 41 | void SetGroundTruthTrajectory(map, Eigen::aligned_allocator> &traj); 42 | 43 | void Shutdown(); 44 | 45 | public: 46 | 47 | shared_ptr mpTracker = nullptr; 48 | shared_ptr mpBackend = nullptr; 49 | shared_ptr mpViewer = nullptr; 50 | 51 | }; 52 | 53 | } 54 | 55 | #endif -------------------------------------------------------------------------------- /examples/simimu/matlabscripts/generateRP.m: -------------------------------------------------------------------------------- 1 | 2 | clear; 3 | close all; 4 | clc; 5 | 6 | [rr, pp] = traj_gen(@t3); 7 | 8 | F = 200; 9 | gw = [0;0;9.8]; 10 | times = 0:1/F:10; 11 | N = length(times); 12 | [omega, acc] = imu_sensor( rr, pp, times, gw); 13 | 14 | wnstd = 2e-4*sqrt(F)*randn(size(omega)); 15 | anstd = 2e-3*sqrt(F)*randn(size(acc)); 16 | 17 | r = zeros(9,N); 18 | p = zeros(3,N); 19 | figure; hold on; 20 | for i=1:N 21 | R = rr(times(i)); 22 | r(:,i) = R(:); 23 | p(:,i) = pp(times(i)); 24 | T = eye(4); 25 | T(1:3,1:3) = R; 26 | T(1:3,4) = p(:,i); 27 | if(mod(i,20)==0) 28 | plotaxis(T); 29 | end; 30 | end; 31 | 32 | ofimu = fopen('imu.txt','w'); 33 | for i=1:N 34 | fprintf(ofimu, '%.6f %.6f %.6f %.6f %.6f %.6f %.6f \n',... 35 | times(i),omega(1,i),omega(2,i),omega(3,i),... 36 | acc(1,i),acc(2,i),acc(3,i)); 37 | end; 38 | fclose(ofimu); 39 | 40 | ofrot = fopen('rot.txt','w'); 41 | for i=1:N 42 | fprintf(ofrot, '%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f \n',... 43 | times(i),r(1,i),r(2,i),r(3,i),r(4,i),... 44 | r(5,i),r(6,i),r(7,i),r(8,i),r(9,i)); 45 | end; 46 | fclose(ofrot); 47 | 48 | ofpos = fopen('pos.txt','w'); 49 | for i=1:N 50 | fprintf(ofpos, '%.6f %.6f %.6f %.6f \n',... 51 | times(i),p(1,i),p(2,i),p(3,i)); 52 | end; 53 | fclose(ofpos); 54 | 55 | ofimunoise = fopen('imunoise.txt','w'); 56 | for i=1:N 57 | fprintf(ofimunoise, '%.6f %.6f %.6f %.6f %.6f %.6f %.6f \n',... 58 | times(i),wnstd(1,i),wnstd(2,i),wnstd(3,i),... 59 | anstd(1,i),anstd(2,i),anstd(3,i)); 60 | end; 61 | fclose(ofimunoise); 62 | -------------------------------------------------------------------------------- /cv/include/ygz/TrackerLK.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_TRACKER_LK_H 2 | #define YGZ_TRACKER_LK_H 3 | 4 | /** 5 | * Tracked implemented by LK flow (like VINS) 6 | */ 7 | 8 | #include "ygz/Tracker.h" 9 | 10 | namespace ygz { 11 | 12 | class TrackerLK : public Tracker { 13 | public: 14 | 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 16 | 17 | TrackerLK(const string &settingFile); 18 | 19 | TrackerLK(); 20 | 21 | virtual ~TrackerLK() {} 22 | 23 | // 向Tracker插入一对新的图像，返回估计的位姿 Twb 24 | // override the ORB tracker 25 | virtual SE3d InsertStereo( 26 | const cv::Mat &imRectLeft, const cv::Mat &imRectRight, // 左右两图 27 | const double ×tamp, // 时间 28 | const VecIMU &vimu) override; // 自上个帧采集到的IMU 29 | protected: 30 | 31 | /** 32 | * 实际的Track函数 33 | */ 34 | virtual void Track() override; 35 | 36 | /** 37 | * use lk optical flow to track the features in last frame 38 | * @return 39 | */ 40 | virtual bool TrackLastFrame(bool usePoseInfo = false) override; 41 | 42 | /** 43 | * Track local map using image alignment 44 | */ 45 | virtual bool TrackLocalMap(int &inliers) override; 46 | 47 | // reset 48 | virtual void Reset() override; 49 | 50 | // Create stereo matched map points 51 | void CreateStereoMapPoints(); 52 | 53 | // Clean the old features 54 | void CleanOldFeatures(); 55 | 56 | SE3d mSpeed; // speed, used to predict the currrent pose 57 | public: 58 | void TestTrackerLK(); 59 | }; 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter.h" 28 | 29 | namespace g2o { 30 | 31 | Parameter::Parameter() : _id(-1) 32 | { 33 | } 34 | 35 | void Parameter::setId(int id_) 36 | { 37 | _id = id_; 38 | } 39 | 40 | } // end namespace 41 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/isometry3d_gradients.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "isometry3d_gradients.h" 28 | #include 29 | namespace g2o { 30 | using namespace std; 31 | using namespace Eigen; 32 | 33 | namespace internal { 34 | #include "dquat2mat.cpp" 35 | } // end namespace internal 36 | 37 | } // end namespace 38 | -------------------------------------------------------------------------------- /cv/include/ygz/Align.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_ALIGN_H_ 2 | #define YGZ_ALIGN_H_ 3 | 4 | #include "ygz/Settings.h" 5 | #include "ygz/NumTypes.h" 6 | 7 | // 有关align部分的算法 8 | // This part is moved from rpg_SVO with modification to support ygz 9 | 10 | namespace ygz { 11 | 12 | const int align_halfpatch_size = 4; 13 | const int align_patch_size = 8; 14 | const int align_patch_area = 64; 15 | 16 | /** 17 | * @brief align a pixel with reference image patch 18 | * 二维对齐 19 | * @param[in] cur_img The current image 20 | * @param[in] ref_patch_with_boarder the patch with boarder, used to compute the gradient (or FEJ) 21 | * @param[in] ref_patch the patch in reference frame, by default is 64x64 22 | * @param[in] n_iter maximum iterations 23 | * @param[out] cur_px_estimate the estimated position in current image, must have an initial value 24 | * @return True if successful 25 | */ 26 | bool Align2D( 27 | const cv::Mat &cur_img, 28 | uint8_t *ref_patch_with_border, 29 | uint8_t *ref_patch, 30 | const int n_iter, 31 | Vector2f &cur_px_estimate); 32 | 33 | /** 34 | * 一维对齐 35 | * @param cur_img 当前帧图像 36 | * @param dir 方向 37 | * @param ref_patch_with_border 带边界的图像块 38 | * @param ref_patch 不带边界的图像块 39 | * @param n_iter 迭代次数 40 | * @param cur_px_estimate 当前点估计 41 | * @param h_inv 42 | * @return 43 | */ 44 | bool Align1D( 45 | const cv::Mat &cur_img, 46 | const Eigen::Vector2f &dir, // direction in which the patch is allowed to move 47 | uint8_t *ref_patch_with_border, 48 | uint8_t *ref_patch, 49 | const int n_iter, 50 | Vector2f &cur_px_estimate, 51 | double &h_inv); 52 | } 53 | 54 | #endif -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "robust_kernel.h" 28 | 29 | namespace g2o { 30 | 31 | RobustKernel::RobustKernel() : 32 | _delta(1.) 33 | { 34 | } 35 | 36 | RobustKernel::RobustKernel(double delta) : 37 | _delta(delta) 38 | { 39 | } 40 | 41 | void RobustKernel::setDelta(double delta) 42 | { 43 | _delta = delta; 44 | } 45 | 46 | } // end namespace g2o 47 | -------------------------------------------------------------------------------- /Thirdparty/fast/include/fast/faster_corner_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_CORNER_UTILITIES_H 2 | #define FAST_CORNER_UTILITIES_H 3 | 4 | #if __ARM_NEON__ 5 | #include 6 | #elif __SSE2__ 7 | #include 8 | #endif 9 | 10 | namespace fast 11 | { 12 | 13 | /// Check if the pointer is aligned to the specified byte granularity 14 | template bool is_aligned(const void* ptr); 15 | template<> inline bool is_aligned<8>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0x7) == 0; } 16 | template<> inline bool is_aligned<16>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0xF) == 0; } 17 | 18 | 19 | struct Less 20 | { 21 | template static bool eval(const T1 a, const T2 b) 22 | { 23 | return a < b; 24 | } 25 | static short prep_t(short pixel_val, short barrier) 26 | { 27 | return pixel_val - barrier; 28 | } 29 | }; 30 | 31 | struct Greater 32 | { 33 | template static bool eval(const T1 a, const T2 b) 34 | { 35 | return a > b; 36 | } 37 | static short prep_t(short pixel_val, short barrier) 38 | { 39 | return pixel_val + barrier; 40 | } 41 | }; 42 | 43 | #if __SSE2__ 44 | 45 | #define CHECK_BARRIER(lo, hi, other, flags) \ 46 | { \ 47 | __m128i diff = _mm_subs_epu8(lo, other); \ 48 | __m128i diff2 = _mm_subs_epu8(other, hi); \ 49 | __m128i z = _mm_setzero_si128(); \ 50 | diff = _mm_cmpeq_epi8(diff, z); \ 51 | diff2 = _mm_cmpeq_epi8(diff2, z); \ 52 | flags = ~(_mm_movemask_epi8(diff) | (_mm_movemask_epi8(diff2) << 16)); \ 53 | } 54 | 55 | template inline __m128i load_si128(const void* addr) { return _mm_loadu_si128((const __m128i*)addr); } 56 | template <> inline __m128i load_si128(const void* addr) { return _mm_load_si128((const __m128i*)addr); } 57 | 58 | #endif 59 | 60 | } // namespace fast 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /Thirdparty/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 | #include "../DUtils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Generic class to encapsulate functions to manage descriptors. 22 | /** 23 | * This class must be inherited. Derived classes can be used as the 24 | * parameter F when creating Templated structures 25 | * (TemplatedVocabulary, TemplatedDatabase, ...) 26 | */ 27 | class EXPORT FClass 28 | { 29 | class TDescriptor; 30 | typedef const TDescriptor *pDescriptor; 31 | 32 | /** 33 | * Calculates the mean value of a set of descriptors 34 | * @param descriptors 35 | * @param mean mean descriptor 36 | */ 37 | virtual void meanValue(const std::vector &descriptors, 38 | TDescriptor &mean) = 0; 39 | 40 | /** 41 | * Calculates the distance between two descriptors 42 | * @param a 43 | * @param b 44 | * @return distance 45 | */ 46 | static double distance(const TDescriptor &a, const TDescriptor &b); 47 | 48 | /** 49 | * Returns a string version of the descriptor 50 | * @param a descriptor 51 | * @return string version 52 | */ 53 | static std::string toString(const TDescriptor &a); 54 | 55 | /** 56 | * Returns a descriptor from a string 57 | * @param a descriptor 58 | * @param s string version 59 | */ 60 | static void fromString(TDescriptor &a, const std::string &s); 61 | 62 | /** 63 | * Returns a mat with the descriptors in float format 64 | * @param descriptors 65 | * @param mat (out) NxL 32F matrix 66 | */ 67 | static void toMat32F(const std::vector &descriptors, 68 | cv::Mat &mat); 69 | }; 70 | 71 | } // namespace DBoW2 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATH_STUFF 28 | #define G2O_MATH_STUFF 29 | 30 | #include 31 | #include 32 | 33 | namespace g2o { 34 | using namespace Eigen; 35 | 36 | inline Matrix3d skew(const Vector3d&v); 37 | inline Vector3d deltaR(const Matrix3d& R); 38 | inline Vector2d project(const Vector3d&); 39 | inline Vector3d project(const Vector4d&); 40 | inline Vector3d unproject(const Vector2d&); 41 | inline Vector4d unproject(const Vector3d&); 42 | 43 | #include "se3_ops.hpp" 44 | 45 | } 46 | 47 | #endif //MATH_STUFF 48 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(types_slam3d ${G2O_LIB_TYPE} 2 | dquat2mat.cpp dquat2mat.h 3 | isometry3d_mappings.h 4 | isometry3d_mappings.cpp 5 | isometry3d_gradients.cpp 6 | isometry3d_gradients.h 7 | vertex_se3.cpp 8 | vertex_se3.h 9 | parameter_se3_offset.cpp 10 | parameter_se3_offset.h 11 | edge_se3.cpp 12 | edge_se3.h 13 | edge_se3_offset.cpp 14 | edge_se3_offset.h 15 | vertex_pointxyz.cpp 16 | vertex_pointxyz.h 17 | parameter_camera.cpp 18 | parameter_camera.h 19 | parameter_stereo_camera.cpp 20 | parameter_stereo_camera.h 21 | edge_se3_pointxyz.cpp 22 | edge_se3_pointxyz.h 23 | edge_se3_pointxyz_disparity.cpp 24 | edge_se3_pointxyz_disparity.h 25 | edge_se3_pointxyz_depth.cpp 26 | edge_se3_pointxyz_depth.h 27 | edge_se3_prior.cpp 28 | edge_se3_prior.h 29 | edge_se3_lotsofxyz.cpp 30 | edge_se3_lotsofxyz.h 31 | se3quat.h 32 | se3_ops.h se3_ops.hpp 33 | edge_pointxyz.cpp edge_pointxyz.h 34 | types_slam3d.cpp 35 | types_slam3d.h 36 | ) 37 | 38 | 39 | SET_TARGET_PROPERTIES(types_slam3d PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_slam3d) 40 | TARGET_LINK_LIBRARIES(types_slam3d core) 41 | IF(G2O_HAVE_OPENGL) 42 | TARGET_LINK_LIBRARIES(types_slam3d opengl_helper ${OPENGL_gl_LIBRARY} ) 43 | ENDIF() 44 | 45 | ADD_EXECUTABLE(test_isometry3d_mappings test_isometry3d_mappings.cpp) 46 | TARGET_LINK_LIBRARIES(test_isometry3d_mappings types_slam3d) 47 | 48 | ADD_EXECUTABLE(test_mat2quat_jacobian test_mat2quat_jacobian.cpp) 49 | TARGET_LINK_LIBRARIES(test_mat2quat_jacobian types_slam3d) 50 | 51 | ADD_EXECUTABLE(test_slam3d_jacobian test_slam3d_jacobian.cpp) 52 | TARGET_LINK_LIBRARIES(test_slam3d_jacobian types_slam3d) 53 | 54 | INSTALL(TARGETS types_slam3d 55 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 56 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 57 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 58 | ) 59 | 60 | FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 61 | INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/types/slam3d) 62 | -------------------------------------------------------------------------------- /Thirdparty/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 | #include "../DUtils/config.h" 20 | 21 | namespace DBoW2 { 22 | 23 | /// Functions to manipulate ORB descriptors 24 | class EXPORT FORB : protected FClass 25 | { 26 | public: 27 | 28 | /// Descriptor type 29 | typedef cv::Mat TDescriptor; // CV_8U 30 | /// Pointer to a single descriptor 31 | typedef const TDescriptor *pDescriptor; 32 | /// Descriptor length (in bytes) 33 | static const int L = 32; 34 | 35 | /** 36 | * Calculates the mean value of a set of descriptors 37 | * @param descriptors 38 | * @param mean mean descriptor 39 | */ 40 | static void meanValue(const std::vector &descriptors, 41 | TDescriptor &mean); 42 | 43 | /** 44 | * Calculates the distance between two descriptors 45 | * @param a 46 | * @param b 47 | * @return distance 48 | */ 49 | static int distance(const TDescriptor &a, const TDescriptor &b); 50 | 51 | /** 52 | * Returns a string version of the descriptor 53 | * @param a descriptor 54 | * @return string version 55 | */ 56 | static std::string toString(const TDescriptor &a); 57 | 58 | /** 59 | * Returns a descriptor from a string 60 | * @param a descriptor 61 | * @param s string version 62 | */ 63 | static void fromString(TDescriptor &a, const std::string &s); 64 | 65 | /** 66 | * Returns a mat with the descriptors in float format 67 | * @param descriptors 68 | * @param mat (out) NxL 32F matrix 69 | */ 70 | static void toMat32F(const std::vector &descriptors, 71 | cv::Mat &mat); 72 | 73 | static void toMat8U(const std::vector &descriptors, 74 | cv::Mat &mat); 75 | 76 | }; 77 | 78 | } // namespace DBoW2 79 | 80 | #endif 81 | 82 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OS_SPECIFIC_HH_ 28 | #define G2O_OS_SPECIFIC_HH_ 29 | 30 | #ifdef WINDOWS 31 | #include 32 | #include 33 | #include 34 | #ifndef _WINDOWS 35 | #include 36 | #endif 37 | #define drand48() ((double) rand()/(double)RAND_MAX) 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | int vasprintf(char** strp, const char* fmt, va_list ap); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | 49 | #endif 50 | 51 | #ifdef UNIX 52 | #include 53 | // nothing to do on real operating systems 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "types_sba.h" 28 | #include 29 | 30 | namespace g2o { 31 | 32 | using namespace std; 33 | 34 | 35 | VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() 36 | { 37 | } 38 | 39 | bool VertexSBAPointXYZ::read(std::istream& is) 40 | { 41 | Vector3d lv; 42 | for (int i=0; i<3; i++) 43 | is >> _estimate[i]; 44 | return true; 45 | } 46 | 47 | bool VertexSBAPointXYZ::write(std::ostream& os) const 48 | { 49 | Vector3d lv=estimate(); 50 | for (int i=0; i<3; i++){ 51 | os << lv[i] << " "; 52 | } 53 | return os.good(); 54 | } 55 | 56 | } // end namespace 57 | -------------------------------------------------------------------------------- /cv/include/ygz/LKFlow.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_LK_FLOW_H 2 | #define YGZ_LK_FLOW_H 3 | 4 | #include "ygz/Settings.h" 5 | #include "ygz/Frame.h" 6 | 7 | namespace ygz { 8 | 9 | /** 10 | * 使用LK光流跟踪ref中的特征点 11 | * @param ref 参考帧 12 | * @param current 当前帧 13 | * @param trackPts 参考帧中每个点在当前帧中的2D坐标，小于零表示追踪失败 14 | * @param keepNotConverged 是否保留未收敛点的估计值 15 | * @return 成功追踪点的数量 16 | */ 17 | int LKFlow( 18 | const shared_ptr ref, 19 | const shared_ptr current, 20 | VecVector2f &trackPts, 21 | bool keepNotConverged = true 22 | ); 23 | 24 | /** 25 | * 对单个点进行LK追踪，给定两个金字塔 26 | * @param pyramid1 27 | * @param pyramid2 28 | * @param pixel1 29 | * @param pixel2 30 | * @return 31 | */ 32 | bool LKFlowSinglePoint( 33 | const vector &pyramid1, 34 | const vector &pyramid2, 35 | const Vector2f &pixel1, 36 | Vector2f &pixel2 37 | ); 38 | 39 | /** 40 | * 一维的光流，用于左右目的匹配（但是对校正要求太高，不太现实） 41 | * @param frame 42 | * @return 43 | */ 44 | int LKFlow1D( 45 | const shared_ptr frame 46 | ); 47 | 48 | /** 49 | * OpenCV's lk flow 50 | * 51 | */ 52 | int LKFlowCV( 53 | const shared_ptr ref, 54 | const shared_ptr current, 55 | VecVector2f &refPts, 56 | VecVector2f &trackedPts 57 | ); 58 | 59 | /** 60 | * 双线性插值 61 | * @param x 62 | * @param y 63 | * @param gray 64 | * @return 65 | */ 66 | inline uchar GetBilateralInterpUchar( 67 | const float &x, const float &y, const Mat &gray) { 68 | const float xx = x - floor(x); 69 | const float yy = y - floor(y); 70 | uchar *data = gray.data + int(y) * gray.step + int(x); 71 | return uchar( 72 | (1 - xx) * (1 - yy) * data[0] + 73 | xx * (1 - yy) * data[1] + 74 | (1 - xx) * yy * data[gray.step] + 75 | xx * yy * data[gray.step + 1] 76 | ); 77 | } 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /Thirdparty/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 | -------------------------------------------------------------------------------- /Thirdparty/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 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_sba.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 Kurt Konolige 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_SBA_TYPES 28 | #define G2O_SBA_TYPES 29 | 30 | #include "../core/base_vertex.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o { 36 | 37 | /** 38 | * \brief Point vertex, XYZ 39 | */ 40 | class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | VertexSBAPointXYZ(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | virtual void setToOriginImpl() { 49 | _estimate.fill(0.); 50 | } 51 | 52 | virtual void oplusImpl(const double* update) 53 | { 54 | Eigen::Map v(update); 55 | _estimate += v; 56 | } 57 | }; 58 | 59 | } // end namespace 60 | 61 | #endif // SBA_TYPES 62 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/os_specific.c: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "os_specific.h" 28 | 29 | #ifdef WINDOWS 30 | 31 | int vasprintf(char** strp, const char* fmt, va_list ap) 32 | { 33 | int n; 34 | int size = 100; 35 | char* p; 36 | char* np; 37 | 38 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 39 | return -1; 40 | 41 | while (1) { 42 | #ifdef _MSC_VER 43 | n = vsnprintf_s(p, size, size - 1, fmt, ap); 44 | #else 45 | n = vsnprintf(p, size, fmt, ap); 46 | #endif 47 | if (n > -1 && n < size) { 48 | *strp = p; 49 | return n; 50 | } 51 | if (n > -1) 52 | size = n+1; 53 | else 54 | size *= 2; 55 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 56 | free(p); 57 | return -1; 58 | } else 59 | p = np; 60 | } 61 | } 62 | 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/parameter_stereo_camera.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_STEREO_CAMERA_PARAMETERS_H_ 28 | #define G2O_STEREO_CAMERA_PARAMETERS_H_ 29 | 30 | #include "g2o/core/hyper_graph_action.h" 31 | #include "parameter_camera.h" 32 | #include "g2o_types_slam3d_api.h" 33 | 34 | namespace g2o { 35 | /** 36 | * \brief parameters for a camera 37 | */ 38 | class G2O_TYPES_SLAM3D_API ParameterStereoCamera: public ParameterCamera { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | ParameterStereoCamera(); 42 | 43 | virtual bool read(std::istream& is); 44 | virtual bool write(std::ostream& os) const; 45 | 46 | void setBaseline(double baseline_) { _baseline = baseline_;} 47 | double baseline() const {return _baseline;} 48 | protected: 49 | double _baseline; 50 | }; 51 | }; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/dquat2mat.cpp: -------------------------------------------------------------------------------- 1 | #include "dquat2mat.h" 2 | #include 3 | namespace g2o { 4 | namespace internal { 5 | using namespace std; 6 | 7 | #include "dquat2mat_maxima_generated.cpp" 8 | 9 | int _q2m(double& S, double& qw, const double& r00 , const double& r10 , const double& r20 , const double& r01 , const double& r11 , const double& r21 , const double& r02 , const double& r12 , const double& r22 ){ 10 | double tr=r00 + r11 + r22; 11 | if (tr > 0) { 12 | S = sqrt(tr + 1.0) * 2; // S=4*qw 13 | qw = 0.25 * S; 14 | // qx = (r21 - r12) / S; 15 | // qy = (r02 - r20) / S; 16 | // qz = (r10 - r01) / S; 17 | return 0; 18 | } else if ((r00 > r11)&(r00 > r22)) { 19 | S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx 20 | qw = (r21 - r12) / S; 21 | // qx = 0.25 * S; 22 | // qy = (r01 + r10) / S; 23 | // qz = (r02 + r20) / S; 24 | return 1; 25 | } else if (r11 > r22) { 26 | S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy 27 | qw = (r02 - r20) / S; 28 | // qx = (r01 + r10) / S; 29 | // qy = 0.25 * S; 30 | // qz = (r12 + r21) / S; 31 | return 2; 32 | } else { 33 | S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz 34 | qw = (r10 - r01) / S; 35 | // qx = (r02 + r20) / S; 36 | // qy = (r12 + r21) / S; 37 | // qz = 0.25 * S; 38 | return 3; 39 | } 40 | } 41 | 42 | void compute_dq_dR ( Eigen::Matrix& dq_dR , const double& r11 , const double& r21 , const double& r31 , const double& r12 , const double& r22 , const double& r32 , const double& r13 , const double& r23 , const double& r33 ){ 43 | double qw; 44 | double S; 45 | int whichCase=_q2m( S, qw, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 46 | S*=.25; 47 | switch(whichCase){ 48 | case 0: compute_dq_dR_w(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 49 | break; 50 | case 1: compute_dq_dR_x(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 51 | break; 52 | case 2: compute_dq_dR_y(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 53 | break; 54 | case 3: compute_dq_dR_z(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 55 | break; 56 | } 57 | if (qw<=0) 58 | dq_dR *= -1; 59 | } 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /common/include/ygz/NumTypes.h: -------------------------------------------------------------------------------- 1 | #ifndef YGZ_NUM_TYPES_H 2 | #define YGZ_NUM_TYPES_H 3 | 4 | // for Eigen 5 | #include 6 | #include // linear algebra 7 | #include 8 | 9 | #include "so3.hpp" 10 | #include "se3.hpp" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | using Sophus::SO3f; 17 | using Sophus::SE3f; 18 | using Sophus::SO3d; 19 | using Sophus::SE3d; 20 | 21 | namespace ygz { 22 | using Eigen::Vector2d; 23 | using Eigen::Vector3d; 24 | using Eigen::Vector2f; 25 | using Eigen::Vector3f; 26 | using Eigen::Vector4f; 27 | 28 | using Eigen::Matrix2f; 29 | using Eigen::Matrix3f; 30 | using Eigen::Matrix4f; 31 | using Eigen::Matrix2d; 32 | using Eigen::Matrix3d; 33 | using Eigen::Matrix4d; 34 | using Eigen::Quaternionf; 35 | 36 | // other things I need in optimiztion 37 | typedef Eigen::Matrix Vector6d; 38 | typedef Eigen::Matrix Vector6f; 39 | 40 | typedef Eigen::Matrix Vector9d; 41 | typedef Eigen::Matrix Vector9f; 42 | 43 | typedef Eigen::Matrix Matrix15f; 44 | typedef Eigen::Matrix Matrix15d; 45 | 46 | typedef Eigen::Matrix SpeedAndBias; 47 | 48 | typedef Eigen::Matrix Matrix9d; 49 | typedef Eigen::Matrix Matrix9f; 50 | 51 | typedef Eigen::Matrix Matrix6d; 52 | typedef Eigen::Matrix Matrix6f; 53 | 54 | typedef Eigen::Matrix Matrix26d; 55 | typedef Eigen::Matrix Matrix26f; 56 | 57 | typedef unsigned char uchar; 58 | 59 | // vector of Eigen things 60 | typedef std::vector> VecVector3d; 61 | typedef std::vector> VecVector3f; 62 | typedef std::vector> VecVector2d; 63 | typedef std::vector> VecVector2f; 64 | 65 | typedef std::vector> VecSE3f; 66 | typedef std::vector> VecSE3d; 67 | typedef std::vector> VecSO3f; 68 | typedef std::vector> VecSO3d; 69 | 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/base_vertex.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | template 28 | BaseVertex::BaseVertex() : 29 | OptimizableGraph::Vertex(), 30 | _hessian(0, D, D) 31 | { 32 | _dimension = D; 33 | } 34 | 35 | template 36 | double BaseVertex::solveDirect(double lambda) { 37 | Matrix tempA=_hessian + Matrix ::Identity()*lambda; 38 | double det=tempA.determinant(); 39 | if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) 40 | return det; 41 | Matrix dx=tempA.llt().solve(_b); 42 | oplus(&dx[0]); 43 | return det; 44 | } 45 | 46 | template 47 | void BaseVertex::clearQuadraticForm() { 48 | _b.setZero(); 49 | } 50 | 51 | template 52 | void BaseVertex::mapHessianMemory(double* d) 53 | { 54 | new (&_hessian) HessianBlockType(d, D, D); 55 | } 56 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_HH_ 28 | #define G2O_GRAPH_PARAMETER_HH_ 29 | 30 | #include 31 | 32 | #include "hyper_graph.h" 33 | 34 | namespace g2o { 35 | 36 | class Parameter : public HyperGraph::HyperGraphElement 37 | { 38 | public: 39 | Parameter(); 40 | virtual ~Parameter() {}; 41 | //! read the data from a stream 42 | virtual bool read(std::istream& is) = 0; 43 | //! write the data to a stream 44 | virtual bool write(std::ostream& os) const = 0; 45 | int id() const {return _id;} 46 | void setId(int id_); 47 | virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} 48 | protected: 49 | int _id; 50 | }; 51 | 52 | typedef std::vector ParameterVector; 53 | 54 | } // end namespace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /Thirdparty/fast/test/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main (int argc, char * argv[]) { 8 | const int n_trials = 1000; 9 | std::vector corners; 10 | cv::Mat img = cv::imread(std::string(TEST_DATA_DIR) + "/test1.png", 0); 11 | cv::Mat downSampled; 12 | cv::resize(img, downSampled, cv::Size(752, 480)); 13 | img = downSampled; 14 | 15 | printf("\nTesting PLAIN version\n"); 16 | double time_accumulator = 0; 17 | for (int i = 0; i < n_trials; ++i) { 18 | corners.clear(); 19 | double t = (double)cv::getTickCount(); 20 | fast::fast_corner_detect_10((fast::fast_byte *)(img.data), img.cols, img.rows, img.cols, 75, corners); 21 | time_accumulator += ((cv::getTickCount() - t) / cv::getTickFrequency()); 22 | } 23 | printf("PLAIN took %f ms (average over %d trials).\n", time_accumulator/((double)n_trials)*1000.0, n_trials ); 24 | printf("PLAIN version extracted %zu features.\n", corners.size()); 25 | 26 | #if __NEON__ 27 | printf("\nTesting NEON version\n"); 28 | time_accumulator = 0; 29 | for (int i = 0; i < n_trials; ++i) { 30 | corners.clear(); 31 | double t = (double)cv::getTickCount(); 32 | fast::fast_corner_detect_9_neon((fast::fast_byte *)(img.data), img.cols, img.rows, img.cols, 75, corners); 33 | time_accumulator += ((cv::getTickCount() - t) / cv::getTickFrequency()); 34 | } 35 | printf("NEON version took %f ms (average over %d trials).\n", time_accumulator/((double)n_trials)*1000.0, n_trials); 36 | printf("NEON version extracted %zu features.\n", corners.size()); 37 | #endif 38 | 39 | #if __SSE2__ 40 | printf("\nTesting SSE2 version\n"); 41 | time_accumulator = 0; 42 | for (int i = 0; i < n_trials; ++i) { 43 | corners.clear(); 44 | double t = (double)cv::getTickCount(); 45 | fast::fast_corner_detect_10_sse2((fast::fast_byte *)(img.data), img.cols, img.rows, img.cols, 75, corners); 46 | time_accumulator += ((cv::getTickCount() - t) / cv::getTickFrequency()); 47 | } 48 | printf("SSE2 version took %f ms (average over %d trials).\n", time_accumulator/((double)n_trials)*1000.0, n_trials); 49 | printf("SSE2 version extracted %zu features.\n", corners.size()); 50 | #endif 51 | 52 | printf("\nBENCHMARK version extracted 167 features.\n"); 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # YGZ-Stereo-Inertial 2 | This is YGZ-stereo-inertial SLAM, a stereo inertial VO code. It is designed for stereo and stereo-inertial sensor modules like vi-sensor. It uses a LK optical flow as front-end and a sliding window bundle adjustment as a backend. Feel free to try it in datasets and your own sensors. 3 | 4 | The code is experimental and I can't guarantee its performance in all cases. 5 | 6 | # Dependency 7 | If you are using ubuntu, just type "./install_dep.sh" to install all the dependencies except pangolin. 8 | 9 | - Pangolin (for visualization): https://github.com/stevenlovegrove/Pangolin 10 | - Eigen3: sudo apt-get install libeigen3-dev 11 | - g2o: sudo apt-get install libcxsparse-dev libqt4-dev libcholmod3.0.6 libsuitesparse-dev qt4-qmake 12 | - OpenCV: sudo apt-get install libopencv-dev 13 | - glog (for logging): sudo apt-get install libgoogle-glog-dev 14 | 15 | # Compile 16 | run "./generate.sh" to compile all the things, or follow the steps in generate.sh 17 | 18 | # Examples 19 | You can put stereo or stereo-imu data into ygz-stereo, for example the EUROC dataset 20 | (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). We provide the pure stereo vision and stereo-inertial vision for EUROC. Run the pure vision examples by typing: 21 | 22 | ``` 23 | bin/EurocStereo ./examples/EurocStereo.yaml 24 | ``` 25 | 26 | to run the pure vision mode. Don't forget to specify the dataset directory in the yaml config file first. Also, to run visual-inertial mode, type: 27 | ``` 28 | bin/EurocStereoVIO ./examples/EurocStereoVIO.yaml 29 | ``` 30 | 31 | to run the stereo VIO case. 32 | 33 | # Other things 34 | YGZ-stereo is more robust than the previous YGZ-ORB-SLAM (and the code is also more clear). In EUROC it can pass the test of all MHxx and V101, V201, V202. For difficult cases it may still fail. As I will go to TUM soon, my future work may be a visual-inertial DSO. 35 | 36 | YGZ stands for Yi-Guo-Zhou (a port of porridge, a group of mess) because it contains feature method, direct method and imu things. 37 | 38 | The Note.md is a file of develop records. 39 | 40 | Contact me (gaoxiang12@mails.tsinghua.edu.cn) or Wang Jing (https://github.com/jingpang) for commercial use. 41 | 42 | Thanks the following companies/people for financial support: 43 | - Usens凌感科技 44 | - Hyperception 远形时空 45 | - Qfeeltech 速感科技 46 | - 天之博特 47 | - 视辰信息科技 48 | 49 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief Implementation of the Gauss Newton Algorithm 36 | */ 37 | class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian 38 | { 39 | public: 40 | /** 41 | * construct the Gauss Newton algorithm, which use the given Solver for solving the 42 | * linearized system. 43 | */ 44 | explicit OptimizationAlgorithmGaussNewton(Solver* solver); 45 | virtual ~OptimizationAlgorithmGaussNewton(); 46 | 47 | virtual SolverResult solve(int iteration, bool online = false); 48 | 49 | virtual void printVerbose(std::ostream& os) const; 50 | }; 51 | 52 | } // end namespace 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "optimization_algorithm.h" 28 | 29 | using namespace std; 30 | 31 | namespace g2o { 32 | 33 | OptimizationAlgorithm::OptimizationAlgorithm() : 34 | _optimizer(0) 35 | { 36 | } 37 | 38 | OptimizationAlgorithm::~OptimizationAlgorithm() 39 | { 40 | } 41 | 42 | void OptimizationAlgorithm::printProperties(std::ostream& os) const 43 | { 44 | os << "------------- Algorithm Properties -------------" << endl; 45 | for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { 46 | BaseProperty* p = it->second; 47 | os << it->first << "\t" << p->toString() << endl; 48 | } 49 | os << "------------------------------------------------" << endl; 50 | } 51 | 52 | bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) 53 | { 54 | return _properties.updateMapFromString(propString); 55 | } 56 | 57 | void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) 58 | { 59 | _optimizer = optimizer; 60 | } 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /common/include/ygz/sophus.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_HPP 24 | #define SOPHUS_HPP 25 | 26 | #include 27 | 28 | // fix log1p not being found on Android in Eigen 29 | #if defined( ANDROID ) 30 | #include 31 | namespace std 32 | { 33 | using ::log1p; 34 | } 35 | #endif 36 | 37 | #include 38 | #include 39 | 40 | namespace Sophus { 41 | using namespace Eigen; 42 | 43 | template 44 | struct SophusConstants { 45 | EIGEN_ALWAYS_INLINE static 46 | const Scalar epsilon() { 47 | return static_cast ( 1e-10 ); 48 | } 49 | 50 | EIGEN_ALWAYS_INLINE static 51 | const Scalar pi() { 52 | return static_cast ( M_PI ); 53 | } 54 | }; 55 | 56 | template<> 57 | struct SophusConstants { 58 | EIGEN_ALWAYS_INLINE static 59 | float epsilon() { 60 | return static_cast ( 1e-5 ); 61 | } 62 | 63 | EIGEN_ALWAYS_INLINE static 64 | float pi() { 65 | return static_cast ( M_PI ); 66 | } 67 | }; 68 | 69 | class SophusException : public std::runtime_error { 70 | public: 71 | SophusException(const std::string &str) 72 | : runtime_error("Sophus exception: " + str) { 73 | } 74 | }; 75 | 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /Thirdparty/fast/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # user config 3 | 4 | # Based on the ARM_ARCHITECTURE environment variable 5 | # Set IS_ARM to build on the Odroid board 6 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 7 | MESSAGE("ARM_ARCHITECTURE environment variable set to " $ENV{ARM_ARCHITECTURE}) 8 | SET(IS_ARM TRUE) 9 | ELSE() 10 | SET(IS_ARM FALSE) 11 | ENDIF() 12 | 13 | SET(BUILD_TEST TRUE) 14 | 15 | ####################################### 16 | SET(PROJECT_NAME fast) 17 | PROJECT(${PROJECT_NAME}) 18 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 19 | SET(CMAKE_VERBOSE_MAKEFILE ON) 20 | 21 | # Build type and flags 22 | SET(CMAKE_BUILD_TYPE Release) # Options: Debug, RelWithDebInfo, Release 23 | SET(CMAKE_CXX_FLAGS "-Wall -Werror ") 24 | SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -O0 -g") 25 | IF(IS_ARM) 26 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -mfpu=neon -march=armv7-a") 27 | ELSE() 28 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -mmmx -msse -msse -msse2 -msse3 -mssse3 -fomit-frame-pointer") 29 | ENDIF() 30 | 31 | # Add Sources 32 | SET(SOURCES ${PROJECT_SOURCE_DIR}/src/fast_10.cpp 33 | ${PROJECT_SOURCE_DIR}/src/fast_10_score.cpp 34 | ${PROJECT_SOURCE_DIR}/src/nonmax_3x3.cpp) 35 | IF(IS_ARM) 36 | # LIST(APPEND SOURCES ${PROJECT_SOURCE_DIR}/src/faster_corner_9_neon.cpp) 37 | ELSE() 38 | LIST(APPEND SOURCES ${PROJECT_SOURCE_DIR}/src/faster_corner_10_sse.cpp) 39 | ENDIF() 40 | 41 | # Add library 42 | INCLUDE_DIRECTORIES(include src) 43 | ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES}) 44 | 45 | # Add Tests 46 | IF(BUILD_TEST) 47 | MESSAGE("Building Fast Test") 48 | FIND_PACKAGE(OpenCV REQUIRED) 49 | ADD_DEFINITIONS(-DTEST_DATA_DIR=\"${PROJECT_SOURCE_DIR}/test/data\") 50 | ADD_EXECUTABLE(fast_test test/test.cpp) 51 | TARGET_LINK_LIBRARIES(fast_test ${OpenCV_LIBS} ${PROJECT_NAME}) 52 | ENDIF() 53 | 54 | 55 | ################################################################################ 56 | # Create the fastConfig.cmake file for other cmake projects. 57 | GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION ) 58 | SET(fast_LIBRARIES ${FULL_LIBRARY_NAME} ) 59 | SET(fast_LIBRARY_DIR ${PROJECT_BINARY_DIR} ) 60 | SET(fast_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include") 61 | CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/fastConfig.cmake.in 62 | ${CMAKE_CURRENT_BINARY_DIR}/fastConfig.cmake @ONLY IMMEDIATE ) 63 | export( PACKAGE fast ) 64 | 65 | INSTALL(DIRECTORY include/fast DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.h" ) 66 | INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) 67 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/types_slam3d.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // 4 | // g2o is free software: you can redistribute it and/or modify 5 | // it under the terms of the GNU Lesser General Public License as published 6 | // by the Free Software Foundation, either version 3 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // g2o is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU Lesser General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU Lesser General Public License 15 | // along with this program. If not, see . 16 | 17 | #include "types_slam3d.h" 18 | #include "g2o/core/factory.h" 19 | #include "g2o/stuff/macros.h" 20 | 21 | #include 22 | 23 | namespace g2o { 24 | 25 | G2O_REGISTER_TYPE_GROUP(slam3d); 26 | 27 | G2O_REGISTER_TYPE(VERTEX_SE3:QUAT, VertexSE3); 28 | G2O_REGISTER_TYPE(EDGE_SE3:QUAT, EdgeSE3); 29 | G2O_REGISTER_TYPE(VERTEX_TRACKXYZ, VertexPointXYZ); 30 | 31 | G2O_REGISTER_TYPE(PARAMS_SE3OFFSET, ParameterSE3Offset); 32 | G2O_REGISTER_TYPE(EDGE_SE3_TRACKXYZ, EdgeSE3PointXYZ); 33 | G2O_REGISTER_TYPE(EDGE_SE3_PRIOR, EdgeSE3Prior); 34 | G2O_REGISTER_TYPE(CACHE_SE3_OFFSET, CacheSE3Offset); 35 | G2O_REGISTER_TYPE(EDGE_SE3_OFFSET, EdgeSE3Offset); 36 | 37 | G2O_REGISTER_TYPE(PARAMS_CAMERACALIB, ParameterCamera); 38 | G2O_REGISTER_TYPE(PARAMS_STEREOCAMERACALIB, ParameterStereoCamera); 39 | G2O_REGISTER_TYPE(CACHE_CAMERA, CacheCamera); 40 | G2O_REGISTER_TYPE(EDGE_PROJECT_DISPARITY, EdgeSE3PointXYZDisparity); 41 | G2O_REGISTER_TYPE(EDGE_PROJECT_DEPTH, EdgeSE3PointXYZDepth); 42 | 43 | G2O_REGISTER_TYPE(EDGE_POINTXYZ, EdgePointXYZ); 44 | 45 | G2O_REGISTER_TYPE(EDGE_SE3_LOTSOF_XYZ, EdgeSE3LotsOfXYZ) 46 | 47 | /*********** ACTIONS ************/ 48 | G2O_REGISTER_ACTION(VertexSE3WriteGnuplotAction); 49 | G2O_REGISTER_ACTION(VertexPointXYZWriteGnuplotAction); 50 | G2O_REGISTER_ACTION(EdgeSE3WriteGnuplotAction); 51 | 52 | #ifdef G2O_HAVE_OPENGL 53 | G2O_REGISTER_ACTION(VertexPointXYZDrawAction); 54 | G2O_REGISTER_ACTION(VertexSE3DrawAction); 55 | G2O_REGISTER_ACTION(EdgeSE3DrawAction); 56 | G2O_REGISTER_ACTION(EdgeSE3PointXYZDrawAction); 57 | G2O_REGISTER_ACTION(EdgeProjectDisparityDrawAction); 58 | G2O_REGISTER_ACTION(CacheCameraDrawAction); 59 | G2O_REGISTER_ACTION(CacheSE3OffsetDrawAction); 60 | #endif 61 | 62 | } // end namespace 63 | -------------------------------------------------------------------------------- /Thirdparty/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 | #include "../DUtils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Id of words 22 | typedef unsigned int WordId; 23 | 24 | /// Value of a word 25 | typedef double WordValue; 26 | 27 | /// Id of nodes in the vocabulary treee 28 | typedef unsigned int NodeId; 29 | 30 | /// L-norms for normalization 31 | EXPORT typedef enum LNorm 32 | { 33 | L1, 34 | L2 35 | } LNorm; 36 | 37 | /// Weighting type 38 | EXPORT typedef enum WeightingType 39 | { 40 | TF_IDF, 41 | TF, 42 | IDF, 43 | BINARY 44 | } WeightingType; 45 | 46 | /// Scoring type 47 | EXPORT typedef enum ScoringType 48 | { 49 | L1_NORM, 50 | L2_NORM, 51 | CHI_SQUARE, 52 | KL, 53 | BHATTACHARYYA, 54 | DOT_PRODUCT, 55 | } ScoringType; 56 | 57 | /// Vector of words to represent images 58 | /// stl的map结构，key为wordId，value为tfidf中的tf 59 | class EXPORT BowVector: 60 | public std::map 61 | { 62 | public: 63 | 64 | /** 65 | * Constructor 66 | */ 67 | BowVector(void); 68 | 69 | /** 70 | * Destructor 71 | */ 72 | ~BowVector(void); 73 | 74 | /** 75 | * Adds a value to a word value existing in the vector, or creates a new 76 | * word with the given value 77 | * @param id word id to look for 78 | * @param v value to create the word with, or to add to existing word 79 | */ 80 | void addWeight(WordId id, WordValue v); 81 | 82 | /** 83 | * Adds a word with a value to the vector only if this does not exist yet 84 | * @param id word id to look for 85 | * @param v value to give to the word if this does not exist 86 | */ 87 | void addIfNotExist(WordId id, WordValue v); 88 | 89 | /** 90 | * L1-Normalizes the values in the vector 91 | * @param norm_type norm used 92 | */ 93 | void normalize(LNorm norm_type); 94 | 95 | /** 96 | * Prints the content of the bow vector 97 | * @param out stream 98 | * @param v 99 | */ 100 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 101 | 102 | /** 103 | * Saves the bow vector as a vector in a matlab file 104 | * @param filename 105 | * @param W number of words in the vocabulary 106 | */ 107 | void saveM(const std::string &filename, size_t W) const; 108 | }; 109 | 110 | } // namespace DBoW2 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/se3_ops.hpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 H. Strasdat 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | Matrix3d skew(const Vector3d&v) 28 | { 29 | Matrix3d m; 30 | m.fill(0.); 31 | m(0,1) = -v(2); 32 | m(0,2) = v(1); 33 | m(1,2) = -v(0); 34 | m(1,0) = v(2); 35 | m(2,0) = -v(1); 36 | m(2,1) = v(0); 37 | return m; 38 | } 39 | 40 | Vector3d deltaR(const Matrix3d& R) 41 | { 42 | Vector3d v; 43 | v(0)=R(2,1)-R(1,2); 44 | v(1)=R(0,2)-R(2,0); 45 | v(2)=R(1,0)-R(0,1); 46 | return v; 47 | } 48 | 49 | Vector2d project(const Vector3d& v) 50 | { 51 | Vector2d res; 52 | res(0) = v(0)/v(2); 53 | res(1) = v(1)/v(2); 54 | return res; 55 | } 56 | 57 | Vector3d project(const Vector4d& v) 58 | { 59 | Vector3d res; 60 | res(0) = v(0)/v(3); 61 | res(1) = v(1)/v(3); 62 | res(2) = v(2)/v(3); 63 | return res; 64 | } 65 | 66 | Vector3d unproject(const Vector2d& v) 67 | { 68 | Vector3d res; 69 | res(0) = v(0); 70 | res(1) = v(1); 71 | res(2) = 1; 72 | return res; 73 | } 74 | 75 | Vector4d unproject(const Vector3d& v) 76 | { 77 | Vector4d res; 78 | res(0) = v(0); 79 | res(1) = v(1); 80 | res(2) = v(2); 81 | res(3) = 1; 82 | return res; 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_pointxyz.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "edge_pointxyz.h" 28 | 29 | namespace g2o { 30 | 31 | EdgePointXYZ::EdgePointXYZ() : 32 | BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexPointXYZ>() 33 | { 34 | _information.setIdentity(); 35 | _error.setZero(); 36 | } 37 | 38 | bool EdgePointXYZ::read(std::istream& is) 39 | { 40 | Vector3d p; 41 | is >> p[0] >> p[1] >> p[2]; 42 | setMeasurement(p); 43 | for (int i = 0; i < 3; ++i) 44 | for (int j = i; j < 3; ++j) { 45 | is >> information()(i, j); 46 | if (i != j) 47 | information()(j, i) = information()(i, j); 48 | } 49 | return true; 50 | } 51 | 52 | bool EdgePointXYZ::write(std::ostream& os) const 53 | { 54 | Vector3d p = measurement(); 55 | os << p.x() << " " << p.y() << " " << p.z(); 56 | for (int i = 0; i < 3; ++i) 57 | for (int j = i; j < 3; ++j) 58 | os << " " << information()(i, j); 59 | return os.good(); 60 | } 61 | 62 | 63 | #ifndef NUMERIC_JACOBIAN_THREE_D_TYPES 64 | void EdgePointXYZ::linearizeOplus() 65 | { 66 | _jacobianOplusXi=-Matrix3d::Identity(); 67 | _jacobianOplusXj= Matrix3d::Identity(); 68 | } 69 | #endif 70 | 71 | } // end namespace 72 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_se3.h: -------------------------------------------------------------------------------- 1 | #ifndef G2O_EDGE_SE3_H_ 2 | #define G2O_EDGE_SE3_H_ 3 | 4 | #include "g2o/core/base_binary_edge.h" 5 | 6 | #include "g2o_types_slam3d_api.h" 7 | #include "vertex_se3.h" 8 | 9 | namespace g2o { 10 | 11 | /** 12 | * \brief Edge between two 3D pose vertices 13 | * 14 | * The transformation between the two vertices is given as an Isometry3D. 15 | * If z denotes the measurement, then the error function is given as follows: 16 | * z^-1 * (x_i^-1 * x_j) 17 | */ 18 | class G2O_TYPES_SLAM3D_API EdgeSE3 : public BaseBinaryEdge<6, Isometry3D, VertexSE3, VertexSE3> { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 21 | EdgeSE3(); 22 | virtual bool read(std::istream& is); 23 | virtual bool write(std::ostream& os) const; 24 | 25 | void computeError(); 26 | 27 | virtual void setMeasurement(const Isometry3D& m){ 28 | _measurement = m; 29 | _inverseMeasurement = m.inverse(); 30 | } 31 | 32 | virtual bool setMeasurementData(const double* d){ 33 | Eigen::Map v(d); 34 | setMeasurement(internal::fromVectorQT(v)); 35 | return true; 36 | } 37 | 38 | virtual bool getMeasurementData(double* d) const{ 39 | Eigen::Map v(d); 40 | v = internal::toVectorQT(_measurement); 41 | return true; 42 | } 43 | 44 | void linearizeOplus(); 45 | 46 | virtual int measurementDimension() const {return 7;} 47 | 48 | virtual bool setMeasurementFromState() ; 49 | 50 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 51 | OptimizableGraph::Vertex* /*to*/) { 52 | return 1.; 53 | } 54 | 55 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 56 | 57 | protected: 58 | Isometry3D _inverseMeasurement; 59 | }; 60 | 61 | /** 62 | * \brief Output the pose-pose constraint to Gnuplot data file 63 | */ 64 | class G2O_TYPES_SLAM3D_API EdgeSE3WriteGnuplotAction: public WriteGnuplotAction { 65 | public: 66 | EdgeSE3WriteGnuplotAction(); 67 | virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 68 | HyperGraphElementAction::Parameters* params_); 69 | }; 70 | 71 | #ifdef G2O_HAVE_OPENGL 72 | /** 73 | * \brief Visualize a 3D pose-pose constraint 74 | */ 75 | class G2O_TYPES_SLAM3D_API EdgeSE3DrawAction: public DrawAction{ 76 | public: 77 | EdgeSE3DrawAction(); 78 | virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 79 | HyperGraphElementAction::Parameters* params_); 80 | }; 81 | #endif 82 | 83 | } // end namespace 84 | #endif 85 | -------------------------------------------------------------------------------- /examples/EurocStereo.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Dataset dirs 4 | LeftFolder: /home/gaoxiang/dataset/euroc/V1_01_easy/cam0/data 5 | RightFolder: /home/gaoxiang/dataset/euroc/V1_01_easy/cam1/data 6 | TimeFolder: /home/gaoxiang/code/ygz-stereo-release/examples/EuRoC_TimeStamps/V101.txt 7 | 8 | # if running in pure stereo vision mode 9 | PureVisionMode: true 10 | 11 | # do we need visualization? 12 | UseViewer: true 13 | 14 | # Camera calibration and distortion parameters (OpenCV) 15 | Camera.fx: 435.2046959714599 16 | Camera.fy: 435.2046959714599 17 | Camera.cx: 367.4517211914062 18 | Camera.cy: 252.2008514404297 19 | 20 | Camera.k1: 0.0 21 | Camera.k2: 0.0 22 | Camera.p1: 0.0 23 | Camera.p2: 0.0 24 | 25 | Camera.width: 752 26 | Camera.height: 480 27 | 28 | # stereo baseline times fx 29 | Camera.bf: 47.90639384423901 30 | 31 | #-------------------------------------------------------------------------------------------- 32 | # Stereo Rectification. Only if you need to pre-rectify the images. 33 | # Camera.fx, .fy, etc must be the same as in LEFT.P 34 | #-------------------------------------------------------------------------------------------- 35 | LEFT.height: 480 36 | LEFT.width: 752 37 | LEFT.D: !!opencv-matrix 38 | rows: 1 39 | cols: 5 40 | dt: d 41 | data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 42 | LEFT.K: !!opencv-matrix 43 | rows: 3 44 | cols: 3 45 | dt: d 46 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 47 | LEFT.R: !!opencv-matrix 48 | rows: 3 49 | cols: 3 50 | dt: d 51 | data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 52 | LEFT.P: !!opencv-matrix 53 | rows: 3 54 | cols: 4 55 | dt: d 56 | data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 57 | 58 | RIGHT.height: 480 59 | RIGHT.width: 752 60 | RIGHT.D: !!opencv-matrix 61 | rows: 1 62 | cols: 5 63 | dt: d 64 | data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0] 65 | RIGHT.K: !!opencv-matrix 66 | rows: 3 67 | cols: 3 68 | dt: d 69 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1] 70 | RIGHT.R: !!opencv-matrix 71 | rows: 3 72 | cols: 3 73 | dt: d 74 | data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644] 75 | RIGHT.P: !!opencv-matrix 76 | rows: 3 77 | cols: 4 78 | dt: d 79 | data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 80 | -------------------------------------------------------------------------------- /examples/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Dataset dirs 4 | LeftFolder: /home/xiang/dataset/euroc/MH_01_easy/cam0/data; 5 | RightFolder: /home/xiang/dataset/euroc/MH_01_easy/cam1/data; 6 | IMUFolder: /home/xiang/dataset/euroc/MH_01_easy/imu0/data.csv; 7 | TimeFolder: /home/xiang/code/ygz-stereo-inertial/examples/EuRoC_TimeStamps/MH01.txt; 8 | 9 | # if running in pure stereo vision mode 10 | PureVisionMode: true 11 | 12 | # do we need visualization? 13 | UseViewer: true 14 | 15 | # Camera calibration and distortion parameters (OpenCV) 16 | Camera.fx: 435.2046959714599 17 | Camera.fy: 435.2046959714599 18 | Camera.cx: 367.4517211914062 19 | Camera.cy: 252.2008514404297 20 | 21 | Camera.k1: 0.0 22 | Camera.k2: 0.0 23 | Camera.p1: 0.0 24 | Camera.p2: 0.0 25 | 26 | Camera.width: 752 27 | Camera.height: 480 28 | 29 | # stereo baseline times fx 30 | Camera.bf: 47.90639384423901 31 | 32 | #-------------------------------------------------------------------------------------------- 33 | # Stereo Rectification. Only if you need to pre-rectify the images. 34 | # Camera.fx, .fy, etc must be the same as in LEFT.P 35 | #-------------------------------------------------------------------------------------------- 36 | LEFT.height: 480 37 | LEFT.width: 752 38 | LEFT.D: !!opencv-matrix 39 | rows: 1 40 | cols: 5 41 | dt: d 42 | data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 43 | LEFT.K: !!opencv-matrix 44 | rows: 3 45 | cols: 3 46 | dt: d 47 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 48 | LEFT.R: !!opencv-matrix 49 | rows: 3 50 | cols: 3 51 | dt: d 52 | data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 53 | LEFT.P: !!opencv-matrix 54 | rows: 3 55 | cols: 4 56 | dt: d 57 | data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 58 | 59 | RIGHT.height: 480 60 | RIGHT.width: 752 61 | RIGHT.D: !!opencv-matrix 62 | rows: 1 63 | cols: 5 64 | dt: d 65 | data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0] 66 | RIGHT.K: !!opencv-matrix 67 | rows: 3 68 | cols: 3 69 | dt: d 70 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1] 71 | RIGHT.R: !!opencv-matrix 72 | rows: 3 73 | cols: 3 74 | dt: d 75 | data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644] 76 | RIGHT.P: !!opencv-matrix 77 | rows: 3 78 | cols: 4 79 | dt: d 80 | data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 81 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_property.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | 34 | /** 35 | * \brief describe the properties of a solver 36 | */ 37 | struct OptimizationAlgorithmProperty 38 | { 39 | std::string name; ///< name of the solver, e.g., var 40 | std::string desc; ///< short description of the solver 41 | std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" 42 | bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks 43 | int poseDim; ///< dimension of the pose vertices (-1 if variable) 44 | int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) 45 | OptimizationAlgorithmProperty() : 46 | name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) 47 | { 48 | } 49 | OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : 50 | name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) 51 | { 52 | } 53 | }; 54 | 55 | } // end namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/color_macros.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_COLOR_MACROS_H 28 | #define G2O_COLOR_MACROS_H 29 | 30 | // font attributes 31 | #define FT_BOLD "\033[1m" 32 | #define FT_UNDERLINE "\033[4m" 33 | 34 | //background color 35 | #define BG_BLACK "\033[40m" 36 | #define BG_RED "\033[41m" 37 | #define BG_GREEN "\033[42m" 38 | #define BG_YELLOW "\033[43m" 39 | #define BG_LIGHTBLUE "\033[44m" 40 | #define BG_MAGENTA "\033[45m" 41 | #define BG_BLUE "\033[46m" 42 | #define BG_WHITE "\033[47m" 43 | 44 | // font color 45 | #define CL_BLACK(s) "\033[30m" << s << "\033[0m" 46 | #define CL_RED(s) "\033[31m" << s << "\033[0m" 47 | #define CL_GREEN(s) "\033[32m" << s << "\033[0m" 48 | #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" 49 | #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" 50 | #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" 51 | #define CL_BLUE(s) "\033[36m" << s << "\033[0m" 52 | #define CL_WHITE(s) "\033[37m" << s << "\033[0m" 53 | 54 | #define FG_BLACK "\033[30m" 55 | #define FG_RED "\033[31m" 56 | #define FG_GREEN "\033[32m" 57 | #define FG_YELLOW "\033[33m" 58 | #define FG_LIGHTBLUE "\033[34m" 59 | #define FG_MAGENTA "\033[35m" 60 | #define FG_BLUE "\033[36m" 61 | #define FG_WHITE "\033[37m" 62 | 63 | #define FG_NORM "\033[0m" 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/parameter_stereo_camera.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "parameter_stereo_camera.h" 28 | #include "isometry3d_gradients.h" 29 | #include "isometry3d_mappings.h" 30 | 31 | #ifdef WINDOWS 32 | #include 33 | #endif 34 | 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | ParameterStereoCamera::ParameterStereoCamera(){ 40 | setBaseline(0.075); 41 | } 42 | 43 | bool ParameterStereoCamera::read(std::istream& is) { 44 | Vector7d off; 45 | for (int i=0; i<7; i++) 46 | is >> off[i]; 47 | // normalize the quaternion to recover numerical precision lost by storing as human readable text 48 | Vector4D::MapType(off.data()+3).normalize(); 49 | setOffset(internal::fromVectorQT(off)); 50 | double fx,fy,cx,cy; 51 | is >> fx >> fy >> cx >> cy; 52 | setKcam(fx,fy,cx,cy); 53 | double baseline_; 54 | is >> baseline_; 55 | setBaseline(baseline_); 56 | return is.good(); 57 | } 58 | 59 | bool ParameterStereoCamera::write(std::ostream& os) const { 60 | Vector7d off = internal::toVectorQT(_offset); 61 | for (int i=0; i<7; i++) 62 | os << off[i] << " "; 63 | os << _Kcam(0,0) << " "; 64 | os << _Kcam(1,1) << " "; 65 | os << _Kcam(0,2) << " "; 66 | os << _Kcam(1,2) << " "; 67 | os << baseline() << " "; 68 | return os.good(); 69 | } 70 | 71 | 72 | } 73 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_structure.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_MATRIX_STRUCTURE_H 28 | #define G2O_MATRIX_STRUCTURE_H 29 | 30 | 31 | namespace g2o { 32 | 33 | /** 34 | * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) 35 | */ 36 | class MatrixStructure 37 | { 38 | public: 39 | MatrixStructure(); 40 | ~MatrixStructure(); 41 | /** 42 | * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will 43 | * then reallocate the memory + additional space (double the required space). 44 | */ 45 | void alloc(int n_, int nz); 46 | 47 | void free(); 48 | 49 | /** 50 | * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) 51 | */ 52 | bool write(const char* filename) const; 53 | 54 | int n; ///< A is m-by-n. n must be >= 0. 55 | int m; ///< A is m-by-n. m must be >= 0. 56 | int* Ap; ///< column pointers for A, of size n+1 57 | int* Aii; ///< row indices of A, of size nz = Ap [n] 58 | 59 | //! max number of non-zeros blocks 60 | int nzMax() const { return maxNz;} 61 | 62 | protected: 63 | int maxN; ///< size of the allocated memory 64 | int maxNz; ///< size of the allocated memory 65 | }; 66 | 67 | } // end namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_se3_offset.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_EDGE_SE3_OFFSET_H_ 28 | #define G2O_EDGE_SE3_OFFSET_H_ 29 | 30 | #include "g2o/core/base_binary_edge.h" 31 | 32 | #include "vertex_se3.h" 33 | #include "edge_se3.h" 34 | #include "g2o_types_slam3d_api.h" 35 | 36 | namespace g2o { 37 | class ParameterSE3Offset; 38 | class CacheSE3Offset; 39 | 40 | /** 41 | * \brief Offset edge 42 | */ 43 | // first two args are the measurement type, second two the connection classes 44 | class G2O_TYPES_SLAM3D_API EdgeSE3Offset : public EdgeSE3 { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | EdgeSE3Offset(); 48 | virtual bool read(std::istream& is); 49 | virtual bool write(std::ostream& os) const; 50 | 51 | void computeError(); 52 | 53 | 54 | void linearizeOplus(); 55 | 56 | virtual bool setMeasurementFromState() ; 57 | 58 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 59 | OptimizableGraph::Vertex* /*to*/) { 60 | return 1.; 61 | } 62 | 63 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 64 | 65 | protected: 66 | virtual bool resolveCaches(); 67 | ParameterSE3Offset *_offsetFrom, *_offsetTo; 68 | CacheSE3Offset *_cacheFrom, *_cacheTo; 69 | }; 70 | 71 | } // end namespace 72 | #endif 73 | -------------------------------------------------------------------------------- /cmake-modules/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) 90 | -------------------------------------------------------------------------------- /Thirdparty/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 | #include "../DUtils/config.h" 16 | 17 | namespace DBoW2 { 18 | 19 | /// Base class of scoring functions 20 | class EXPORT GeneralScoring 21 | { 22 | public: 23 | /** 24 | * Computes the score between two vectors. Vectors must be sorted and 25 | * normalized if necessary 26 | * @param v (in/out) 27 | * @param w (in/out) 28 | * @return score 29 | */ 30 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 31 | 32 | /** 33 | * Returns whether a vector must be normalized before scoring according 34 | * to the scoring scheme 35 | * @param norm norm to use 36 | * @return true iff must normalize 37 | */ 38 | virtual bool mustNormalize(LNorm &norm) const = 0; 39 | 40 | /// Log of epsilon 41 | static const double LOG_EPS; 42 | // If you change the type of WordValue, make sure you change also the 43 | // epsilon value (this is needed by the KL method) 44 | 45 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 46 | 47 | }; 48 | 49 | /** 50 | * Macro for defining Scoring classes 51 | * @param NAME name of class 52 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 53 | * @param NORM type of norm to use when MUSTNORMALIZE 54 | */ 55 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 56 | NAME: public GeneralScoring \ 57 | { public: \ 58 | /** \ 59 | * Computes score between two vectors \ 60 | * @param v \ 61 | * @param w \ 62 | * @return score between v and w \ 63 | */ \ 64 | virtual double score(const BowVector &v, const BowVector &w) const; \ 65 | \ 66 | /** \ 67 | * Says if a vector must be normalized according to the scoring function \ 68 | * @param norm (out) if true, norm to use 69 | * @return true iff vectors must be normalized \ 70 | */ \ 71 | virtual inline bool mustNormalize(LNorm &norm) const \ 72 | { norm = NORM; return MUSTNORMALIZE; } \ 73 | } 74 | 75 | /// L1 Scoring object 76 | class EXPORT __SCORING_CLASS(L1Scoring, true, L1); 77 | 78 | /// L2 Scoring object 79 | class EXPORT __SCORING_CLASS(L2Scoring, true, L2); 80 | 81 | /// Chi square Scoring object 82 | class EXPORT __SCORING_CLASS(ChiSquareScoring, true, L1); 83 | 84 | /// KL divergence Scoring object 85 | class EXPORT __SCORING_CLASS(KLScoring, true, L1); 86 | 87 | /// Bhattacharyya Scoring object 88 | class EXPORT __SCORING_CLASS(BhattacharyyaScoring, true, L1); 89 | 90 | /// Dot product Scoring object 91 | class EXPORT __SCORING_CLASS(DotProductScoring, false, L1); 92 | 93 | #undef __SCORING_CLASS 94 | 95 | } // namespace DBoW2 96 | 97 | #endif 98 | 99 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/creators.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CREATORS_H 28 | #define G2O_CREATORS_H 29 | 30 | #include "hyper_graph.h" 31 | 32 | #include 33 | #include 34 | 35 | namespace g2o 36 | { 37 | 38 | /** 39 | * \brief Abstract interface for allocating HyperGraphElement 40 | */ 41 | class AbstractHyperGraphElementCreator 42 | { 43 | public: 44 | /** 45 | * create a hyper graph element. Has to implemented in derived class. 46 | */ 47 | virtual HyperGraph::HyperGraphElement* construct() = 0; 48 | /** 49 | * name of the class to be created. Has to implemented in derived class. 50 | */ 51 | virtual const std::string& name() const = 0; 52 | 53 | virtual ~AbstractHyperGraphElementCreator() { } 54 | }; 55 | 56 | /** 57 | * \brief templatized creator class which creates graph elements 58 | */ 59 | template 60 | class HyperGraphElementCreator : public AbstractHyperGraphElementCreator 61 | { 62 | public: 63 | HyperGraphElementCreator() : _name(typeid(T).name()) {} 64 | #if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC 65 | __attribute__((force_align_arg_pointer)) 66 | #endif 67 | HyperGraph::HyperGraphElement* construct() { return new T;} 68 | virtual const std::string& name() const { return _name;} 69 | protected: 70 | std::string _name; 71 | }; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/solver.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "solver.h" 28 | 29 | #include 30 | #include 31 | 32 | namespace g2o { 33 | 34 | Solver::Solver() : 35 | _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), 36 | _isLevenberg(false), _additionalVectorSpace(0) 37 | { 38 | } 39 | 40 | Solver::~Solver() 41 | { 42 | delete[] _x; 43 | delete[] _b; 44 | } 45 | 46 | void Solver::resizeVector(size_t sx) 47 | { 48 | size_t oldSize = _xSize; 49 | _xSize = sx; 50 | sx += _additionalVectorSpace; // allocate some additional space if requested 51 | if (_maxXSize < sx) { 52 | _maxXSize = 2*sx; 53 | delete[] _x; 54 | _x = new double[_maxXSize]; 55 | #ifndef NDEBUG 56 | memset(_x, 0, _maxXSize * sizeof(double)); 57 | #endif 58 | if (_b) { // backup the former b, might still be needed for online processing 59 | memcpy(_x, _b, oldSize * sizeof(double)); 60 | delete[] _b; 61 | _b = new double[_maxXSize]; 62 | std::swap(_b, _x); 63 | } else { 64 | _b = new double[_maxXSize]; 65 | #ifndef NDEBUG 66 | memset(_b, 0, _maxXSize * sizeof(double)); 67 | #endif 68 | } 69 | } 70 | } 71 | 72 | void Solver::setOptimizer(SparseOptimizer* optimizer) 73 | { 74 | _optimizer = optimizer; 75 | } 76 | 77 | void Solver::setLevenberg(bool levenberg) 78 | { 79 | _isLevenberg = levenberg; 80 | } 81 | 82 | void Solver::setAdditionalVectorSpace(size_t s) 83 | { 84 | _additionalVectorSpace = s; 85 | } 86 | 87 | } // end namespace 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H 29 | 30 | #include "optimization_algorithm.h" 31 | 32 | namespace g2o { 33 | 34 | class Solver; 35 | 36 | /** 37 | * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg 38 | */ 39 | class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm 40 | { 41 | public: 42 | explicit OptimizationAlgorithmWithHessian(Solver* solver); 43 | virtual ~OptimizationAlgorithmWithHessian(); 44 | 45 | virtual bool init(bool online = false); 46 | 47 | virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); 48 | 49 | virtual bool buildLinearStructure(); 50 | 51 | virtual void updateLinearSystem(); 52 | 53 | virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); 54 | 55 | //! return the underlying solver used to solve the linear system 56 | Solver* solver() { return _solver;} 57 | 58 | /** 59 | * write debug output of the Hessian if system is not positive definite 60 | */ 61 | virtual void setWriteDebug(bool writeDebug); 62 | virtual bool writeDebug() const { return _writeDebug->value();} 63 | 64 | protected: 65 | Solver* _solver; 66 | Property* _writeDebug; 67 | 68 | }; 69 | 70 | }// end namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/parameter_container.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_ 28 | #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace g2o { 35 | 36 | class Parameter; 37 | 38 | /** 39 | * \brief map id to parameters 40 | */ 41 | class ParameterContainer : protected std::map 42 | { 43 | public: 44 | typedef std::map BaseClass; 45 | 46 | /** 47 | * create a container for the parameters. 48 | * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor 49 | */ 50 | ParameterContainer(bool isMainStorage_=true); 51 | virtual ~ParameterContainer(); 52 | //! add parameter to the container 53 | bool addParameter(Parameter* p); 54 | //! return a parameter based on its ID 55 | Parameter* getParameter(int id); 56 | //! remove a parameter from the container, i.e., the user now owns the pointer 57 | Parameter* detachParameter(int id); 58 | //! read parameters from a stream 59 | virtual bool read(std::istream& is, const std::map* renamedMap =0); 60 | //! write the data to a stream 61 | virtual bool write(std::ostream& os) const; 62 | bool isMainStorage() const {return _isMainStorage;} 63 | void clear(); 64 | 65 | // stuff of the base class that should re-appear 66 | using BaseClass::size; 67 | 68 | protected: 69 | bool _isMainStorage; 70 | }; 71 | 72 | } // end namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/robust_kernel.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_ROBUST_KERNEL_H 28 | #define G2O_ROBUST_KERNEL_H 29 | 30 | #ifdef _MSC_VER 31 | #include 32 | #else 33 | #include 34 | #endif 35 | #include 36 | 37 | 38 | namespace g2o { 39 | 40 | /** 41 | * \brief base for all robust cost functions 42 | * 43 | * Note in all the implementations for the other cost functions the e in the 44 | * funtions corresponds to the sqaured errors, i.e., the robustification 45 | * functions gets passed the squared error. 46 | * 47 | * e.g. the robustified least squares function is 48 | * 49 | * chi^2 = sum_{e} rho( e^T Omega e ) 50 | */ 51 | class RobustKernel 52 | { 53 | public: 54 | RobustKernel(); 55 | explicit RobustKernel(double delta); 56 | virtual ~RobustKernel() {} 57 | /** 58 | * compute the scaling factor for a error: 59 | * The error is e^T Omega e 60 | * The output rho is 61 | * rho[0]: The actual scaled error value 62 | * rho[1]: First derivative of the scaling function 63 | * rho[2]: Second derivative of the scaling function 64 | */ 65 | virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; 66 | 67 | /** 68 | * set the window size of the error. A squared error above delta^2 is considered 69 | * as outlier in the data. 70 | */ 71 | virtual void setDelta(double delta); 72 | double delta() const { return _delta;} 73 | 74 | protected: 75 | double _delta; 76 | }; 77 | typedef std::shared_ptr RobustKernelPtr; 78 | 79 | } // end namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /examples/EurocStereoVIO.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # Dataset dirs 4 | LeftFolder: /home/gaoxiang/dataset/euroc/V1_01_easy/cam0/data 5 | RightFolder: /home/gaoxiang/dataset/euroc/V1_01_easy/cam1/data 6 | IMUFolder: /home/gaoxiang/dataset/euroc/V1_01_easy/imu0/data.csv 7 | TimeFolder: /home/gaoxiang/code/ygz-stereo-inertial/examples/EuRoC_TimeStamps/V101.txt 8 | 9 | # if running in pure stereo vision mode 10 | PureVisionMode: false 11 | 12 | # do we need visualization? 13 | UseViewer: true 14 | 15 | # Camera calibration and distortion parameters (OpenCV) 16 | Camera.fx: 435.2046959714599 17 | Camera.fy: 435.2046959714599 18 | Camera.cx: 367.4517211914062 19 | Camera.cy: 252.2008514404297 20 | 21 | Camera.k1: 0.0 22 | Camera.k2: 0.0 23 | Camera.p1: 0.0 24 | Camera.p2: 0.0 25 | 26 | Camera.width: 752 27 | Camera.height: 480 28 | 29 | # stereo baseline times fx 30 | Camera.bf: 47.90639384423901 31 | 32 | # left cam to IMU transform 33 | RBC: !!opencv-matrix 34 | rows: 3 35 | cols: 3 36 | dt: d 37 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, 38 | 0.999557249008, 0.0149672133247, 0.025715529948, 39 | -0.0257744366974, 0.00375618835797, 0.999660727178] 40 | 41 | TBC: !!opencv-matrix 42 | rows: 3 43 | cols: 1 44 | dt: d 45 | data: [-0.0216401454975, -0.064676986768, 0.00981073058949] 46 | 47 | #-------------------------------------------------------------------------------------------- 48 | # Stereo Rectification. Only if you need to pre-rectify the images. 49 | # Camera.fx, .fy, etc must be the same as in LEFT.P 50 | #-------------------------------------------------------------------------------------------- 51 | LEFT.height: 480 52 | LEFT.width: 752 53 | LEFT.D: !!opencv-matrix 54 | rows: 1 55 | cols: 5 56 | dt: d 57 | data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 58 | LEFT.K: !!opencv-matrix 59 | rows: 3 60 | cols: 3 61 | dt: d 62 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 63 | LEFT.R: !!opencv-matrix 64 | rows: 3 65 | cols: 3 66 | dt: d 67 | data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 68 | LEFT.P: !!opencv-matrix 69 | rows: 3 70 | cols: 4 71 | dt: d 72 | data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 73 | 74 | RIGHT.height: 480 75 | RIGHT.width: 752 76 | RIGHT.D: !!opencv-matrix 77 | rows: 1 78 | cols: 5 79 | dt: d 80 | data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0] 81 | RIGHT.K: !!opencv-matrix 82 | rows: 3 83 | cols: 3 84 | dt: d 85 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1] 86 | RIGHT.R: !!opencv-matrix 87 | rows: 3 88 | cols: 3 89 | dt: d 90 | data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644] 91 | RIGHT.P: !!opencv-matrix 92 | rows: 3 93 | cols: 4 94 | dt: d 95 | data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 96 | -------------------------------------------------------------------------------- /examples/EurocStereo.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This is the Euroc stereo visual odometry program 3 | * Please specify the dataset directory in the config file 4 | */ 5 | 6 | #include 7 | 8 | #include "ygz/System.h" 9 | #include "ygz/EurocReader.h" 10 | 11 | using namespace std; 12 | using namespace ygz; 13 | 14 | int main(int argc, char **argv) { 15 | if (argc != 2) { 16 | LOG(INFO) << "Usage: EurocStereo path_to_config" << endl; 17 | return 1; 18 | } 19 | google::InitGoogleLogging(argv[0]); 20 | 21 | string configFile(argv[1]); 22 | cv::FileStorage fsSettings(configFile, cv::FileStorage::READ); 23 | if (fsSettings.isOpened() == false) { 24 | LOG(FATAL) << "Cannot load the config file from " << argv[1] << endl; 25 | } 26 | 27 | System system(argv[1]); 28 | 29 | // rectification parameters 30 | cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; 31 | fsSettings["LEFT.K"] >> K_l; 32 | fsSettings["RIGHT.K"] >> K_r; 33 | 34 | fsSettings["LEFT.P"] >> P_l; 35 | fsSettings["RIGHT.P"] >> P_r; 36 | 37 | fsSettings["LEFT.R"] >> R_l; 38 | fsSettings["RIGHT.R"] >> R_r; 39 | 40 | fsSettings["LEFT.D"] >> D_l; 41 | fsSettings["RIGHT.D"] >> D_r; 42 | 43 | int rows_l = fsSettings["LEFT.height"]; 44 | int cols_l = fsSettings["LEFT.width"]; 45 | int rows_r = fsSettings["RIGHT.height"]; 46 | int cols_r = fsSettings["RIGHT.width"]; 47 | 48 | if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || 49 | D_r.empty() || 50 | rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) { 51 | cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; 52 | return 1; 53 | } 54 | 55 | cv::Mat M1l, M2l, M1r, M2r; 56 | cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, M1l, 57 | M2l); 58 | cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, M1r, 59 | M2r); 60 | 61 | string leftFolder = fsSettings["LeftFolder"]; 62 | string rightFolder = fsSettings["RightFolder"]; 63 | string timeFolder = fsSettings["TimeFolder"]; 64 | 65 | vector vstrImageLeft; 66 | vector vstrImageRight; 67 | vector vTimeStamp; 68 | if (LoadImages(leftFolder, rightFolder, timeFolder, vstrImageLeft, vstrImageRight, vTimeStamp) == false) 69 | return 1; 70 | 71 | for (size_t i = 0; i < vstrImageLeft.size(); i++) { 72 | cv::Mat imLeft, imRight, imLeftRect, imRightRect; 73 | 74 | // Read left and right images from file 75 | imLeft = cv::imread(vstrImageLeft[i], CV_LOAD_IMAGE_UNCHANGED); 76 | imRight = cv::imread(vstrImageRight[i], CV_LOAD_IMAGE_UNCHANGED); 77 | 78 | if (imLeft.empty() || imRight.empty()) { 79 | LOG(WARNING) << "Cannot load image " << i << endl; 80 | continue; 81 | } 82 | 83 | cv::remap(imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR); 84 | cv::remap(imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR); 85 | 86 | system.AddStereo(imLeftRect, imRightRect, vTimeStamp[i]); 87 | } 88 | 89 | 90 | return 0; 91 | 92 | } -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/openmp_mutex.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPENMP_MUTEX 28 | #define G2O_OPENMP_MUTEX 29 | 30 | #include "../../config.h" 31 | 32 | #ifdef G2O_OPENMP 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | namespace g2o { 39 | 40 | #ifdef G2O_OPENMP 41 | 42 | /** 43 | * \brief Mutex realized via OpenMP 44 | */ 45 | class OpenMPMutex 46 | { 47 | public: 48 | OpenMPMutex() { omp_init_lock(&_lock); } 49 | ~OpenMPMutex() { omp_destroy_lock(&_lock); } 50 | void lock() { omp_set_lock(&_lock); } 51 | void unlock() { omp_unset_lock(&_lock); } 52 | protected: 53 | omp_lock_t _lock; 54 | }; 55 | 56 | #else 57 | 58 | /* 59 | * dummy which does nothing in case we don't have OpenMP support. 60 | * In debug mode, the mutex allows to verify the correct lock and unlock behavior 61 | */ 62 | class OpenMPMutex 63 | { 64 | public: 65 | #ifdef NDEBUG 66 | OpenMPMutex() {} 67 | #else 68 | OpenMPMutex() : _cnt(0) {} 69 | #endif 70 | ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} 71 | void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} 72 | void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} 73 | protected: 74 | #ifndef NDEBUG 75 | char _cnt; 76 | #endif 77 | }; 78 | 79 | #endif 80 | 81 | /** 82 | * \brief lock a mutex within a scope 83 | */ 84 | class ScopedOpenMPMutex 85 | { 86 | public: 87 | explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } 88 | ~ScopedOpenMPMutex() { _mutex->unlock(); } 89 | private: 90 | OpenMPMutex* const _mutex; 91 | ScopedOpenMPMutex(const ScopedOpenMPMutex&); 92 | void operator=(const ScopedOpenMPMutex&); 93 | }; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /system/src/System.cpp: -------------------------------------------------------------------------------- 1 | #include "ygz/System.h" 2 | #include "ygz/TrackerLK.h" 3 | #include "ygz/BackendSlidingWindowG2O.h" 4 | 5 | namespace ygz { 6 | 7 | System::System(const string &configPath) { 8 | 9 | // Read rectification parameters 10 | cv::FileStorage fsSettings(configPath, cv::FileStorage::READ); 11 | 12 | if (!fsSettings.isOpened()) { 13 | LOG(FATAL) << "ERROR: Wrong path to settings" << endl; 14 | return; 15 | } 16 | 17 | int rows_l = fsSettings["Camera.height"]; 18 | int cols_l = fsSettings["Camera.width"]; 19 | 20 | setting::imageHeight = rows_l; 21 | setting::imageWidth = cols_l; 22 | 23 | // Create camera object 24 | setting::initSettings(); 25 | float fx = fsSettings["Camera.fx"]; 26 | float fy = fsSettings["Camera.fy"]; 27 | float cx = fsSettings["Camera.cx"]; 28 | float cy = fsSettings["Camera.cy"]; 29 | float bf = fsSettings["Camera.bf"]; 30 | 31 | shared_ptr camera(new CameraParam(fx, fy, cx, cy, bf)); 32 | 33 | // create a tracker 34 | mpTracker = shared_ptr(new TrackerLK(configPath)); 35 | mpTracker->SetCamera(camera); 36 | 37 | bool pureVisionMode = string(fsSettings["PureVisionMode"]) == "true"; 38 | if (pureVisionMode) { 39 | LOG(INFO) << "Running in pure stereo vision mode!" << endl; 40 | mpTracker->SetPureVisionMode(true); 41 | } 42 | 43 | // create a backend 44 | mpBackend = shared_ptr(new BackendSlidingWindowG2O(mpTracker)); 45 | mpTracker->SetBackEnd(mpBackend); 46 | 47 | // create a viewer 48 | bool useViewer = string(fsSettings["UseViewer"]) == "true"; 49 | if (useViewer) { 50 | mpViewer = shared_ptr(new Viewer()); 51 | mpTracker->SetViewer(mpViewer); 52 | } 53 | 54 | LOG(INFO) << "YGZ system all ready, waiting for images ..." << endl; 55 | } 56 | 57 | System::~System() { 58 | setting::destroySettings(); 59 | } 60 | 61 | SE3d System::AddStereoIMU( 62 | const cv::Mat &imRectLeft, const cv::Mat &imRectRight, 63 | const double ×tamp, const VecIMU &vimu) { 64 | return mpTracker->InsertStereo(imRectLeft, imRectRight, timestamp, vimu); 65 | } 66 | 67 | SE3d System::AddStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectright, const double ×tamp) { 68 | return mpTracker->InsertStereo(imRectLeft, imRectright, timestamp, VecIMU() ); 69 | } 70 | 71 | void System::SetGroundTruthTrajectory(map, Eigen::aligned_allocator> &traj) { 72 | if ( mpViewer ) { 73 | map, Eigen::aligned_allocator> trajTrans; // 平移部分 74 | for ( auto& t: traj ) { 75 | trajTrans[t.first] = t.second.translation(); 76 | } 77 | mpViewer->SetGTTraj( trajTrans ); 78 | } 79 | } 80 | 81 | void System::Shutdown() { 82 | LOG(INFO) << "System shutdown" << endl; 83 | mpBackend->Shutdown(); 84 | if (mpViewer) { 85 | LOG(INFO) << "Please close the GUI to shutdown all the system" << endl; 86 | mpViewer->WaitToFinish(); 87 | } 88 | } 89 | 90 | } -------------------------------------------------------------------------------- /Thirdparty/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 | -------------------------------------------------------------------------------- /Thirdparty/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 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_se3_prior.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_EDGE_SE3_PRIOR_H_ 28 | #define G2O_EDGE_SE3_PRIOR_H_ 29 | 30 | #include "vertex_se3.h" 31 | #include "g2o/core/base_unary_edge.h" 32 | #include "parameter_se3_offset.h" 33 | #include "g2o_types_slam3d_api.h" 34 | namespace g2o { 35 | /** 36 | * \brief prior for an SE3 element 37 | * 38 | * Provides a prior for a 3d pose vertex. Again the measurement is represented by an 39 | * Isometry3D matrix. 40 | */ 41 | class G2O_TYPES_SLAM3D_API EdgeSE3Prior : public BaseUnaryEdge<6, Isometry3D, VertexSE3> { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | EdgeSE3Prior(); 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | // return the error estimate as a 3-vector 49 | void computeError(); 50 | 51 | // jacobian 52 | virtual void linearizeOplus(); 53 | 54 | virtual void setMeasurement(const Isometry3D& m){ 55 | _measurement = m; 56 | _inverseMeasurement = m.inverse(); 57 | } 58 | 59 | virtual bool setMeasurementData(const double* d){ 60 | Eigen::Map v(d); 61 | _measurement = internal::fromVectorQT(v); 62 | _inverseMeasurement = _measurement.inverse(); 63 | return true; 64 | } 65 | 66 | virtual bool getMeasurementData(double* d) const{ 67 | Eigen::Map v(d); 68 | v = internal::toVectorQT(_measurement); 69 | return true; 70 | } 71 | 72 | virtual int measurementDimension() const {return 7;} 73 | 74 | virtual bool setMeasurementFromState() ; 75 | 76 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 77 | OptimizableGraph::Vertex* /*to*/) { 78 | return 1.; 79 | } 80 | 81 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 82 | protected: 83 | Isometry3D _inverseMeasurement; 84 | virtual bool resolveCaches(); 85 | ParameterSE3Offset* _offsetParam; 86 | CacheSE3Offset* _cache; 87 | }; 88 | 89 | } 90 | #endif 91 | -------------------------------------------------------------------------------- /cmake-modules/FindEigen3.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 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_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 Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_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(EIGEN3_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(EIGEN3_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(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_se3_pointxyz_depth.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_EDGE_PROJECT_DEPTH_H_ 28 | #define G2O_EDGE_PROJECT_DEPTH_H_ 29 | 30 | #include "g2o/core/base_binary_edge.h" 31 | 32 | #include "vertex_se3.h" 33 | #include "vertex_pointxyz.h" 34 | #include "parameter_camera.h" 35 | #include "g2o_types_slam3d_api.h" 36 | 37 | namespace g2o { 38 | 39 | /*! \class EdgeProjectDepth 40 | * \brief g2o edge from a track to a depth camera node using a depth measurement (true distance, not disparity) 41 | */ 42 | class G2O_TYPES_SLAM3D_API EdgeSE3PointXYZDepth : public BaseBinaryEdge<3, Vector3D, VertexSE3, VertexPointXYZ> { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | EdgeSE3PointXYZDepth(); 46 | virtual bool read(std::istream& is); 47 | virtual bool write(std::ostream& os) const; 48 | 49 | // return the error estimate as a 3-vector 50 | void computeError(); 51 | // jacobian 52 | virtual void linearizeOplus(); 53 | 54 | 55 | virtual void setMeasurement(const Vector3D& m){ 56 | _measurement = m; 57 | } 58 | 59 | virtual bool setMeasurementData(const double* d){ 60 | Eigen::Map v(d); 61 | _measurement = v; 62 | return true; 63 | } 64 | 65 | virtual bool getMeasurementData(double* d) const{ 66 | Eigen::Map v(d); 67 | v=_measurement; 68 | return true; 69 | } 70 | 71 | virtual int measurementDimension() const {return 3;} 72 | 73 | virtual bool setMeasurementFromState() ; 74 | 75 | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, 76 | OptimizableGraph::Vertex* to) { 77 | (void) to; 78 | return (from.count(_vertices[0]) == 1 ? 1.0 : -1.0); 79 | } 80 | 81 | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 82 | 83 | private: 84 | Eigen::Matrix J; // jacobian before projection 85 | 86 | virtual bool resolveCaches(); 87 | ParameterCamera* params; 88 | CacheCamera* cache; 89 | }; 90 | 91 | } 92 | #endif 93 | -------------------------------------------------------------------------------- /Thirdparty/g2o/cmake_modules/FindEigen3.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 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_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 Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_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(EIGEN3_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(EIGEN3_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(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/parameter_camera.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CAMERA_PARAMETERS_H_ 28 | #define G2O_CAMERA_PARAMETERS_H_ 29 | 30 | #include "g2o/core/hyper_graph_action.h" 31 | #include "parameter_se3_offset.h" 32 | #include "g2o_types_slam3d_api.h" 33 | 34 | namespace g2o { 35 | /** 36 | * \brief parameters for a camera 37 | */ 38 | class G2O_TYPES_SLAM3D_API ParameterCamera: public ParameterSE3Offset { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | ParameterCamera(); 42 | void setKcam(double fx, double fy, double cx, double cy); 43 | void setOffset(const Isometry3D& offset_ = Isometry3D::Identity()); 44 | 45 | virtual bool read(std::istream& is); 46 | virtual bool write(std::ostream& os) const; 47 | 48 | const Matrix3D& Kcam() const { return _Kcam;} 49 | const Matrix3D& invKcam() const { return _invKcam;} 50 | const Matrix3D& Kcam_inverseOffsetR() const { return _Kcam_inverseOffsetR;} 51 | 52 | protected: 53 | Matrix3D _Kcam; 54 | Matrix3D _invKcam; 55 | Matrix3D _Kcam_inverseOffsetR; 56 | }; 57 | 58 | class G2O_TYPES_SLAM3D_API CacheCamera: public CacheSE3Offset { 59 | public: 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 61 | //! parameters of the camera 62 | const ParameterCamera* camParams() const {return params;} 63 | //! return the world to image transform 64 | const Affine3D& w2i() const {return _w2i;} 65 | 66 | protected: 67 | virtual void updateImpl(); 68 | virtual bool resolveDependancies(); 69 | Affine3D _w2i; ///< world to image transform 70 | ParameterCamera* params; 71 | }; 72 | 73 | 74 | #ifdef G2O_HAVE_OPENGL 75 | class G2O_TYPES_SLAM3D_API CacheCameraDrawAction: public DrawAction{ 76 | public: 77 | CacheCameraDrawAction(); 78 | virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_ ); 79 | protected: 80 | virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_); 81 | FloatProperty* _cameraZ, *_cameraSide; 82 | }; 83 | #endif 84 | 85 | } // end namespace 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/matrix_operations.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_CORE_MATRIX_OPERATIONS_H 28 | #define G2O_CORE_MATRIX_OPERATIONS_H 29 | 30 | #include 31 | 32 | namespace g2o { 33 | namespace internal { 34 | 35 | template 36 | inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 37 | { 38 | y.segment(yoff) += A * x.segment(xoff); 39 | } 40 | 41 | template 42 | inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 43 | { 44 | y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); 45 | } 46 | 47 | template<> 48 | inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 49 | { 50 | y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); 51 | } 52 | 53 | template 54 | inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 55 | { 56 | y.segment(yoff) += A.transpose() * x.segment(xoff); 57 | } 58 | 59 | template 60 | inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 61 | { 62 | y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); 63 | } 64 | 65 | template<> 66 | inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) 67 | { 68 | y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); 69 | } 70 | 71 | } // end namespace internal 72 | } // end namespace g2o 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/types/edge_pointxyz.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_EDGE_POINTXYZ_H 28 | #define G2O_EDGE_POINTXYZ_H 29 | 30 | #include "vertex_pointxyz.h" 31 | #include "config.h" 32 | #include "g2o/core/base_binary_edge.h" 33 | #include "g2o_types_slam3d_api.h" 34 | 35 | using namespace Eigen; 36 | 37 | namespace g2o { 38 | 39 | class G2O_TYPES_SLAM3D_API EdgePointXYZ : public BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexPointXYZ> { 40 | public: 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | 43 | EdgePointXYZ(); 44 | 45 | void computeError() { 46 | const VertexPointXYZ *v1 = static_cast(_vertices[0]); 47 | const VertexPointXYZ *v2 = static_cast(_vertices[1]); 48 | _error = (v2->estimate() - v1->estimate()) - _measurement; 49 | } 50 | 51 | virtual bool read(std::istream &is); 52 | 53 | virtual bool write(std::ostream &os) const; 54 | 55 | virtual void setMeasurement(const Vector3d &m) { 56 | _measurement = m; 57 | } 58 | 59 | virtual bool setMeasurementData(const double *d) { 60 | _measurement = Vector3d(d[0], d[1], d[2]); 61 | return true; 62 | } 63 | 64 | virtual bool getMeasurementData(double *d) const { 65 | Eigen::Map m(d); 66 | m = _measurement; 67 | return true; 68 | } 69 | 70 | virtual int measurementDimension() const { return 3; } 71 | 72 | virtual bool setMeasurementFromState() { 73 | const VertexPointXYZ *v1 = static_cast(_vertices[0]); 74 | const VertexPointXYZ *v2 = static_cast(_vertices[1]); 75 | _measurement = v2->estimate() - v1->estimate(); 76 | return true; 77 | } 78 | 79 | 80 | virtual double 81 | initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *) { return 0.; } 82 | 83 | #ifndef NUMERIC_JACOBIAN_THREE_D_TYPES 84 | 85 | virtual void linearizeOplus(); 86 | 87 | #endif 88 | }; 89 | 90 | 91 | } // end namespace 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/jacobian_workspace.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef JACOBIAN_WORKSPACE_H 28 | #define JACOBIAN_WORKSPACE_H 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include "hyper_graph.h" 37 | 38 | namespace g2o { 39 | 40 | struct OptimizableGraph; 41 | 42 | /** 43 | * \brief provide memory workspace for computing the Jacobians 44 | * 45 | * The workspace is used by an OptimizableGraph to provide temporary memory 46 | * for computing the Jacobian of the error functions. 47 | * Before calling linearizeOplus on an edge, the workspace needs to be allocated 48 | * by calling allocate(). 49 | */ 50 | class JacobianWorkspace 51 | { 52 | public: 53 | typedef std::vector > WorkspaceVector; 54 | 55 | public: 56 | JacobianWorkspace(); 57 | ~JacobianWorkspace(); 58 | 59 | /** 60 | * allocate the workspace 61 | */ 62 | bool allocate(); 63 | 64 | /** 65 | * update the maximum required workspace needed by taking into account this edge 66 | */ 67 | void updateSize(const HyperGraph::Edge* e); 68 | 69 | /** 70 | * update the required workspace by looking at a full graph 71 | */ 72 | void updateSize(const OptimizableGraph& graph); 73 | 74 | /** 75 | * manually update with the given parameters 76 | */ 77 | void updateSize(int numVertices, int dimension); 78 | 79 | /** 80 | * return the workspace for a vertex in an edge 81 | */ 82 | double* workspaceForVertex(int vertexIndex) 83 | { 84 | assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); 85 | return _workspace[vertexIndex].data(); 86 | } 87 | 88 | protected: 89 | WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians 90 | int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge 91 | int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian 92 | }; 93 | 94 | } // end namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/batch_stats.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "batch_stats.h" 28 | #include 29 | 30 | namespace g2o { 31 | using namespace std; 32 | 33 | G2OBatchStatistics* G2OBatchStatistics::_globalStats=0; 34 | 35 | #ifndef PTHING 36 | #define PTHING(s) \ 37 | #s << "= " << (st.s) << "\t " 38 | #endif 39 | 40 | G2OBatchStatistics::G2OBatchStatistics(){ 41 | // zero all. 42 | memset (this, 0, sizeof(G2OBatchStatistics)); 43 | 44 | // set the iteration to -1 to show that it isn't valid 45 | iteration = -1; 46 | } 47 | 48 | std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st) 49 | { 50 | os << PTHING(iteration); 51 | 52 | os << PTHING( numVertices ); // how many vertices are involved 53 | os << PTHING( numEdges ); // hoe many edges 54 | os << PTHING( chi2 ); // total chi2 55 | 56 | /** timings **/ 57 | // nonlinear part 58 | os << PTHING( timeResiduals ); 59 | os << PTHING( timeLinearize ); // jacobians 60 | os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph 61 | 62 | // block_solver (constructs Ax=b, plus maybe schur); 63 | os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done); 64 | 65 | // linear solver (computes Ax=b); ); 66 | os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done); 67 | os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done); 68 | os << PTHING( timeLinearSolution ); // total time for solving Ax=b 69 | os << PTHING( iterationsLinearSolver ); // iterations of PCG 70 | os << PTHING( timeUpdate ); // oplus 71 | os << PTHING( timeIteration ); // total time ); 72 | 73 | os << PTHING( levenbergIterations ); 74 | os << PTHING( timeLinearSolver); 75 | 76 | os << PTHING(hessianDimension); 77 | os << PTHING(hessianPoseDimension); 78 | os << PTHING(hessianLandmarkDimension); 79 | os << PTHING(choleskyNNZ); 80 | os << PTHING(timeMarginals); 81 | 82 | return os; 83 | }; 84 | 85 | void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b) 86 | { 87 | _globalStats = b; 88 | } 89 | 90 | } // end namespace 91 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/jacobian_workspace.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "jacobian_workspace.h" 28 | 29 | #include 30 | 31 | #include "optimizable_graph.h" 32 | 33 | using namespace std; 34 | 35 | namespace g2o { 36 | 37 | JacobianWorkspace::JacobianWorkspace() : 38 | _maxNumVertices(-1), _maxDimension(-1) 39 | { 40 | } 41 | 42 | JacobianWorkspace::~JacobianWorkspace() 43 | { 44 | } 45 | 46 | bool JacobianWorkspace::allocate() 47 | { 48 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 49 | if (_maxNumVertices <=0 || _maxDimension <= 0) 50 | return false; 51 | _workspace.resize(_maxNumVertices); 52 | for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { 53 | it->resize(_maxDimension); 54 | it->setZero(); 55 | } 56 | return true; 57 | } 58 | 59 | void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) 60 | { 61 | const OptimizableGraph::Edge* e = static_cast(e_); 62 | int errorDimension = e->dimension(); 63 | int numVertices = e->vertices().size(); 64 | int maxDimensionForEdge = -1; 65 | for (int i = 0; i < numVertices; ++i) { 66 | const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); 67 | assert(v && "Edge has no vertex assigned"); 68 | maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); 69 | } 70 | _maxNumVertices = max(numVertices, _maxNumVertices); 71 | _maxDimension = max(maxDimensionForEdge, _maxDimension); 72 | //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; 73 | } 74 | 75 | void JacobianWorkspace::updateSize(const OptimizableGraph& graph) 76 | { 77 | for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { 78 | const OptimizableGraph::Edge* e = static_cast(*it); 79 | updateSize(e); 80 | } 81 | } 82 | 83 | void JacobianWorkspace::updateSize(int numVertices, int dimension) 84 | { 85 | _maxNumVertices = max(numVertices, _maxNumVertices); 86 | _maxDimension = max(dimension, _maxDimension); 87 | } 88 | 89 | } // end namespace 90 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "sparse_block_matrix.h" 28 | #include 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | using namespace Eigen; 33 | 34 | typedef SparseBlockMatrix< MatrixXd > 35 | SparseBlockMatrixX; 36 | 37 | std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { 38 | for (int i=0; iblock(0,0, true); 55 | cerr << b->rows() << " " << b->cols() << endl; 56 | for (int i=0; irows(); ++i) 57 | for (int j=0; jcols(); ++j){ 58 | (*b)(i,j)=i*b->cols()+j; 59 | } 60 | 61 | 62 | cerr << "block access 2" << endl; 63 | b=M->block(0,2, true); 64 | cerr << b->rows() << " " << b->cols() << endl; 65 | for (int i=0; irows(); ++i) 66 | for (int j=0; jcols(); ++j){ 67 | (*b)(i,j)=i*b->cols()+j; 68 | } 69 | 70 | b=M->block(3,2, true); 71 | cerr << b->rows() << " " << b->cols() << endl; 72 | for (int i=0; irows(); ++i) 73 | for (int j=0; jcols(); ++j){ 74 | (*b)(i,j)=i*b->cols()+j; 75 | } 76 | 77 | cerr << *M << endl; 78 | 79 | cerr << "SUM" << endl; 80 | 81 | SparseBlockMatrixX* Ms=0; 82 | M->add(Ms); 83 | M->add(Ms); 84 | cerr << *Ms; 85 | 86 | SparseBlockMatrixX* Mt=0; 87 | M->transpose(Mt); 88 | cerr << *Mt << endl; 89 | 90 | SparseBlockMatrixX* Mp=0; 91 | M->multiply(Mp, Mt); 92 | cerr << *Mp << endl; 93 | 94 | int iperm[]={3,2,1,0}; 95 | SparseBlockMatrixX* PMp=0; 96 | 97 | Mp->symmPermutation(PMp,iperm, false); 98 | cerr << *PMp << endl; 99 | 100 | PMp->clear(true); 101 | Mp->block(3,0)->fill(0.); 102 | Mp->symmPermutation(PMp,iperm, true); 103 | cerr << *PMp << endl; 104 | 105 | 106 | 107 | } 108 | -------------------------------------------------------------------------------- /cmake-modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 28 | #define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H 29 | 30 | #include "optimization_algorithm_with_hessian.h" 31 | 32 | namespace g2o { 33 | 34 | class BlockSolverBase; 35 | 36 | /** 37 | * \brief Implementation of Powell's Dogleg Algorithm 38 | */ 39 | class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian 40 | { 41 | public: 42 | /** \brief type of the step to take */ 43 | enum { 44 | STEP_UNDEFINED, 45 | STEP_SD, STEP_GN, STEP_DL 46 | }; 47 | 48 | public: 49 | /** 50 | * construct the Dogleg algorithm, which will use the given Solver for solving the 51 | * linearized system. 52 | */ 53 | explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); 54 | virtual ~OptimizationAlgorithmDogleg(); 55 | 56 | virtual SolverResult solve(int iteration, bool online = false); 57 | 58 | virtual void printVerbose(std::ostream& os) const; 59 | 60 | //! return the type of the last step taken by the algorithm 61 | int lastStep() const { return _lastStep;} 62 | //! return the diameter of the trust region 63 | double trustRegion() const { return _delta;} 64 | 65 | //! convert the type into an integer 66 | static const char* stepType2Str(int stepType); 67 | 68 | protected: 69 | // parameters 70 | Property* _maxTrialsAfterFailure; 71 | Property* _userDeltaInit; 72 | // damping to enforce positive definite matrix 73 | Property* _initialLambda; 74 | Property* _lamdbaFactor; 75 | 76 | Eigen::VectorXd _hsd; ///< steepest decent step 77 | Eigen::VectorXd _hdl; ///< final dogleg step 78 | Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff 79 | 80 | double _currentLambda; ///< the damping factor to force positive definite matrix 81 | double _delta; ///< trust region 82 | int _lastStep; ///< type of the step taken by the algorithm 83 | bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping 84 | int _lastNumTries; 85 | }; 86 | 87 | } // end namespace 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /Thirdparty/g2o/g2o/stuff/property.cpp: -------------------------------------------------------------------------------- 1 | // g2o - General Graph Optimization 2 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are 7 | // met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, 10 | // 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 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | #include "property.h" 28 | 29 | #include 30 | #include 31 | 32 | #include "macros.h" 33 | 34 | #include "string_tools.h" 35 | using namespace std; 36 | 37 | namespace g2o { 38 | 39 | BaseProperty::BaseProperty(const std::string name_) :_name(name_){ 40 | } 41 | 42 | BaseProperty::~BaseProperty(){} 43 | 44 | bool PropertyMap::addProperty(BaseProperty* p) { 45 | std::pair result = insert(make_pair(p->name(), p)); 46 | return result.second; 47 | } 48 | 49 | bool PropertyMap::eraseProperty(const std::string& name) { 50 | PropertyMapIterator it=find(name); 51 | if (it==end()) 52 | return false; 53 | delete it->second; 54 | erase(it); 55 | return true; 56 | } 57 | 58 | PropertyMap::~PropertyMap() { 59 | for (PropertyMapIterator it=begin(); it!=end(); it++){ 60 | if (it->second) 61 | delete it->second; 62 | } 63 | } 64 | 65 | bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) 66 | { 67 | PropertyMapIterator it = find(name); 68 | if (it == end()) 69 | return false; 70 | it->second->fromString(value); 71 | return true; 72 | } 73 | 74 | void PropertyMap::writeToCSV(std::ostream& os) const 75 | { 76 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 77 | BaseProperty* p =it->second; 78 | os << p->name() << ", "; 79 | } 80 | os << std::endl; 81 | for (PropertyMapConstIterator it=begin(); it!=end(); it++){ 82 | BaseProperty* p =it->second; 83 | os << p->toString() << ", "; 84 | } 85 | os << std::endl; 86 | } 87 | 88 | bool PropertyMap::updateMapFromString(const std::string& values) 89 | { 90 | bool status = true; 91 | vector valuesMap = strSplit(values, ","); 92 | for (size_t i = 0; i < valuesMap.size(); ++i) { 93 | vector m = strSplit(valuesMap[i], "="); 94 | if (m.size() != 2) { 95 | cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; 96 | continue; 97 | } 98 | string name = trim(m[0]); 99 | string value = trim(m[1]); 100 | status = status && updatePropertyFromString(name, value); 101 | } 102 | return status; 103 | } 104 | 105 | } // end namespace 106 | --------------------------------------------------------------------------------