├── .gitignore
├── .gitmodules
├── CHANGELOG
├── CMakeLists.txt
├── LICENSE
├── README.md
├── cmake
├── FindEigen3.cmake
├── FindLibZip.cmake
└── FindSuiteParse.cmake
├── src
├── FullSystem
│ ├── CoarseInitializer.cpp
│ ├── CoarseInitializer.h
│ ├── CoarseTracker.cpp
│ ├── CoarseTracker.h
│ ├── FullSystem.cpp
│ ├── FullSystem.h
│ ├── FullSystemDebugStuff.cpp
│ ├── FullSystemMarginalize.cpp
│ ├── FullSystemOptPoint.cpp
│ ├── FullSystemOptimize.cpp
│ ├── HessianBlocks.cpp
│ ├── HessianBlocks.h
│ ├── ImmaturePoint.cpp
│ ├── ImmaturePoint.h
│ ├── PixelSelector.h
│ ├── PixelSelector2.cpp
│ ├── PixelSelector2.h
│ ├── ResidualProjections.h
│ ├── Residuals.cpp
│ └── Residuals.h
├── IOWrapper
│ ├── ImageDisplay.h
│ ├── ImageDisplay_dummy.cpp
│ ├── ImageRW.h
│ ├── ImageRW_dummy.cpp
│ ├── OpenCV
│ │ ├── ImageDisplay_OpenCV.cpp
│ │ └── ImageRW_OpenCV.cpp
│ ├── Output3DWrapper.h
│ ├── OutputWrapper
│ │ └── SampleOutputWrapper.h
│ └── Pangolin
│ │ ├── KeyFrameDisplay.cpp
│ │ ├── KeyFrameDisplay.h
│ │ ├── PangolinDSOViewer.cpp
│ │ └── PangolinDSOViewer.h
├── OptimizationBackend
│ ├── AccumulatedSCHessian.cpp
│ ├── AccumulatedSCHessian.h
│ ├── AccumulatedTopHessian.cpp
│ ├── AccumulatedTopHessian.h
│ ├── EnergyFunctional.cpp
│ ├── EnergyFunctional.h
│ ├── EnergyFunctionalStructs.cpp
│ ├── EnergyFunctionalStructs.h
│ ├── MatrixAccumulators.h
│ └── RawResidualJacobian.h
├── main_dso_pangolin.cpp
└── util
│ ├── DatasetReader.h
│ ├── FrameShell.h
│ ├── ImageAndExposure.h
│ ├── IndexThreadReduce.h
│ ├── MinimalImage.h
│ ├── NumType.h
│ ├── Undistort.cpp
│ ├── Undistort.h
│ ├── globalCalib.cpp
│ ├── globalCalib.h
│ ├── globalFuncs.h
│ ├── nanoflann.h
│ ├── settings.cpp
│ └── settings.h
└── thirdparty
├── Sophus
├── CMakeLists.txt
├── FindEigen3.cmake
├── README
├── SophusConfig.cmake.in
├── cmake_modules
│ └── FindEigen3.cmake
└── sophus
│ ├── rxso3.hpp
│ ├── se2.hpp
│ ├── se3.hpp
│ ├── sim3.hpp
│ ├── so2.hpp
│ ├── so3.hpp
│ ├── sophus.hpp
│ ├── test_rxso3.cpp
│ ├── test_se2.cpp
│ ├── test_se3.cpp
│ ├── test_sim3.cpp
│ ├── test_so2.cpp
│ ├── test_so3.cpp
│ └── tests.hpp
└── libzip-1.1.1.tar.gz
/.gitignore:
--------------------------------------------------------------------------------
1 | *.pro
2 | *.pro.user
3 | *.pro.user*
4 | build*
5 | build-*
6 | *.o
7 | *.so
8 | *.a
9 | *.so.*
10 | thirdparty/libzip-1.1.1
--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "thirdparty/sse2neon"]
2 | path = thirdparty/sse2neon
3 | url = https://github.com/jratcliff63367/sse2neon.git
4 |
--------------------------------------------------------------------------------
/CHANGELOG:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | ========= 16.11.2016 ==========
5 | * Added Sample output wrapper IOWrapper/OutputWrapper/SampleOutputWrapper.h.
6 | * Added extensive comments to IOWrapper/Output3DWrapper.h.
7 | * Added support for multiple 3DOutputWrapper simulataneously.
8 | * Added options "quiet=1" and "sampleoutput=1".
9 | * Did some minor code cleaning.
10 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | SET(PROJECT_NAME DSO)
2 |
3 | PROJECT(${PROJECT_NAME})
4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
5 | #set(CMAKE_VERBOSE_MAKEFILE ON)
6 |
7 |
8 | set(BUILD_TYPE Release)
9 | #set(BUILD_TYPE RelWithDebInfo)
10 |
11 | set(EXECUTABLE_OUTPUT_PATH bin)
12 | set(LIBRARY_OUTPUT_PATH lib)
13 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
14 |
15 | # required libraries
16 | find_package(SuiteParse REQUIRED)
17 | find_package(Eigen3 REQUIRED)
18 | find_package(Boost COMPONENTS system thread)
19 |
20 | # optional libraries
21 | find_package(LibZip QUIET)
22 | find_package(Pangolin 0.2 QUIET)
23 | find_package(OpenCV QUIET)
24 |
25 | # flags
26 | add_definitions("-DENABLE_SSE")
27 | set(CMAKE_CXX_FLAGS
28 | "${SSE_FLAGS} -O3 -g -std=c++0x -march=native"
29 | # "${SSE_FLAGS} -O3 -g -std=c++0x -fno-omit-frame-pointer"
30 | )
31 |
32 | if (MSVC)
33 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc")
34 | endif (MSVC)
35 |
36 | # Sources files
37 | set(dso_SOURCE_FILES
38 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystem.cpp
39 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptimize.cpp
40 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptPoint.cpp
41 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemDebugStuff.cpp
42 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemMarginalize.cpp
43 | ${PROJECT_SOURCE_DIR}/src/FullSystem/Residuals.cpp
44 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseTracker.cpp
45 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseInitializer.cpp
46 | ${PROJECT_SOURCE_DIR}/src/FullSystem/ImmaturePoint.cpp
47 | ${PROJECT_SOURCE_DIR}/src/FullSystem/HessianBlocks.cpp
48 | ${PROJECT_SOURCE_DIR}/src/FullSystem/PixelSelector2.cpp
49 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctional.cpp
50 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedTopHessian.cpp
51 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedSCHessian.cpp
52 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctionalStructs.cpp
53 | ${PROJECT_SOURCE_DIR}/src/util/settings.cpp
54 | ${PROJECT_SOURCE_DIR}/src/util/Undistort.cpp
55 | ${PROJECT_SOURCE_DIR}/src/util/globalCalib.cpp
56 | )
57 |
58 |
59 |
60 | include_directories(
61 | ${PROJECT_SOURCE_DIR}/src
62 | ${PROJECT_SOURCE_DIR}/thirdparty/Sophus
63 | ${PROJECT_SOURCE_DIR}/thirdparty/sse2neon
64 | ${EIGEN3_INCLUDE_DIR}
65 | )
66 |
67 |
68 | # decide if we have pangolin
69 | if (Pangolin_FOUND)
70 | message("--- found PANGOLIN, compiling dso_pangolin library.")
71 | include_directories( ${Pangolin_INCLUDE_DIRS} )
72 | set(dso_pangolin_SOURCE_FILES
73 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/KeyFrameDisplay.cpp
74 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/PangolinDSOViewer.cpp)
75 | set(HAS_PANGOLIN 1)
76 | else ()
77 | message("--- could not find PANGOLIN, not compiling dso_pangolin library.")
78 | message(" this means there will be no 3D display / GUI available for dso_dataset.")
79 | set(dso_pangolin_SOURCE_FILES )
80 | set(HAS_PANGOLIN 0)
81 | endif ()
82 |
83 | # decide if we have openCV
84 | if (OpenCV_FOUND)
85 | message("--- found OpenCV, compiling dso_opencv library.")
86 | include_directories( ${OpenCV_INCLUDE_DIRS} )
87 | set(dso_opencv_SOURCE_FILES
88 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp
89 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp)
90 | set(HAS_OPENCV 1)
91 | else ()
92 | message("--- could not find OpenCV, not compiling dso_opencv library.")
93 | message(" this means there will be no image display, and image read / load functionality.")
94 | set(dso_opencv_SOURCE_FILES
95 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageDisplay_dummy.cpp
96 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageRW_dummy.cpp)
97 | set(HAS_OPENCV 0)
98 | endif ()
99 |
100 | # decide if we have ziplib.
101 | if (LIBZIP_LIBRARY)
102 | message("--- found ziplib (${LIBZIP_VERSION}), compiling with zip capability.")
103 | add_definitions(-DHAS_ZIPLIB=1)
104 | include_directories( ${LIBZIP_INCLUDE_DIR_ZIP} ${LIBZIP_INCLUDE_DIR_ZIPCONF} )
105 | else()
106 | message("--- not found ziplib (${LIBZIP_LIBRARY}), compiling without zip capability.")
107 | set(LIBZIP_LIBRARY "")
108 | endif()
109 |
110 |
111 | # compile main library.
112 | include_directories( ${CSPARSE_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR})
113 | add_library(dso ${dso_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SOURCE_FILES})
114 |
115 | #set_property( TARGET dso APPEND_STRING PROPERTY COMPILE_FLAGS -Wall )
116 |
117 |
118 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX
119 | set(BOOST_THREAD_LIBRARY boost_thread-mt)
120 | else()
121 | set(BOOST_THREAD_LIBRARY boost_thread)
122 | endif()
123 |
124 | # build main executable (only if we have both OpenCV and Pangolin)
125 | if (OpenCV_FOUND AND Pangolin_FOUND)
126 | message("--- compiling dso_dataset.")
127 | add_executable(dso_dataset ${PROJECT_SOURCE_DIR}/src/main_dso_pangolin.cpp )
128 | target_link_libraries(dso_dataset dso boost_system cxsparse ${BOOST_THREAD_LIBRARY} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS})
129 | else()
130 | message("--- not building dso_dataset, since either don't have openCV or Pangolin.")
131 | endif()
132 |
133 |
--------------------------------------------------------------------------------
/cmake/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, <montel@kde.org>
14 | # Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
15 | # Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
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 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
65 | PATHS
66 | ${CMAKE_INSTALL_PREFIX}/include
67 | ${KDE4_INCLUDE_DIR}
68 | PATH_SUFFIXES eigen3 eigen
69 | )
70 |
71 | if(EIGEN3_INCLUDE_DIR)
72 | _eigen3_check_version()
73 | endif(EIGEN3_INCLUDE_DIR)
74 |
75 | include(FindPackageHandleStandardArgs)
76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
77 |
78 | mark_as_advanced(EIGEN3_INCLUDE_DIR)
79 |
80 | endif(EIGEN3_INCLUDE_DIR)
81 |
82 |
--------------------------------------------------------------------------------
/cmake/FindLibZip.cmake:
--------------------------------------------------------------------------------
1 | # Finds libzip.
2 | #
3 | # This module defines:
4 | # LIBZIP_INCLUDE_DIR_ZIP
5 | # LIBZIP_INCLUDE_DIR_ZIPCONF
6 | # LIBZIP_LIBRARY
7 | #
8 |
9 | find_package(PkgConfig)
10 | pkg_check_modules(PC_LIBZIP QUIET libzip)
11 |
12 | find_path(LIBZIP_INCLUDE_DIR_ZIP
13 | NAMES zip.h
14 | HINTS ${PC_LIBZIP_INCLUDE_DIRS})
15 |
16 | find_path(LIBZIP_INCLUDE_DIR_ZIPCONF
17 | NAMES zipconf.h
18 | HINTS ${PC_LIBZIP_INCLUDE_DIRS})
19 |
20 | find_library(LIBZIP_LIBRARY
21 | NAMES zip)
22 |
23 | include(FindPackageHandleStandardArgs)
24 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(
25 | LIBZIP DEFAULT_MSG
26 | LIBZIP_LIBRARY LIBZIP_INCLUDE_DIR_ZIP LIBZIP_INCLUDE_DIR_ZIPCONF)
27 |
28 | set(LIBZIP_VERSION 0)
29 |
30 | if (LIBZIP_INCLUDE_DIR_ZIPCONF)
31 | FILE(READ "${LIBZIP_INCLUDE_DIR_ZIPCONF}/zipconf.h" _LIBZIP_VERSION_CONTENTS)
32 | if (_LIBZIP_VERSION_CONTENTS)
33 | STRING(REGEX REPLACE ".*#define LIBZIP_VERSION \"([0-9.]+)\".*" "\\1" LIBZIP_VERSION "${_LIBZIP_VERSION_CONTENTS}")
34 | endif ()
35 | endif ()
36 |
37 | set(LIBZIP_VERSION ${LIBZIP_VERSION} CACHE STRING "Version number of libzip")
38 |
--------------------------------------------------------------------------------
/cmake/FindSuiteParse.cmake:
--------------------------------------------------------------------------------
1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h
2 | PATHS
3 | ${SUITE_SPARSE_ROOT}/include
4 | /usr/include/suitesparse
5 | /usr/include/ufsparse
6 | /opt/local/include/ufsparse
7 | /usr/local/include/ufsparse
8 | /sw/include/ufsparse
9 | )
10 |
11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod
12 | PATHS
13 | ${SUITE_SPARSE_ROOT}/lib
14 | /usr/lib
15 | /usr/local/lib
16 | /opt/local/lib
17 | /sw/lib
18 | )
19 |
20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd
21 | PATHS
22 | ${SUITE_SPARSE_ROOT}/lib
23 | /usr/lib
24 | /usr/local/lib
25 | /opt/local/lib
26 | /sw/lib
27 | )
28 |
29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd
30 | PATHS
31 | ${SUITE_SPARSE_ROOT}/lib
32 | /usr/lib
33 | /usr/local/lib
34 | /opt/local/lib
35 | /sw/lib
36 | )
37 |
38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
39 | PATHS
40 | ${SUITE_SPARSE_ROOT}/lib
41 | /usr/lib
42 | /usr/local/lib
43 | /opt/local/lib
44 | /sw/lib
45 | )
46 |
47 |
48 | # Different platforms seemingly require linking against different sets of libraries
49 | IF(CYGWIN)
50 | FIND_PACKAGE(PkgConfig)
51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
52 | PATHS
53 | /usr/lib
54 | /usr/local/lib
55 | /opt/local/lib
56 | /sw/lib
57 | )
58 | PKG_CHECK_MODULES(LAPACK lapack)
59 |
60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES})
61 |
62 | # MacPorts build of the SparseSuite requires linking against extra libraries
63 |
64 | ELSEIF(APPLE)
65 |
66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
67 | PATHS
68 | /usr/lib
69 | /usr/local/lib
70 | /opt/local/lib
71 | /sw/lib
72 | )
73 |
74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd
75 | PATHS
76 | /usr/lib
77 | /usr/local/lib
78 | /opt/local/lib
79 | /sw/lib
80 | )
81 |
82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis
83 | PATHS
84 | /usr/lib
85 | /usr/local/lib
86 | /opt/local/lib
87 | /sw/lib
88 | )
89 |
90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate")
91 | ELSE(APPLE)
92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY})
93 | ENDIF(CYGWIN)
94 |
95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
96 | SET(CHOLMOD_FOUND TRUE)
97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
98 | SET(CHOLMOD_FOUND FALSE)
99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
100 |
101 | # Look for csparse; note the difference in the directory specifications!
102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
103 | PATHS
104 | /usr/include/suitesparse
105 | /usr/include
106 | /opt/local/include
107 | /usr/local/include
108 | /sw/include
109 | /usr/include/ufsparse
110 | /opt/local/include/ufsparse
111 | /usr/local/include/ufsparse
112 | /sw/include/ufsparse
113 | )
114 |
115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
116 | PATHS
117 | /usr/lib
118 | /usr/local/lib
119 | /opt/local/lib
120 | /sw/lib
121 | )
122 |
123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
124 | SET(CSPARSE_FOUND TRUE)
125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
126 | SET(CSPARSE_FOUND FALSE)
127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
128 |
129 |
--------------------------------------------------------------------------------
/src/FullSystem/CoarseInitializer.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "util/NumType.h"
28 | #include "OptimizationBackend/MatrixAccumulators.h"
29 | #include "IOWrapper/Output3DWrapper.h"
30 | #include "util/settings.h"
31 | #include "vector"
32 | #include <math.h>
33 |
34 |
35 |
36 |
37 | namespace dso
38 | {
39 | struct CalibHessian;
40 | struct FrameHessian;
41 |
42 |
43 | struct Pnt
44 | {
45 | public:
46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
47 | // index in jacobian. never changes (actually, there is no reason why).
48 | float u,v;
49 |
50 | // idepth / isgood / energy during optimization.
51 | float idepth;
52 | bool isGood;
53 | Vec2f energy; // (UenergyPhotometric, energyRegularizer)
54 | bool isGood_new;
55 | float idepth_new;
56 | Vec2f energy_new;
57 |
58 | float iR;
59 | float iRSumNum;
60 |
61 | float lastHessian;
62 | float lastHessian_new;
63 |
64 | // max stepsize for idepth (corresponding to max. movement in pixel-space).
65 | float maxstep;
66 |
67 | // idx (x+y*w) of closest point one pyramid level above.
68 | int parent;
69 | float parentDist;
70 |
71 | // idx (x+y*w) of up to 10 nearest points in pixel space.
72 | int neighbours[10];
73 | float neighboursDist[10];
74 |
75 | float my_type;
76 | float outlierTH;
77 | };
78 |
79 | class CoarseInitializer {
80 | public:
81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
82 | CoarseInitializer(int w, int h);
83 | ~CoarseInitializer();
84 |
85 |
86 | void setFirst( CalibHessian* HCalib, FrameHessian* newFrameHessian);
87 | bool trackFrame(FrameHessian* newFrameHessian, std::vector<IOWrap::Output3DWrapper*> &wraps);
88 | void calcTGrads(FrameHessian* newFrameHessian);
89 |
90 | int frameID;
91 | bool fixAffine;
92 | bool printDebug;
93 |
94 | Pnt* points[PYR_LEVELS];
95 | int numPoints[PYR_LEVELS];
96 | AffLight thisToNext_aff;
97 | SE3 thisToNext;
98 |
99 |
100 | FrameHessian* firstFrame;
101 | FrameHessian* newFrame;
102 | private:
103 | Mat33 K[PYR_LEVELS];
104 | Mat33 Ki[PYR_LEVELS];
105 | double fx[PYR_LEVELS];
106 | double fy[PYR_LEVELS];
107 | double fxi[PYR_LEVELS];
108 | double fyi[PYR_LEVELS];
109 | double cx[PYR_LEVELS];
110 | double cy[PYR_LEVELS];
111 | double cxi[PYR_LEVELS];
112 | double cyi[PYR_LEVELS];
113 | int w[PYR_LEVELS];
114 | int h[PYR_LEVELS];
115 | void makeK(CalibHessian* HCalib);
116 |
117 | bool snapped;
118 | int snappedAt;
119 |
120 | // pyramid images & levels on all levels
121 | Eigen::Vector3f* dINew[PYR_LEVELS];
122 | Eigen::Vector3f* dIFist[PYR_LEVELS];
123 |
124 | Eigen::DiagonalMatrix<float, 8> wM;
125 |
126 | // temporary buffers for H and b.
127 | Vec10f* JbBuffer; // 0-7: sum(dd * dp). 8: sum(res*dd). 9: 1/(1+sum(dd*dd))=inverse hessian entry.
128 | Vec10f* JbBuffer_new;
129 |
130 | Accumulator9 acc9;
131 | Accumulator9 acc9SC;
132 |
133 |
134 | Vec3f dGrads[PYR_LEVELS];
135 |
136 | float alphaK;
137 | float alphaW;
138 | float regWeight;
139 | float couplingWeight;
140 |
141 | Vec3f calcResAndGS(
142 | int lvl,
143 | Mat88f &H_out, Vec8f &b_out,
144 | Mat88f &H_out_sc, Vec8f &b_out_sc,
145 | const SE3 &refToNew, AffLight refToNew_aff,
146 | bool plot);
147 | Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS.
148 | void optReg(int lvl);
149 |
150 | void propagateUp(int srcLvl);
151 | void propagateDown(int srcLvl);
152 | float rescale();
153 |
154 | void resetPoints(int lvl);
155 | void doStep(int lvl, float lambda, Vec8f inc);
156 | void applyStep(int lvl);
157 |
158 | void makeGradients(Eigen::Vector3f** data);
159 |
160 | void debugPlot(int lvl, std::vector<IOWrap::Output3DWrapper*> &wraps);
161 | void makeNN();
162 | };
163 |
164 |
165 |
166 |
167 | struct FLANNPointcloud
168 | {
169 | inline FLANNPointcloud() {num=0; points=0;}
170 | inline FLANNPointcloud(int n, Pnt* p) : num(n), points(p) {}
171 | int num;
172 | Pnt* points;
173 | inline size_t kdtree_get_point_count() const { return num; }
174 | inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const
175 | {
176 | const float d0=p1[0]-points[idx_p2].u;
177 | const float d1=p1[1]-points[idx_p2].v;
178 | return d0*d0+d1*d1;
179 | }
180 |
181 | inline float kdtree_get_pt(const size_t idx, int dim) const
182 | {
183 | if (dim==0) return points[idx].u;
184 | else return points[idx].v;
185 | }
186 | template <class BBOX>
187 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
188 | };
189 |
190 | }
191 |
192 |
193 |
--------------------------------------------------------------------------------
/src/FullSystem/CoarseTracker.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 | #include "vector"
30 | #include <math.h>
31 | #include "util/settings.h"
32 | #include "OptimizationBackend/MatrixAccumulators.h"
33 | #include "IOWrapper/Output3DWrapper.h"
34 |
35 |
36 |
37 |
38 | namespace dso
39 | {
40 | struct CalibHessian;
41 | struct FrameHessian;
42 | struct PointFrameResidual;
43 |
44 | class CoarseTracker {
45 | public:
46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
47 |
48 | CoarseTracker(int w, int h);
49 | ~CoarseTracker();
50 |
51 | bool trackNewestCoarse(
52 | FrameHessian* newFrameHessian,
53 | SE3 &lastToNew_out, AffLight &aff_g2l_out,
54 | int coarsestLvl, Vec5 minResForAbort,
55 | IOWrap::Output3DWrapper* wrap=0);
56 |
57 | void setCoarseTrackingRef(
58 | std::vector<FrameHessian*> frameHessians);
59 |
60 | void makeK(
61 | CalibHessian* HCalib);
62 |
63 | bool debugPrint, debugPlot;
64 |
65 | Mat33f K[PYR_LEVELS];
66 | Mat33f Ki[PYR_LEVELS];
67 | float fx[PYR_LEVELS];
68 | float fy[PYR_LEVELS];
69 | float fxi[PYR_LEVELS];
70 | float fyi[PYR_LEVELS];
71 | float cx[PYR_LEVELS];
72 | float cy[PYR_LEVELS];
73 | float cxi[PYR_LEVELS];
74 | float cyi[PYR_LEVELS];
75 | int w[PYR_LEVELS];
76 | int h[PYR_LEVELS];
77 |
78 | void debugPlotIDepthMap(float* minID, float* maxID, std::vector<IOWrap::Output3DWrapper*> &wraps);
79 | void debugPlotIDepthMapFloat(std::vector<IOWrap::Output3DWrapper*> &wraps);
80 |
81 | FrameHessian* lastRef;
82 | AffLight lastRef_aff_g2l;
83 | FrameHessian* newFrame;
84 | int refFrameID;
85 |
86 | // act as pure ouptut
87 | Vec5 lastResiduals;
88 | Vec3 lastFlowIndicators;
89 | double firstCoarseRMSE;
90 | private:
91 |
92 |
93 | void makeCoarseDepthL0(std::vector<FrameHessian*> frameHessians);
94 | float* idepth[PYR_LEVELS];
95 | float* weightSums[PYR_LEVELS];
96 | float* weightSums_bak[PYR_LEVELS];
97 |
98 |
99 | Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
100 | Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
101 | void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
102 | void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
103 |
104 | // pc buffers
105 | float* pc_u[PYR_LEVELS];
106 | float* pc_v[PYR_LEVELS];
107 | float* pc_idepth[PYR_LEVELS];
108 | float* pc_color[PYR_LEVELS];
109 | int pc_n[PYR_LEVELS];
110 |
111 | // warped buffers
112 | float* buf_warped_idepth;
113 | float* buf_warped_u;
114 | float* buf_warped_v;
115 | float* buf_warped_dx;
116 | float* buf_warped_dy;
117 | float* buf_warped_residual;
118 | float* buf_warped_weight;
119 | float* buf_warped_refColor;
120 | int buf_warped_n;
121 |
122 |
123 | std::vector<float*> ptrToDelete;
124 |
125 |
126 | Accumulator9 acc;
127 | };
128 |
129 |
130 | class CoarseDistanceMap {
131 | public:
132 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
133 |
134 | CoarseDistanceMap(int w, int h);
135 | ~CoarseDistanceMap();
136 |
137 | void makeDistanceMap(
138 | std::vector<FrameHessian*> frameHessians,
139 | FrameHessian* frame);
140 |
141 | void makeInlierVotes(
142 | std::vector<FrameHessian*> frameHessians);
143 |
144 | void makeK( CalibHessian* HCalib);
145 |
146 |
147 | float* fwdWarpedIDDistFinal;
148 |
149 | Mat33f K[PYR_LEVELS];
150 | Mat33f Ki[PYR_LEVELS];
151 | float fx[PYR_LEVELS];
152 | float fy[PYR_LEVELS];
153 | float fxi[PYR_LEVELS];
154 | float fyi[PYR_LEVELS];
155 | float cx[PYR_LEVELS];
156 | float cy[PYR_LEVELS];
157 | float cxi[PYR_LEVELS];
158 | float cyi[PYR_LEVELS];
159 | int w[PYR_LEVELS];
160 | int h[PYR_LEVELS];
161 |
162 | void addIntoDistFinal(int u, int v);
163 |
164 |
165 | private:
166 |
167 | PointFrameResidual** coarseProjectionGrid;
168 | int* coarseProjectionGridNum;
169 | Eigen::Vector2i* bfsList1;
170 | Eigen::Vector2i* bfsList2;
171 |
172 | void growDistBFS(int bfsNum);
173 | };
174 |
175 | }
176 |
177 |
--------------------------------------------------------------------------------
/src/FullSystem/FullSystem.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 | #define MAX_ACTIVE_FRAMES 100
27 |
28 | #include <deque>
29 | #include "util/NumType.h"
30 | #include "util/globalCalib.h"
31 | #include "vector"
32 |
33 | #include <iostream>
34 | #include <fstream>
35 | #include "util/NumType.h"
36 | #include "FullSystem/Residuals.h"
37 | #include "FullSystem/HessianBlocks.h"
38 | #include "util/FrameShell.h"
39 | #include "util/IndexThreadReduce.h"
40 | #include "OptimizationBackend/EnergyFunctional.h"
41 | #include "FullSystem/PixelSelector2.h"
42 |
43 | #include <math.h>
44 |
45 | namespace dso
46 | {
47 | namespace IOWrap
48 | {
49 | class Output3DWrapper;
50 | }
51 |
52 | class PixelSelector;
53 | class PCSyntheticPoint;
54 | class CoarseTracker;
55 | struct FrameHessian;
56 | struct PointHessian;
57 | class CoarseInitializer;
58 | struct ImmaturePointTemporaryResidual;
59 | class ImageAndExposure;
60 | class CoarseDistanceMap;
61 |
62 | class EnergyFunctional;
63 |
64 | template<typename T> inline void deleteOut(std::vector<T*> &v, const int i)
65 | {
66 | delete v[i];
67 | v[i] = v.back();
68 | v.pop_back();
69 | }
70 | template<typename T> inline void deleteOutPt(std::vector<T*> &v, const T* i)
71 | {
72 | delete i;
73 |
74 | for(unsigned int k=0;k<v.size();k++)
75 | if(v[k] == i)
76 | {
77 | v[k] = v.back();
78 | v.pop_back();
79 | }
80 | }
81 | template<typename T> inline void deleteOutOrder(std::vector<T*> &v, const int i)
82 | {
83 | delete v[i];
84 | for(unsigned int k=i+1; k<v.size();k++)
85 | v[k-1] = v[k];
86 | v.pop_back();
87 | }
88 | template<typename T> inline void deleteOutOrder(std::vector<T*> &v, const T* element)
89 | {
90 | int i=-1;
91 | for(unsigned int k=0; k<v.size();k++)
92 | {
93 | if(v[k] == element)
94 | {
95 | i=k;
96 | break;
97 | }
98 | }
99 | assert(i!=-1);
100 |
101 | for(unsigned int k=i+1; k<v.size();k++)
102 | v[k-1] = v[k];
103 | v.pop_back();
104 |
105 | delete element;
106 | }
107 |
108 |
109 | inline bool eigenTestNan(const MatXX &m, std::string msg)
110 | {
111 | bool foundNan = false;
112 | for(int y=0;y<m.rows();y++)
113 | for(int x=0;x<m.cols();x++)
114 | {
115 | if(!std::isfinite((double)m(y,x))) foundNan = true;
116 | }
117 |
118 | if(foundNan)
119 | {
120 | printf("NAN in %s:\n",msg.c_str());
121 | std::cout << m << "\n\n";
122 | }
123 |
124 |
125 | return foundNan;
126 | }
127 |
128 |
129 |
130 |
131 |
132 | class FullSystem {
133 | public:
134 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135 | FullSystem();
136 | virtual ~FullSystem();
137 |
138 | // adds a new frame, and creates point & residual structs.
139 | void addActiveFrame(ImageAndExposure* image, int id);
140 |
141 | // marginalizes a frame. drops / marginalizes points & residuals.
142 | void marginalizeFrame(FrameHessian* frame);
143 | void blockUntilMappingIsFinished();
144 |
145 | float optimize(int mnumOptIts);
146 |
147 | void printResult(std::string file);
148 |
149 | void debugPlot(std::string name);
150 |
151 | void printFrameLifetimes();
152 | // contains pointers to active frames
153 |
154 | std::vector<IOWrap::Output3DWrapper*> outputWrapper;
155 |
156 | bool isLost;
157 | bool initFailed;
158 | bool initialized;
159 | bool linearizeOperation;
160 |
161 |
162 | void setGammaFunction(float* BInv);
163 | void setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH);
164 |
165 | private:
166 |
167 | CalibHessian Hcalib;
168 |
169 |
170 |
171 |
172 | // opt single point
173 | int optimizePoint(PointHessian* point, int minObs, bool flagOOB);
174 | PointHessian* optimizeImmaturePoint(ImmaturePoint* point, int minObs, ImmaturePointTemporaryResidual* residuals);
175 |
176 | double linAllPointSinle(PointHessian* point, float outlierTHSlack, bool plot);
177 |
178 | // mainPipelineFunctions
179 | Vec4 trackNewCoarse(FrameHessian* fh);
180 | void traceNewCoarse(FrameHessian* fh);
181 | void activatePoints();
182 | void activatePointsMT();
183 | void activatePointsOldFirst();
184 | void flagPointsForRemoval();
185 | void makeNewTraces(FrameHessian* newFrame, float* gtDepth);
186 | void initializeFromInitializer(FrameHessian* newFrame);
187 | void flagFramesForMarginalization(FrameHessian* newFH);
188 |
189 |
190 | void removeOutliers();
191 |
192 |
193 | // set precalc values.
194 | void setPrecalcValues();
195 |
196 |
197 | // solce. eventually migrate to ef.
198 | void solveSystem(int iteration, double lambda);
199 | Vec3 linearizeAll(bool fixLinearization);
200 | bool doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD);
201 | void backupState(bool backupLastStep);
202 | void loadSateBackup();
203 | double calcLEnergy();
204 | double calcMEnergy();
205 | void linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid);
206 | void activatePointsMT_Reductor(std::vector<PointHessian*>* optimized,std::vector<ImmaturePoint*>* toOptimize,int min, int max, Vec10* stats, int tid);
207 | void applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid);
208 |
209 | void printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b);
210 |
211 | void debugPlotTracking();
212 |
213 | std::vector<VecX> getNullspaces(
214 | std::vector<VecX> &nullspaces_pose,
215 | std::vector<VecX> &nullspaces_scale,
216 | std::vector<VecX> &nullspaces_affA,
217 | std::vector<VecX> &nullspaces_affB);
218 |
219 | void setNewFrameEnergyTH();
220 |
221 |
222 | void printLogLine();
223 | void printEvalLine();
224 | void printEigenValLine();
225 | std::ofstream* calibLog;
226 | std::ofstream* numsLog;
227 | std::ofstream* errorsLog;
228 | std::ofstream* eigenAllLog;
229 | std::ofstream* eigenPLog;
230 | std::ofstream* eigenALog;
231 | std::ofstream* DiagonalLog;
232 | std::ofstream* variancesLog;
233 | std::ofstream* nullspacesLog;
234 |
235 | std::ofstream* coarseTrackingLog;
236 |
237 | // statistics
238 | long int statistics_lastNumOptIts;
239 | long int statistics_numDroppedPoints;
240 | long int statistics_numActivatedPoints;
241 | long int statistics_numCreatedPoints;
242 | long int statistics_numForceDroppedResBwd;
243 | long int statistics_numForceDroppedResFwd;
244 | long int statistics_numMargResFwd;
245 | long int statistics_numMargResBwd;
246 | float statistics_lastFineTrackRMSE;
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 | // =================== changed by tracker-thread. protected by trackMutex ============
255 | boost::mutex trackMutex;
256 | std::vector<FrameShell*> allFrameHistory;
257 | CoarseInitializer* coarseInitializer;
258 | Vec5 lastCoarseRMSE;
259 |
260 |
261 | // ================== changed by mapper-thread. protected by mapMutex ===============
262 | boost::mutex mapMutex;
263 | std::vector<FrameShell*> allKeyFramesHistory;
264 |
265 | EnergyFunctional* ef;
266 | IndexThreadReduce<Vec10> treadReduce;
267 |
268 | float* selectionMap;
269 | PixelSelector* pixelSelector;
270 | CoarseDistanceMap* coarseDistanceMap;
271 |
272 | std::vector<FrameHessian*> frameHessians; // ONLY changed in marginalizeFrame and addFrame.
273 | std::vector<PointFrameResidual*> activeResiduals;
274 | float currentMinActDist;
275 |
276 |
277 | std::vector<float> allResVec;
278 |
279 |
280 |
281 | // mutex etc. for tracker exchange.
282 | boost::mutex coarseTrackerSwapMutex; // if tracker sees that there is a new reference, tracker locks [coarseTrackerSwapMutex] and swaps the two.
283 | CoarseTracker* coarseTracker_forNewKF; // set as as reference. protected by [coarseTrackerSwapMutex].
284 | CoarseTracker* coarseTracker; // always used to track new frames. protected by [trackMutex].
285 | float minIdJetVisTracker, maxIdJetVisTracker;
286 | float minIdJetVisDebug, maxIdJetVisDebug;
287 |
288 |
289 |
290 |
291 |
292 | // mutex for camToWorl's in shells (these are always in a good configuration).
293 | boost::mutex shellPoseMutex;
294 |
295 |
296 |
297 | /*
298 | * tracking always uses the newest KF as reference.
299 | *
300 | */
301 |
302 | void makeKeyFrame( FrameHessian* fh);
303 | void makeNonKeyFrame( FrameHessian* fh);
304 | void deliverTrackedFrame(FrameHessian* fh, bool needKF);
305 | void mappingLoop();
306 |
307 | // tracking / mapping synchronization. All protected by [trackMapSyncMutex].
308 | boost::mutex trackMapSyncMutex;
309 | boost::condition_variable trackedFrameSignal;
310 | boost::condition_variable mappedFrameSignal;
311 | std::deque<FrameHessian*> unmappedTrackedFrames;
312 | int needNewKFAfter; // Otherwise, a new KF is *needed that has ID bigger than [needNewKFAfter]*.
313 | boost::thread mappingThread;
314 | bool runMapping;
315 | bool needToKetchupMapping;
316 |
317 | int lastRefStopID;
318 | };
319 | }
320 |
321 |
--------------------------------------------------------------------------------
/src/FullSystem/FullSystemMarginalize.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | /*
26 | * KFBuffer.cpp
27 | *
28 | * Created on: Jan 7, 2014
29 | * Author: engelj
30 | */
31 |
32 | #include "FullSystem/FullSystem.h"
33 |
34 | #include "stdio.h"
35 | #include "util/globalFuncs.h"
36 | #include <Eigen/LU>
37 | #include <algorithm>
38 | #include "IOWrapper/ImageDisplay.h"
39 | #include "util/globalCalib.h"
40 |
41 | #include <Eigen/SVD>
42 | #include <Eigen/Eigenvalues>
43 | #include "FullSystem/ResidualProjections.h"
44 | #include "FullSystem/ImmaturePoint.h"
45 |
46 | #include "OptimizationBackend/EnergyFunctional.h"
47 | #include "OptimizationBackend/EnergyFunctionalStructs.h"
48 |
49 | #include "IOWrapper/Output3DWrapper.h"
50 |
51 | #include "FullSystem/CoarseTracker.h"
52 |
53 | namespace dso
54 | {
55 |
56 |
57 |
58 | void FullSystem::flagFramesForMarginalization(FrameHessian* newFH)
59 | {
60 | if(setting_minFrameAge > setting_maxFrames)
61 | {
62 | for(int i=setting_maxFrames;i<(int)frameHessians.size();i++)
63 | {
64 | FrameHessian* fh = frameHessians[i-setting_maxFrames];
65 | fh->flaggedForMarginalization = true;
66 | }
67 | return;
68 | }
69 |
70 |
71 | int flagged = 0;
72 | // marginalize all frames that have not enough points.
73 | for(int i=0;i<(int)frameHessians.size();i++)
74 | {
75 | FrameHessian* fh = frameHessians[i];
76 | int in = fh->pointHessians.size() + fh->immaturePoints.size();
77 | int out = fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size();
78 |
79 |
80 | Vec2 refToFh=AffLight::fromToVecExposure(frameHessians.back()->ab_exposure, fh->ab_exposure,
81 | frameHessians.back()->aff_g2l(), fh->aff_g2l());
82 |
83 |
84 | if( (in < setting_minPointsRemaining *(in+out) || fabs(logf((float)refToFh[0])) > setting_maxLogAffFacInWindow)
85 | && ((int)frameHessians.size())-flagged > setting_minFrames)
86 | {
87 | // printf("MARGINALIZE frame %d, as only %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n",
88 | // fh->frameID, in, in+out,
89 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(),
90 | // (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(),
91 | // visInLast, outInLast,
92 | // fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame);
93 | fh->flaggedForMarginalization = true;
94 | flagged++;
95 | }
96 | else
97 | {
98 | // printf("May Keep frame %d, as %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n",
99 | // fh->frameID, in, in+out,
100 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(),
101 | // (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(),
102 | // visInLast, outInLast,
103 | // fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame);
104 | }
105 | }
106 |
107 | // marginalize one.
108 | if((int)frameHessians.size()-flagged >= setting_maxFrames)
109 | {
110 | double smallestScore = 1;
111 | FrameHessian* toMarginalize=0;
112 | FrameHessian* latest = frameHessians.back();
113 |
114 |
115 | for(FrameHessian* fh : frameHessians)
116 | {
117 | if(fh->frameID > latest->frameID-setting_minFrameAge || fh->frameID == 0) continue;
118 | //if(fh==frameHessians.front() == 0) continue;
119 |
120 | double distScore = 0;
121 | for(FrameFramePrecalc &ffh : fh->targetPrecalc)
122 | {
123 | if(ffh.target->frameID > latest->frameID-setting_minFrameAge+1 || ffh.target == ffh.host) continue;
124 | distScore += 1/(1e-5+ffh.distanceLL);
125 |
126 | }
127 | distScore *= -sqrtf(fh->targetPrecalc.back().distanceLL);
128 |
129 |
130 | if(distScore < smallestScore)
131 | {
132 | smallestScore = distScore;
133 | toMarginalize = fh;
134 | }
135 | }
136 |
137 | // printf("MARGINALIZE frame %d, as it is the closest (score %.2f)!\n",
138 | // toMarginalize->frameID, smallestScore);
139 | toMarginalize->flaggedForMarginalization = true;
140 | flagged++;
141 | }
142 |
143 | // printf("FRAMES LEFT: ");
144 | // for(FrameHessian* fh : frameHessians)
145 | // printf("%d ", fh->frameID);
146 | // printf("\n");
147 | }
148 |
149 |
150 |
151 |
152 | void FullSystem::marginalizeFrame(FrameHessian* frame)
153 | {
154 | // marginalize or remove all this frames points.
155 |
156 | assert((int)frame->pointHessians.size()==0);
157 |
158 |
159 | ef->marginalizeFrame(frame->efFrame);
160 |
161 | // drop all observations of existing points in that frame.
162 |
163 | for(FrameHessian* fh : frameHessians)
164 | {
165 | if(fh==frame) continue;
166 |
167 | for(PointHessian* ph : fh->pointHessians)
168 | {
169 | for(unsigned int i=0;i<ph->residuals.size();i++)
170 | {
171 | PointFrameResidual* r = ph->residuals[i];
172 | if(r->target == frame)
173 | {
174 | if(ph->lastResiduals[0].first == r)
175 | ph->lastResiduals[0].first=0;
176 | else if(ph->lastResiduals[1].first == r)
177 | ph->lastResiduals[1].first=0;
178 |
179 |
180 | if(r->host->frameID < r->target->frameID)
181 | statistics_numForceDroppedResFwd++;
182 | else
183 | statistics_numForceDroppedResBwd++;
184 |
185 | ef->dropResidual(r->efResidual);
186 | deleteOut<PointFrameResidual>(ph->residuals,i);
187 | break;
188 | }
189 | }
190 | }
191 | }
192 |
193 |
194 |
195 | {
196 | std::vector<FrameHessian*> v;
197 | v.push_back(frame);
198 | for(IOWrap::Output3DWrapper* ow : outputWrapper)
199 | ow->publishKeyframes(v, true, &Hcalib);
200 | }
201 |
202 |
203 | frame->shell->marginalizedAt = frameHessians.back()->shell->id;
204 | frame->shell->movedByOpt = frame->w2c_leftEps().norm();
205 |
206 | deleteOutOrder<FrameHessian>(frameHessians, frame);
207 | for(unsigned int i=0;i<frameHessians.size();i++)
208 | frameHessians[i]->idx = i;
209 |
210 |
211 |
212 |
213 | setPrecalcValues();
214 | ef->setAdjointsF(&Hcalib);
215 | }
216 |
217 |
218 |
219 |
220 | }
221 |
--------------------------------------------------------------------------------
/src/FullSystem/FullSystemOptPoint.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | /*
26 | * KFBuffer.cpp
27 | *
28 | * Created on: Jan 7, 2014
29 | * Author: engelj
30 | */
31 |
32 | #include "FullSystem/FullSystem.h"
33 |
34 | #include "stdio.h"
35 | #include "util/globalFuncs.h"
36 | #include <Eigen/LU>
37 | #include <algorithm>
38 | #include "IOWrapper/ImageDisplay.h"
39 | #include "util/globalCalib.h"
40 |
41 | #include <Eigen/SVD>
42 | #include <Eigen/Eigenvalues>
43 | #include "FullSystem/ImmaturePoint.h"
44 | #include "math.h"
45 |
46 | namespace dso
47 | {
48 |
49 |
50 |
51 | PointHessian* FullSystem::optimizeImmaturePoint(
52 | ImmaturePoint* point, int minObs,
53 | ImmaturePointTemporaryResidual* residuals)
54 | {
55 | int nres = 0;
56 | for(FrameHessian* fh : frameHessians)
57 | {
58 | if(fh != point->host)
59 | {
60 | residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0;
61 | residuals[nres].state_NewState = ResState::OUTLIER;
62 | residuals[nres].state_state = ResState::IN;
63 | residuals[nres].target = fh;
64 | nres++;
65 | }
66 | }
67 | assert(nres == ((int)frameHessians.size())-1);
68 |
69 | bool print = false;//rand()%50==0;
70 |
71 | float lastEnergy = 0;
72 | float lastHdd=0;
73 | float lastbd=0;
74 | float currentIdepth=(point->idepth_max+point->idepth_min)*0.5f;
75 |
76 |
77 |
78 |
79 |
80 |
81 | for(int i=0;i<nres;i++)
82 | {
83 | lastEnergy += point->linearizeResidual(&Hcalib, 1000, residuals+i,lastHdd, lastbd, currentIdepth);
84 | residuals[i].state_state = residuals[i].state_NewState;
85 | residuals[i].state_energy = residuals[i].state_NewEnergy;
86 | }
87 |
88 | if(!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act)
89 | {
90 | if(print)
91 | printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
92 | nres, lastHdd, lastEnergy);
93 | return 0;
94 | }
95 |
96 | if(print) printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" ,
97 | nres, lastHdd,lastEnergy,currentIdepth);
98 |
99 | float lambda = 0.1;
100 | for(int iteration=0;iteration<setting_GNItsOnPointActivation;iteration++)
101 | {
102 | float H = lastHdd;
103 | H *= 1+lambda;
104 | float step = (1.0/H) * lastbd;
105 | float newIdepth = currentIdepth - step;
106 |
107 | float newHdd=0; float newbd=0; float newEnergy=0;
108 | for(int i=0;i<nres;i++)
109 | newEnergy += point->linearizeResidual(&Hcalib, 1, residuals+i,newHdd, newbd, newIdepth);
110 |
111 | if(!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act)
112 | {
113 | if(print) printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
114 | nres,
115 | newHdd,
116 | lastEnergy);
117 | return 0;
118 | }
119 |
120 | if(print) printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n",
121 | (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT",
122 | iteration,
123 | log10(lambda),
124 | "",
125 | lastEnergy, newEnergy, newIdepth);
126 |
127 | if(newEnergy < lastEnergy)
128 | {
129 | currentIdepth = newIdepth;
130 | lastHdd = newHdd;
131 | lastbd = newbd;
132 | lastEnergy = newEnergy;
133 | for(int i=0;i<nres;i++)
134 | {
135 | residuals[i].state_state = residuals[i].state_NewState;
136 | residuals[i].state_energy = residuals[i].state_NewEnergy;
137 | }
138 |
139 | lambda *= 0.5;
140 | }
141 | else
142 | {
143 | lambda *= 5;
144 | }
145 |
146 | if(fabsf(step) < 0.0001*currentIdepth)
147 | break;
148 | }
149 |
150 | if(!std::isfinite(currentIdepth))
151 | {
152 | printf("MAJOR ERROR! point idepth is nan after initialization (%f).\n", currentIdepth);
153 | return (PointHessian*)((long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems.
154 | }
155 |
156 |
157 | int numGoodRes=0;
158 | for(int i=0;i<nres;i++)
159 | if(residuals[i].state_state == ResState::IN) numGoodRes++;
160 |
161 | if(numGoodRes < minObs)
162 | {
163 | if(print) printf("OptPoint: OUTLIER!\n");
164 | return (PointHessian*)((long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems.
165 | }
166 |
167 |
168 |
169 | PointHessian* p = new PointHessian(point, &Hcalib);
170 | if(!std::isfinite(p->energyTH)) {delete p; return (PointHessian*)((long)(-1));}
171 |
172 | p->lastResiduals[0].first = 0;
173 | p->lastResiduals[0].second = ResState::OOB;
174 | p->lastResiduals[1].first = 0;
175 | p->lastResiduals[1].second = ResState::OOB;
176 | p->setIdepthZero(currentIdepth);
177 | p->setIdepth(currentIdepth);
178 | p->setPointStatus(PointHessian::ACTIVE);
179 |
180 | for(int i=0;i<nres;i++)
181 | if(residuals[i].state_state == ResState::IN)
182 | {
183 | PointFrameResidual* r = new PointFrameResidual(p, p->host, residuals[i].target);
184 | r->state_NewEnergy = r->state_energy = 0;
185 | r->state_NewState = ResState::OUTLIER;
186 | r->setState(ResState::IN);
187 | p->residuals.push_back(r);
188 |
189 | if(r->target == frameHessians.back())
190 | {
191 | p->lastResiduals[0].first = r;
192 | p->lastResiduals[0].second = ResState::IN;
193 | }
194 | else if(r->target == (frameHessians.size()<2 ? 0 : frameHessians[frameHessians.size()-2]))
195 | {
196 | p->lastResiduals[1].first = r;
197 | p->lastResiduals[1].second = ResState::IN;
198 | }
199 | }
200 |
201 | if(print) printf("point activated!\n");
202 |
203 | statistics_numActivatedPoints++;
204 | return p;
205 | }
206 |
207 |
208 |
209 | }
210 |
--------------------------------------------------------------------------------
/src/FullSystem/HessianBlocks.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #include "FullSystem/HessianBlocks.h"
27 | #include "util/FrameShell.h"
28 | #include "FullSystem/ImmaturePoint.h"
29 | #include "OptimizationBackend/EnergyFunctionalStructs.h"
30 |
31 | namespace dso
32 | {
33 |
34 |
35 | PointHessian::PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib)
36 | {
37 | instanceCounter++;
38 | host = rawPoint->host;
39 | hasDepthPrior=false;
40 |
41 | idepth_hessian=0;
42 | maxRelBaseline=0;
43 | numGoodResiduals=0;
44 |
45 | // set static values & initialization.
46 | u = rawPoint->u;
47 | v = rawPoint->v;
48 | assert(std::isfinite(rawPoint->idepth_max));
49 | //idepth_init = rawPoint->idepth_GT;
50 |
51 | my_type = rawPoint->my_type;
52 |
53 | setIdepthScaled((rawPoint->idepth_max + rawPoint->idepth_min)*0.5);
54 | setPointStatus(PointHessian::INACTIVE);
55 |
56 | int n = patternNum;
57 | memcpy(color, rawPoint->color, sizeof(float)*n);
58 | memcpy(weights, rawPoint->weights, sizeof(float)*n);
59 | energyTH = rawPoint->energyTH;
60 |
61 | efPoint=0;
62 |
63 |
64 | }
65 |
66 |
67 | void PointHessian::release()
68 | {
69 | for(unsigned int i=0;i<residuals.size();i++) delete residuals[i];
70 | residuals.clear();
71 | }
72 |
73 |
74 | void FrameHessian::setStateZero(const Vec10 &state_zero)
75 | {
76 | assert(state_zero.head<6>().squaredNorm() < 1e-20);
77 |
78 | this->state_zero = state_zero;
79 |
80 |
81 | for(int i=0;i<6;i++)
82 | {
83 | Vec6 eps; eps.setZero(); eps[i] = 1e-3;
84 | SE3 EepsP = Sophus::SE3::exp(eps);
85 | SE3 EepsM = Sophus::SE3::exp(-eps);
86 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse();
87 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse();
88 | nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3);
89 | }
90 | //nullspaces_pose.topRows<3>() *= SCALE_XI_TRANS_INVERSE;
91 | //nullspaces_pose.bottomRows<3>() *= SCALE_XI_ROT_INVERSE;
92 |
93 | // scale change
94 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT());
95 | w2c_leftEps_P_x0.translation() *= 1.00001;
96 | w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse();
97 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT());
98 | w2c_leftEps_M_x0.translation() /= 1.00001;
99 | w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse();
100 | nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3);
101 |
102 |
103 | nullspaces_affine.setZero();
104 | nullspaces_affine.topLeftCorner<2,1>() = Vec2(1,0);
105 | assert(ab_exposure > 0);
106 | nullspaces_affine.topRightCorner<2,1>() = Vec2(0, expf(aff_g2l_0().a)*ab_exposure);
107 | };
108 |
109 |
110 |
111 | void FrameHessian::release()
112 | {
113 | // DELETE POINT
114 | // DELETE RESIDUAL
115 | for(unsigned int i=0;i<pointHessians.size();i++) delete pointHessians[i];
116 | for(unsigned int i=0;i<pointHessiansMarginalized.size();i++) delete pointHessiansMarginalized[i];
117 | for(unsigned int i=0;i<pointHessiansOut.size();i++) delete pointHessiansOut[i];
118 | for(unsigned int i=0;i<immaturePoints.size();i++) delete immaturePoints[i];
119 |
120 |
121 | pointHessians.clear();
122 | pointHessiansMarginalized.clear();
123 | pointHessiansOut.clear();
124 | immaturePoints.clear();
125 | }
126 |
127 |
128 | void FrameHessian::makeImages(float* color, CalibHessian* HCalib)
129 | {
130 |
131 | for(int i=0;i<pyrLevelsUsed;i++)
132 | {
133 | dIp[i] = new Eigen::Vector3f[wG[i]*hG[i]];
134 | absSquaredGrad[i] = new float[wG[i]*hG[i]];
135 | }
136 | dI = dIp[0];
137 |
138 |
139 | // make d0
140 | int w=wG[0];
141 | int h=hG[0];
142 | for(int i=0;i<w*h;i++)
143 | dI[i][0] = color[i];
144 |
145 | for(int lvl=0; lvl<pyrLevelsUsed; lvl++)
146 | {
147 | int wl = wG[lvl], hl = hG[lvl];
148 | Eigen::Vector3f* dI_l = dIp[lvl];
149 |
150 | float* dabs_l = absSquaredGrad[lvl];
151 | if(lvl>0)
152 | {
153 | int lvlm1 = lvl-1;
154 | int wlm1 = wG[lvlm1];
155 | Eigen::Vector3f* dI_lm = dIp[lvlm1];
156 |
157 |
158 |
159 | for(int y=0;y<hl;y++)
160 | for(int x=0;x<wl;x++)
161 | {
162 | dI_l[x + y*wl][0] = 0.25f * (dI_lm[2*x + 2*y*wlm1][0] +
163 | dI_lm[2*x+1 + 2*y*wlm1][0] +
164 | dI_lm[2*x + 2*y*wlm1+wlm1][0] +
165 | dI_lm[2*x+1 + 2*y*wlm1+wlm1][0]);
166 | }
167 | }
168 |
169 | for(int idx=wl;idx < wl*(hl-1);idx++)
170 | {
171 | float dx = 0.5f*(dI_l[idx+1][0] - dI_l[idx-1][0]);
172 | float dy = 0.5f*(dI_l[idx+wl][0] - dI_l[idx-wl][0]);
173 |
174 |
175 | if(!std::isfinite(dx)) dx=0;
176 | if(!std::isfinite(dy)) dy=0;
177 |
178 | dI_l[idx][1] = dx;
179 | dI_l[idx][2] = dy;
180 |
181 |
182 | dabs_l[idx] = dx*dx+dy*dy;
183 |
184 | if(setting_gammaWeightsPixelSelect==1 && HCalib!=0)
185 | {
186 | float gw = HCalib->getBGradOnly((float)(dI_l[idx][0]));
187 | dabs_l[idx] *= gw*gw; // convert to gradient of original color space (before removing response).
188 | }
189 | }
190 | }
191 | }
192 |
193 | void FrameFramePrecalc::set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib )
194 | {
195 | this->host = host;
196 | this->target = target;
197 |
198 | SE3 leftToLeft_0 = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse();
199 | PRE_RTll_0 = (leftToLeft_0.rotationMatrix()).cast<float>();
200 | PRE_tTll_0 = (leftToLeft_0.translation()).cast<float>();
201 |
202 |
203 |
204 | SE3 leftToLeft = target->PRE_worldToCam * host->PRE_camToWorld;
205 | PRE_RTll = (leftToLeft.rotationMatrix()).cast<float>();
206 | PRE_tTll = (leftToLeft.translation()).cast<float>();
207 | distanceLL = leftToLeft.translation().norm();
208 |
209 |
210 | Mat33f K = Mat33f::Zero();
211 | K(0,0) = HCalib->fxl();
212 | K(1,1) = HCalib->fyl();
213 | K(0,2) = HCalib->cxl();
214 | K(1,2) = HCalib->cyl();
215 | K(2,2) = 1;
216 | PRE_KRKiTll = K * PRE_RTll * K.inverse();
217 | PRE_RKiTll = PRE_RTll * K.inverse();
218 | PRE_KtTll = K * PRE_tTll;
219 |
220 |
221 | PRE_aff_mode = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l(), target->aff_g2l()).cast<float>();
222 | PRE_b0_mode = host->aff_g2l_0().b;
223 | }
224 |
225 | }
226 |
227 |
--------------------------------------------------------------------------------
/src/FullSystem/ImmaturePoint.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 |
30 | #include "FullSystem/HessianBlocks.h"
31 | namespace dso
32 | {
33 |
34 |
35 | struct ImmaturePointTemporaryResidual
36 | {
37 | public:
38 | ResState state_state;
39 | double state_energy;
40 | ResState state_NewState;
41 | double state_NewEnergy;
42 | FrameHessian* target;
43 | };
44 |
45 |
46 | enum ImmaturePointStatus {
47 | IPS_GOOD=0, // traced well and good
48 | IPS_OOB, // OOB: end tracking & marginalize!
49 | IPS_OUTLIER, // energy too high: if happens again: outlier!
50 | IPS_SKIPPED, // traced well and good (but not actually traced).
51 | IPS_BADCONDITION, // not traced because of bad condition.
52 | IPS_UNINITIALIZED}; // not even traced once.
53 |
54 |
55 | class ImmaturePoint
56 | {
57 | public:
58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
59 | // static values
60 | float color[MAX_RES_PER_POINT];
61 | float weights[MAX_RES_PER_POINT];
62 |
63 |
64 |
65 |
66 |
67 | Mat22f gradH;
68 | Vec2f gradH_ev;
69 | Mat22f gradH_eig;
70 | float energyTH;
71 | float u,v;
72 | FrameHessian* host;
73 | int idxInImmaturePoints;
74 |
75 | float quality;
76 |
77 | float my_type;
78 |
79 | float idepth_min;
80 | float idepth_max;
81 | ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib);
82 | ~ImmaturePoint();
83 |
84 | ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false);
85 |
86 | ImmaturePointStatus lastTraceStatus;
87 | Vec2f lastTraceUV;
88 | float lastTracePixelInterval;
89 |
90 | float idepth_GT;
91 |
92 | double linearizeResidual(
93 | CalibHessian * HCalib, const float outlierTHSlack,
94 | ImmaturePointTemporaryResidual* tmpRes,
95 | float &Hdd, float &bd,
96 | float idepth);
97 | float getdPixdd(
98 | CalibHessian * HCalib,
99 | ImmaturePointTemporaryResidual* tmpRes,
100 | float idepth);
101 |
102 | float calcResidual(
103 | CalibHessian * HCalib, const float outlierTHSlack,
104 | ImmaturePointTemporaryResidual* tmpRes,
105 | float idepth);
106 |
107 | private:
108 | };
109 |
110 | }
111 |
112 |
--------------------------------------------------------------------------------
/src/FullSystem/PixelSelector.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 |
30 |
31 |
32 |
33 | namespace dso
34 | {
35 |
36 |
37 | const float minUseGrad_pixsel = 10;
38 |
39 |
40 | template<int pot>
41 | inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac)
42 | {
43 |
44 | memset(map_out, 0, sizeof(bool)*w*h);
45 |
46 | int numGood = 0;
47 | for(int y=1;y<h-pot;y+=pot)
48 | {
49 | for(int x=1;x<w-pot;x+=pot)
50 | {
51 | int bestXXID = -1;
52 | int bestYYID = -1;
53 | int bestXYID = -1;
54 | int bestYXID = -1;
55 |
56 | float bestXX=0, bestYY=0, bestXY=0, bestYX=0;
57 |
58 | Eigen::Vector3f* grads0 = grads+x+y*w;
59 | for(int dx=0;dx<pot;dx++)
60 | for(int dy=0;dy<pot;dy++)
61 | {
62 | int idx = dx+dy*w;
63 | Eigen::Vector3f g=grads0[idx];
64 | float sqgd = g.tail<2>().squaredNorm();
65 | float TH = THFac*minUseGrad_pixsel * (0.75f);
66 |
67 | if(sqgd > TH*TH)
68 | {
69 | float agx = fabs((float)g[1]);
70 | if(agx > bestXX) {bestXX=agx; bestXXID=idx;}
71 |
72 | float agy = fabs((float)g[2]);
73 | if(agy > bestYY) {bestYY=agy; bestYYID=idx;}
74 |
75 | float gxpy = fabs((float)(g[1]-g[2]));
76 | if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}
77 |
78 | float gxmy = fabs((float)(g[1]+g[2]));
79 | if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
80 | }
81 | }
82 |
83 | bool* map0 = map_out+x+y*w;
84 |
85 | if(bestXXID>=0)
86 | {
87 | if(!map0[bestXXID])
88 | numGood++;
89 | map0[bestXXID] = true;
90 |
91 | }
92 | if(bestYYID>=0)
93 | {
94 | if(!map0[bestYYID])
95 | numGood++;
96 | map0[bestYYID] = true;
97 |
98 | }
99 | if(bestXYID>=0)
100 | {
101 | if(!map0[bestXYID])
102 | numGood++;
103 | map0[bestXYID] = true;
104 |
105 | }
106 | if(bestYXID>=0)
107 | {
108 | if(!map0[bestYXID])
109 | numGood++;
110 | map0[bestYXID] = true;
111 |
112 | }
113 | }
114 | }
115 |
116 | return numGood;
117 | }
118 |
119 |
120 | inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac)
121 | {
122 |
123 | memset(map_out, 0, sizeof(bool)*w*h);
124 |
125 | int numGood = 0;
126 | for(int y=1;y<h-pot;y+=pot)
127 | {
128 | for(int x=1;x<w-pot;x+=pot)
129 | {
130 | int bestXXID = -1;
131 | int bestYYID = -1;
132 | int bestXYID = -1;
133 | int bestYXID = -1;
134 |
135 | float bestXX=0, bestYY=0, bestXY=0, bestYX=0;
136 |
137 | Eigen::Vector3f* grads0 = grads+x+y*w;
138 | for(int dx=0;dx<pot;dx++)
139 | for(int dy=0;dy<pot;dy++)
140 | {
141 | int idx = dx+dy*w;
142 | Eigen::Vector3f g=grads0[idx];
143 | float sqgd = g.tail<2>().squaredNorm();
144 | float TH = THFac*minUseGrad_pixsel * (0.75f);
145 |
146 | if(sqgd > TH*TH)
147 | {
148 | float agx = fabs((float)g[1]);
149 | if(agx > bestXX) {bestXX=agx; bestXXID=idx;}
150 |
151 | float agy = fabs((float)g[2]);
152 | if(agy > bestYY) {bestYY=agy; bestYYID=idx;}
153 |
154 | float gxpy = fabs((float)(g[1]-g[2]));
155 | if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}
156 |
157 | float gxmy = fabs((float)(g[1]+g[2]));
158 | if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
159 | }
160 | }
161 |
162 | bool* map0 = map_out+x+y*w;
163 |
164 | if(bestXXID>=0)
165 | {
166 | if(!map0[bestXXID])
167 | numGood++;
168 | map0[bestXXID] = true;
169 |
170 | }
171 | if(bestYYID>=0)
172 | {
173 | if(!map0[bestYYID])
174 | numGood++;
175 | map0[bestYYID] = true;
176 |
177 | }
178 | if(bestXYID>=0)
179 | {
180 | if(!map0[bestXYID])
181 | numGood++;
182 | map0[bestXYID] = true;
183 |
184 | }
185 | if(bestYXID>=0)
186 | {
187 | if(!map0[bestYXID])
188 | numGood++;
189 | map0[bestYXID] = true;
190 |
191 | }
192 | }
193 | }
194 |
195 | return numGood;
196 | }
197 |
198 |
199 | inline int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1)
200 | {
201 | if(sparsityFactor < 1) sparsityFactor = 1;
202 |
203 | int numGoodPoints;
204 |
205 |
206 | if(sparsityFactor==1) numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac);
207 | else if(sparsityFactor==2) numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac);
208 | else if(sparsityFactor==3) numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac);
209 | else if(sparsityFactor==4) numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac);
210 | else if(sparsityFactor==5) numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac);
211 | else if(sparsityFactor==6) numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac);
212 | else if(sparsityFactor==7) numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac);
213 | else if(sparsityFactor==8) numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac);
214 | else if(sparsityFactor==9) numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac);
215 | else if(sparsityFactor==10) numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac);
216 | else if(sparsityFactor==11) numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac);
217 | else numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac);
218 |
219 |
220 | /*
221 | * #points is approximately proportional to sparsityFactor^2.
222 | */
223 |
224 | float quotia = numGoodPoints / (float)(desiredDensity);
225 |
226 | int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f;
227 |
228 |
229 | if(newSparsity < 1) newSparsity=1;
230 |
231 |
232 | float oldTHFac = THFac;
233 | if(newSparsity==1 && sparsityFactor==1) THFac = 0.5;
234 |
235 |
236 | if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) ||
237 | ( quotia > 0.8 && 1.0f / quotia > 0.8) ||
238 | recsLeft == 0)
239 | {
240 |
241 | // printf(" \n");
242 | //all good
243 | sparsityFactor = newSparsity;
244 | return numGoodPoints;
245 | }
246 | else
247 | {
248 | // printf(" -> re-evaluate! \n");
249 | // re-evaluate.
250 | sparsityFactor = newSparsity;
251 | return makePixelStatus(grads, map, w,h, desiredDensity, recsLeft-1, THFac);
252 | }
253 | }
254 |
255 | }
256 |
257 |
--------------------------------------------------------------------------------
/src/FullSystem/PixelSelector2.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "util/NumType.h"
28 |
29 | namespace dso
30 | {
31 |
32 | enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3};
33 |
34 |
35 | class FrameHessian;
36 |
37 | class PixelSelector
38 | {
39 | public:
40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
41 | int makeMaps(
42 | const FrameHessian* const fh,
43 | float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1);
44 |
45 | PixelSelector(int w, int h);
46 | ~PixelSelector();
47 | int currentPotential;
48 |
49 |
50 | bool allowFast;
51 | void makeHists(const FrameHessian* const fh);
52 | private:
53 |
54 | Eigen::Vector3i select(const FrameHessian* const fh,
55 | float* map_out, int pot, float thFactor=1);
56 |
57 |
58 | unsigned char* randomPattern;
59 |
60 |
61 | int* gradHist;
62 | float* ths;
63 | float* thsSmoothed;
64 | int thsStep;
65 | const FrameHessian* gradHistFrame;
66 | };
67 |
68 |
69 |
70 |
71 | }
72 |
73 |
--------------------------------------------------------------------------------
/src/FullSystem/ResidualProjections.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "util/NumType.h"
28 | #include "FullSystem/FullSystem.h"
29 | #include "FullSystem/HessianBlocks.h"
30 | #include "util/settings.h"
31 |
32 | namespace dso
33 | {
34 |
35 |
36 | EIGEN_STRONG_INLINE float derive_idepth(
37 | const Vec3f &t, const float &u, const float &v,
38 | const int &dx, const int &dy, const float &dxInterp,
39 | const float &dyInterp, const float &drescale)
40 | {
41 | return (dxInterp*drescale * (t[0]-t[2]*u)
42 | + dyInterp*drescale * (t[1]-t[2]*v))*SCALE_IDEPTH;
43 | }
44 |
45 |
46 |
47 | EIGEN_STRONG_INLINE bool projectPoint(
48 | const float &u_pt,const float &v_pt,
49 | const float &idepth,
50 | const Mat33f &KRKi, const Vec3f &Kt,
51 | float &Ku, float &Kv)
52 | {
53 | Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth;
54 | Ku = ptp[0] / ptp[2];
55 | Kv = ptp[1] / ptp[2];
56 | return Ku>1.1f && Kv>1.1f && Ku<wM3G && Kv<hM3G;
57 | }
58 |
59 |
60 |
61 | EIGEN_STRONG_INLINE bool projectPoint(
62 | const float &u_pt,const float &v_pt,
63 | const float &idepth,
64 | const int &dx, const int &dy,
65 | CalibHessian* const &HCalib,
66 | const Mat33f &R, const Vec3f &t,
67 | float &drescale, float &u, float &v,
68 | float &Ku, float &Kv, Vec3f &KliP, float &new_idepth)
69 | {
70 | KliP = Vec3f(
71 | (u_pt+dx-HCalib->cxl())*HCalib->fxli(),
72 | (v_pt+dy-HCalib->cyl())*HCalib->fyli(),
73 | 1);
74 |
75 | Vec3f ptp = R * KliP + t*idepth;
76 | drescale = 1.0f/ptp[2];
77 | new_idepth = idepth*drescale;
78 |
79 | if(!(drescale>0)) return false;
80 |
81 | u = ptp[0] * drescale;
82 | v = ptp[1] * drescale;
83 | Ku = u*HCalib->fxl() + HCalib->cxl();
84 | Kv = v*HCalib->fyl() + HCalib->cyl();
85 |
86 | return Ku>1.1f && Kv>1.1f && Ku<wM3G && Kv<hM3G;
87 | }
88 |
89 |
90 |
91 |
92 | }
93 |
94 |
--------------------------------------------------------------------------------
/src/FullSystem/Residuals.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/globalCalib.h"
29 | #include "vector"
30 |
31 | #include "util/NumType.h"
32 | #include <iostream>
33 | #include <fstream>
34 | #include "util/globalFuncs.h"
35 | #include "OptimizationBackend/RawResidualJacobian.h"
36 |
37 | namespace dso
38 | {
39 | class PointHessian;
40 | class FrameHessian;
41 | class CalibHessian;
42 |
43 | class EFResidual;
44 |
45 |
46 | enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE};
47 | enum ResState {IN=0, OOB, OUTLIER};
48 |
49 | struct FullJacRowT
50 | {
51 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT];
52 | };
53 |
54 | class PointFrameResidual
55 | {
56 | public:
57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 |
59 | EFResidual* efResidual;
60 |
61 | static int instanceCounter;
62 |
63 |
64 | ResState state_state;
65 | double state_energy;
66 | ResState state_NewState;
67 | double state_NewEnergy;
68 | double state_NewEnergyWithOutlier;
69 |
70 |
71 | void setState(ResState s) {state_state = s;}
72 |
73 |
74 | PointHessian* point;
75 | FrameHessian* host;
76 | FrameHessian* target;
77 | RawResidualJacobian* J;
78 |
79 |
80 | bool isNew;
81 |
82 |
83 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT];
84 | Vec3f centerProjectedTo;
85 |
86 | ~PointFrameResidual();
87 | PointFrameResidual();
88 | PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_);
89 | double linearize(CalibHessian* HCalib);
90 |
91 |
92 | void resetOOB()
93 | {
94 | state_NewEnergy = state_energy = 0;
95 | state_NewState = ResState::OUTLIER;
96 |
97 | setState(ResState::IN);
98 | };
99 | void applyRes( bool copyJacobians);
100 |
101 | void debugPlot();
102 |
103 | void printRows(std::vector<VecX> &v, VecX &r, int nFrames, int nPoints, int M, int res);
104 | };
105 | }
106 |
107 |
--------------------------------------------------------------------------------
/src/IOWrapper/ImageDisplay.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #pragma once
27 | #include <vector>
28 | #include "util/NumType.h"
29 | #include "util/MinimalImage.h"
30 |
31 |
32 | namespace dso
33 | {
34 |
35 | namespace IOWrap
36 | {
37 |
38 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize = false);
39 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize = false);
40 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize = false);
41 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize = false);
42 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize = false);
43 |
44 |
45 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageB*> images, int cc=0, int rc=0);
46 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageB3*> images, int cc=0, int rc=0);
47 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageF*> images, int cc=0, int rc=0);
48 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageF3*> images, int cc=0, int rc=0);
49 |
50 | int waitKey(int milliseconds);
51 | void closeAllWindows();
52 |
53 | }
54 |
55 | }
56 |
--------------------------------------------------------------------------------
/src/IOWrapper/ImageDisplay_dummy.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #include "IOWrapper/ImageDisplay.h"
27 |
28 | namespace dso
29 | {
30 |
31 |
32 | namespace IOWrap
33 | {
34 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) {};
35 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) {};
36 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) {};
37 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) {};
38 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) {};
39 |
40 |
41 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageB*> images, int cc, int rc) {};
42 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageB3*> images, int cc, int rc) {};
43 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageF*> images, int cc, int rc) {};
44 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageF3*> images, int cc, int rc) {};
45 |
46 | int waitKey(int milliseconds) {return 0;};
47 | void closeAllWindows() {};
48 | }
49 |
50 | }
51 |
--------------------------------------------------------------------------------
/src/IOWrapper/ImageRW.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #pragma once
27 | #include "util/NumType.h"
28 | #include "util/MinimalImage.h"
29 |
30 | namespace dso
31 | {
32 | namespace IOWrap
33 | {
34 |
35 | MinimalImageB* readImageBW_8U(std::string filename);
36 | MinimalImageB3* readImageRGB_8U(std::string filename);
37 | MinimalImage<unsigned short>* readImageBW_16U(std::string filename);
38 |
39 |
40 | MinimalImageB* readStreamBW_8U(char* data, int numBytes);
41 |
42 | void writeImage(std::string filename, MinimalImageB* img);
43 | void writeImage(std::string filename, MinimalImageB3* img);
44 | void writeImage(std::string filename, MinimalImageF* img);
45 | void writeImage(std::string filename, MinimalImageF3* img);
46 |
47 | }
48 | }
49 |
--------------------------------------------------------------------------------
/src/IOWrapper/ImageRW_dummy.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #include "IOWrapper/ImageRW.h"
27 |
28 | namespace dso
29 | {
30 |
31 |
32 | namespace IOWrap
33 | {
34 |
35 | MinimalImageB* readImageBW_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;};
36 | MinimalImageB3* readImageRGB_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;};
37 | MinimalImage<unsigned short>* readImageBW_16U(std::string filename) {printf("not implemented. bye!\n"); return 0;};
38 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) {printf("not implemented. bye!\n"); return 0;};
39 | void writeImage(std::string filename, MinimalImageB* img) {};
40 | void writeImage(std::string filename, MinimalImageB3* img) {};
41 | void writeImage(std::string filename, MinimalImageF* img) {};
42 | void writeImage(std::string filename, MinimalImageF3* img) {};
43 |
44 | }
45 |
46 | }
47 |
--------------------------------------------------------------------------------
/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #include "IOWrapper/ImageDisplay.h"
27 |
28 | #include <opencv2/highgui/highgui.hpp>
29 |
30 | #include <string>
31 | #include <unordered_set>
32 |
33 | #include <boost/thread.hpp>
34 |
35 | #include "util/settings.h"
36 |
37 | namespace dso
38 | {
39 |
40 |
41 | namespace IOWrap
42 | {
43 |
44 | std::unordered_set<std::string> openWindows;
45 | boost::mutex openCVdisplayMutex;
46 |
47 |
48 |
49 | void displayImage(const char* windowName, const cv::Mat& image, bool autoSize)
50 | {
51 | if(disableAllDisplay) return;
52 |
53 | boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
54 | if(!autoSize)
55 | {
56 | if(openWindows.find(windowName) == openWindows.end())
57 | {
58 | cv::namedWindow(windowName, cv::WINDOW_NORMAL);
59 | cv::resizeWindow(windowName, image.cols, image.rows);
60 | openWindows.insert(windowName);
61 | }
62 | }
63 | cv::imshow(windowName, image);
64 | }
65 |
66 |
67 | void displayImageStitch(const char* windowName, const std::vector<cv::Mat*> images, int cc, int rc)
68 | {
69 | if(disableAllDisplay) return;
70 | if(images.size() == 0) return;
71 |
72 | // get dimensions.
73 | int w = images[0]->cols;
74 | int h = images[0]->rows;
75 |
76 | int num = std::max((int)setting_maxFrames, (int)images.size());
77 |
78 | // get optimal dimensions.
79 | int bestCC = 0;
80 | float bestLoss = 1e10;
81 | for(int cc=1;cc<10;cc++)
82 | {
83 | int ww = w * cc;
84 | int hh = h * ((num+cc-1)/cc);
85 |
86 |
87 | float wLoss = ww/16.0f;
88 | float hLoss = hh/10.0f;
89 | float loss = std::max(wLoss, hLoss);
90 |
91 | if(loss < bestLoss)
92 | {
93 | bestLoss = loss;
94 | bestCC = cc;
95 | }
96 | }
97 |
98 | int bestRC = ((num+bestCC-1)/bestCC);
99 | if(cc != 0)
100 | {
101 | bestCC = cc;
102 | bestRC= rc;
103 | }
104 | cv::Mat stitch = cv::Mat(bestRC*h, bestCC*w, images[0]->type());
105 | stitch.setTo(0);
106 | for(int i=0;i<(int)images.size() && i < bestCC*bestRC;i++)
107 | {
108 | int c = i%bestCC;
109 | int r = i/bestCC;
110 |
111 | cv::Mat roi = stitch(cv::Rect(c*w, r*h, w,h));
112 | images[i]->copyTo(roi);
113 | }
114 | displayImage(windowName, stitch, false);
115 | }
116 |
117 |
118 |
119 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize)
120 | {
121 | displayImage(windowName, cv::Mat(img->h, img->w, CV_8U, img->data), autoSize);
122 | }
123 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize)
124 | {
125 | displayImage(windowName, cv::Mat(img->h, img->w, CV_8UC3, img->data), autoSize);
126 | }
127 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize)
128 | {
129 | displayImage(windowName, cv::Mat(img->h, img->w, CV_32F, img->data)*(1/254.0f), autoSize);
130 | }
131 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize)
132 | {
133 | displayImage(windowName, cv::Mat(img->h, img->w, CV_32FC3, img->data)*(1/254.0f), autoSize);
134 | }
135 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize)
136 | {
137 | displayImage(windowName, cv::Mat(img->h, img->w, CV_16U, img->data), autoSize);
138 | }
139 |
140 |
141 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageB*> images, int cc, int rc)
142 | {
143 | std::vector<cv::Mat*> imagesCV;
144 | for(size_t i=0; i < images.size();i++)
145 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8U, images[i]->data));
146 | displayImageStitch(windowName, imagesCV, cc, rc);
147 | for(size_t i=0; i < images.size();i++)
148 | delete imagesCV[i];
149 | }
150 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageB3*> images, int cc, int rc)
151 | {
152 | std::vector<cv::Mat*> imagesCV;
153 | for(size_t i=0; i < images.size();i++)
154 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8UC3, images[i]->data));
155 | displayImageStitch(windowName, imagesCV, cc, rc);
156 | for(size_t i=0; i < images.size();i++)
157 | delete imagesCV[i];
158 | }
159 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageF*> images, int cc, int rc)
160 | {
161 | std::vector<cv::Mat*> imagesCV;
162 | for(size_t i=0; i < images.size();i++)
163 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32F, images[i]->data));
164 | displayImageStitch(windowName, imagesCV, cc, rc);
165 | for(size_t i=0; i < images.size();i++)
166 | delete imagesCV[i];
167 | }
168 | void displayImageStitch(const char* windowName, const std::vector<MinimalImageF3*> images, int cc, int rc)
169 | {
170 | std::vector<cv::Mat*> imagesCV;
171 | for(size_t i=0; i < images.size();i++)
172 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32FC3, images[i]->data));
173 | displayImageStitch(windowName, imagesCV, cc, rc);
174 | for(size_t i=0; i < images.size();i++)
175 | delete imagesCV[i];
176 | }
177 |
178 |
179 |
180 | int waitKey(int milliseconds)
181 | {
182 | if(disableAllDisplay) return 0;
183 |
184 | boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
185 | return cv::waitKey(milliseconds);
186 | }
187 |
188 | void closeAllWindows()
189 | {
190 | if(disableAllDisplay) return;
191 | boost::unique_lock<boost::mutex> lock(openCVdisplayMutex);
192 | cv::destroyAllWindows();
193 | openWindows.clear();
194 | }
195 | }
196 |
197 | }
198 |
--------------------------------------------------------------------------------
/src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #include "IOWrapper/ImageRW.h"
27 | #include <opencv2/highgui/highgui.hpp>
28 |
29 |
30 | namespace dso
31 | {
32 |
33 | namespace IOWrap
34 | {
35 | MinimalImageB* readImageBW_8U(std::string filename)
36 | {
37 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
38 | if(m.rows*m.cols==0)
39 | {
40 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str());
41 | return 0;
42 | }
43 | if(m.type() != CV_8U)
44 | {
45 | printf("cv::imread did something strange! this may segfault. \n");
46 | return 0;
47 | }
48 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows);
49 | memcpy(img->data, m.data, m.rows*m.cols);
50 | return img;
51 | }
52 |
53 | MinimalImageB3* readImageRGB_8U(std::string filename)
54 | {
55 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
56 | if(m.rows*m.cols==0)
57 | {
58 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str());
59 | return 0;
60 | }
61 | if(m.type() != CV_8UC3)
62 | {
63 | printf("cv::imread did something strange! this may segfault. \n");
64 | return 0;
65 | }
66 | MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows);
67 | memcpy(img->data, m.data, 3*m.rows*m.cols);
68 | return img;
69 | }
70 |
71 | MinimalImage<unsigned short>* readImageBW_16U(std::string filename)
72 | {
73 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED);
74 | if(m.rows*m.cols==0)
75 | {
76 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str());
77 | return 0;
78 | }
79 | if(m.type() != CV_16U)
80 | {
81 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n");
82 | return 0;
83 | }
84 | MinimalImage<unsigned short>* img = new MinimalImage<unsigned short>(m.cols, m.rows);
85 | memcpy(img->data, m.data, 2*m.rows*m.cols);
86 | return img;
87 | }
88 |
89 | MinimalImageB* readStreamBW_8U(char* data, int numBytes)
90 | {
91 | cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), CV_LOAD_IMAGE_GRAYSCALE);
92 | if(m.rows*m.cols==0)
93 | {
94 | printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes);
95 | return 0;
96 | }
97 | if(m.type() != CV_8U)
98 | {
99 | printf("cv::imdecode did something strange! this may segfault. \n");
100 | return 0;
101 | }
102 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows);
103 | memcpy(img->data, m.data, m.rows*m.cols);
104 | return img;
105 | }
106 |
107 |
108 |
109 | void writeImage(std::string filename, MinimalImageB* img)
110 | {
111 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data));
112 | }
113 | void writeImage(std::string filename, MinimalImageB3* img)
114 | {
115 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data));
116 | }
117 | void writeImage(std::string filename, MinimalImageF* img)
118 | {
119 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data));
120 | }
121 | void writeImage(std::string filename, MinimalImageF3* img)
122 | {
123 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data));
124 | }
125 |
126 | }
127 |
128 | }
129 |
--------------------------------------------------------------------------------
/src/IOWrapper/Output3DWrapper.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #pragma once
27 | #include <vector>
28 | #include <string>
29 |
30 | #include "util/NumType.h"
31 | #include "util/MinimalImage.h"
32 | #include "map"
33 |
34 | namespace cv {
35 | class Mat;
36 | }
37 |
38 | namespace dso
39 | {
40 |
41 | class FrameHessian;
42 | class CalibHessian;
43 | class FrameShell;
44 |
45 | namespace IOWrap
46 | {
47 |
48 | /* ======================= Some typical usecases: ===============
49 | *
50 | * (1) always get the pose of the most recent frame:
51 | * -> Implement [publishCamPose].
52 | *
53 | * (2) always get the depthmap of the most recent keyframe
54 | * -> Implement [pushDepthImageFloat] (use inverse depth in [image], and pose / frame information from [KF]).
55 | *
56 | * (3) accumulate final model
57 | * -> Implement [publishKeyframes] (skip for final!=false), and accumulate frames.
58 | *
59 | * (4) get evolving model in real-time
60 | * -> Implement [publishKeyframes] (update all frames for final==false).
61 | *
62 | *
63 | *
64 | *
65 | * ==================== How to use the structs: ===================
66 | * [FrameShell]: minimal struct kept for each frame ever tracked.
67 | * ->camToWorld = camera to world transformation
68 | * ->poseValid = false if [camToWorld] is invalid (only happens for frames during initialization).
69 | * ->trackingRef = Shell of the frame this frame was tracked on.
70 | * ->id = ID of that frame, starting with 0 for the very first frame.
71 | *
72 | * ->incoming_id = ID passed into [addActiveFrame( ImageAndExposure* image, int id )].
73 | * ->timestamp = timestamp passed into [addActiveFrame( ImageAndExposure* image, int id )] as image.timestamp.
74 | *
75 | * [FrameHessian]
76 | * ->immaturePoints: contains points that have not been "activated" (they do however have a depth initialization).
77 | * ->pointHessians: contains active points.
78 | * ->pointHessiansMarginalized: contains marginalized points.
79 | * ->pointHessiansOut: contains outlier points.
80 | *
81 | * ->frameID: incremental ID for keyframes only.
82 | * ->shell: corresponding [FrameShell] struct.
83 | *
84 | *
85 | * [CalibHessian]
86 | * ->fxl(), fyl(), cxl(), cyl(): get optimized, most recent (pinhole) camera intrinsics.
87 | *
88 | *
89 | * [PointHessian]
90 | * ->u,v: pixel-coordinates of point.
91 | * ->idepth_scaled: inverse depth of point.
92 | * DO NOT USE [idepth], since it may be scaled with [SCALE_IDEPTH] ... however that is currently set to 1 so never mind.
93 | * ->host: pointer to host-frame of point.
94 | * ->status: current status of point (ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED)
95 | * ->numGoodResiduals: number of non-outlier residuals supporting this point (approximate).
96 | * ->maxRelBaseline: value roughly proportional to the relative baseline this point was observed with (0 = no baseline).
97 | * points for which this value is low are badly contrained.
98 | * ->idepth_hessian: hessian value (inverse variance) of inverse depth.
99 | *
100 | * [ImmaturePoint]
101 | * ->u,v: pixel-coordinates of point.
102 | * ->idepth_min, idepth_max: the initialization sais that the inverse depth of this point is very likely
103 | * between these two thresholds (their mean being the best guess)
104 | * ->host: pointer to host-frame of point.
105 | */
106 |
107 |
108 |
109 |
110 |
111 |
112 |
113 | class Output3DWrapper
114 | {
115 | public:
116 | Output3DWrapper() {}
117 | virtual ~Output3DWrapper() {}
118 |
119 |
120 | /* Usage:
121 | * Called once after each new Keyframe is inserted & optimized.
122 | * [connectivity] contains for each frame-frame pair the number of [0] active residuals in between them,
123 | * and [1] the number of marginalized reisduals between them.
124 | * frame-frame pairs are encoded as HASH_IDX = [(int)hostFrameKFID << 32 + (int)targetFrameKFID].
125 | * the [***frameKFID] used for hashing correspond to the [FrameHessian]->frameID.
126 | *
127 | * Calling:
128 | * Always called, no overhead if not used.
129 | */
130 | virtual void publishGraph(const std::map<uint64_t,Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i> > > &connectivity) {}
131 |
132 |
133 |
134 |
135 |
136 | /* Usage:
137 | * Called after each new Keyframe is inserted & optimized, with all keyframes that were part of the active window during
138 | * that optimization in [frames] (with final=false). Use to access the new frame pose and points.
139 | * Also called just before a frame is marginalized (with final=true) with only that frame in [frames]; at that point,
140 | * no further updates will ever occur to it's optimized values (pose & idepth values of it's points).
141 | *
142 | * If you want always all most recent values for all frames, just use the [final=false] calls.
143 | * If you only want to get the final model, but don't care about it being delay-free, only use the
144 | * [final=true] calls, to save compute.
145 | *
146 | * Calling:
147 | * Always called, negligible overhead if not used.
148 | */
149 | virtual void publishKeyframes(std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) {}
150 |
151 |
152 |
153 |
154 |
155 | /* Usage:
156 | * Called once for each tracked frame, with the real-time, low-delay frame pose.
157 | *
158 | * Calling:
159 | * Always called, no overhead if not used.
160 | */
161 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) {}
162 |
163 |
164 |
165 |
166 |
167 | /* Usage:
168 | * Called once for each new frame, before it is tracked (i.e., it doesn't have a pose yet).
169 | *
170 | * Calling:
171 | * Always called, no overhead if not used.
172 | */
173 | virtual void pushLiveFrame(FrameHessian* image) {}
174 |
175 |
176 |
177 |
178 | /* called once after a new keyframe is created, with the color-coded, forward-warped inverse depthmap for that keyframe,
179 | * which is used for initial alignment of future frames. Meant for visualization.
180 | *
181 | * Calling:
182 | * Needs to prepare the depth image, so it is only called if [needPushDepthImage()] returned true.
183 | */
184 | virtual void pushDepthImage(MinimalImageB3* image) {}
185 | virtual bool needPushDepthImage() {return false;}
186 |
187 |
188 |
189 | /* Usage:
190 | * called once after a new keyframe is created, with the forward-warped inverse depthmap for that keyframe.
191 | * (<= 0 for pixels without inv. depth value, >0 for pixels with inv. depth value)
192 | *
193 | * Calling:
194 | * Always called, almost no overhead if not used.
195 | */
196 | virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) {}
197 |
198 |
199 |
200 | /* call on finish */
201 | virtual void join() {}
202 |
203 | /* call on reset */
204 | virtual void reset() {}
205 |
206 | };
207 |
208 | }
209 | }
210 |
--------------------------------------------------------------------------------
/src/IOWrapper/OutputWrapper/SampleOutputWrapper.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 | #include "boost/thread.hpp"
27 | #include "util/MinimalImage.h"
28 | #include "IOWrapper/Output3DWrapper.h"
29 |
30 |
31 |
32 | #include "FullSystem/HessianBlocks.h"
33 | #include "util/FrameShell.h"
34 |
35 | namespace dso
36 | {
37 |
38 | class FrameHessian;
39 | class CalibHessian;
40 | class FrameShell;
41 |
42 |
43 | namespace IOWrap
44 | {
45 |
46 | class SampleOutputWrapper : public Output3DWrapper
47 | {
48 | public:
49 | inline SampleOutputWrapper()
50 | {
51 | printf("OUT: Created SampleOutputWrapper\n");
52 | }
53 |
54 | virtual ~SampleOutputWrapper()
55 | {
56 | printf("OUT: Destroyed SampleOutputWrapper\n");
57 | }
58 |
59 | virtual void publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity) override
60 | {
61 | printf("OUT: got graph with %d edges\n", (int)connectivity.size());
62 |
63 | int maxWrite = 5;
64 |
65 | for(const std::pair<uint64_t,Eigen::Vector2i> &p : connectivity)
66 | {
67 | int idHost = p.first>>32;
68 | int idTarget = p.first & ((uint64_t)0xFFFFFFFF);
69 | printf("OUT: Example Edge %d -> %d has %d active and %d marg residuals\n", idHost, idTarget, p.second[0], p.second[1]);
70 | maxWrite--;
71 | if(maxWrite==0) break;
72 | }
73 | }
74 |
75 |
76 |
77 | virtual void publishKeyframes( std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) override
78 | {
79 | for(FrameHessian* f : frames)
80 | {
81 | printf("OUT: KF %d (%s) (id %d, tme %f): %d active, %d marginalized, %d immature points. CameraToWorld:\n",
82 | f->frameID,
83 | final ? "final" : "non-final",
84 | f->shell->incoming_id,
85 | f->shell->timestamp,
86 | (int)f->pointHessians.size(), (int)f->pointHessiansMarginalized.size(), (int)f->immaturePoints.size());
87 | std::cout << f->shell->camToWorld.matrix3x4() << "\n";
88 |
89 |
90 | int maxWrite = 5;
91 | for(PointHessian* p : f->pointHessians)
92 | {
93 | printf("OUT: Example Point x=%.1f, y=%.1f, idepth=%f, idepth std.dev. %f, %d inlier-residuals\n",
94 | p->u, p->v, p->idepth_scaled, sqrt(1.0f / p->idepth_hessian), p->numGoodResiduals );
95 | maxWrite--;
96 | if(maxWrite==0) break;
97 | }
98 | }
99 | }
100 |
101 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override
102 | {
103 | printf("OUT: Current Frame %d (time %f, internal ID %d). CameraToWorld:\n",
104 | frame->incoming_id,
105 | frame->timestamp,
106 | frame->id);
107 | std::cout << frame->camToWorld.matrix3x4() << "\n";
108 | }
109 |
110 |
111 | virtual void pushLiveFrame(FrameHessian* image) override
112 | {
113 | // can be used to get the raw image / intensity pyramid.
114 | }
115 |
116 | virtual void pushDepthImage(MinimalImageB3* image) override
117 | {
118 | // can be used to get the raw image with depth overlay.
119 | }
120 | virtual bool needPushDepthImage() override
121 | {
122 | return false;
123 | }
124 |
125 | virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) override
126 | {
127 | printf("OUT: Predicted depth for KF %d (id %d, time %f, internal frame-ID %d). CameraToWorld:\n",
128 | KF->frameID,
129 | KF->shell->incoming_id,
130 | KF->shell->timestamp,
131 | KF->shell->id);
132 | std::cout << KF->shell->camToWorld.matrix3x4() << "\n";
133 |
134 | int maxWrite = 5;
135 | for(int y=0;y<image->h;y++)
136 | {
137 | for(int x=0;x<image->w;x++)
138 | {
139 | if(image->at(x,y) <= 0) continue;
140 |
141 | printf("OUT: Example Idepth at pixel (%d,%d): %f.\n", x,y,image->at(x,y));
142 | maxWrite--;
143 | if(maxWrite==0) break;
144 | }
145 | if(maxWrite==0) break;
146 | }
147 | }
148 |
149 |
150 | };
151 |
152 |
153 |
154 | }
155 |
156 |
157 |
158 | }
159 |
--------------------------------------------------------------------------------
/src/IOWrapper/Pangolin/KeyFrameDisplay.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #undef Success
28 | #include <Eigen/Core>
29 | #include "util/NumType.h"
30 | #include <pangolin/pangolin.h>
31 |
32 | #include <sstream>
33 | #include <fstream>
34 |
35 | namespace dso
36 | {
37 | class CalibHessian;
38 | class FrameHessian;
39 | class FrameShell;
40 |
41 | namespace IOWrap
42 | {
43 |
44 | template<int ppp>
45 | struct InputPointSparse
46 | {
47 | float u;
48 | float v;
49 | float idpeth;
50 | float idepth_hessian;
51 | float relObsBaseline;
52 | int numGoodRes;
53 | unsigned char color[ppp];
54 | unsigned char status;
55 | };
56 |
57 | struct MyVertex
58 | {
59 | float point[3];
60 | unsigned char color[4];
61 | };
62 |
63 | // stores a pointcloud associated to a Keyframe.
64 | class KeyFrameDisplay
65 | {
66 |
67 | public:
68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
69 | KeyFrameDisplay();
70 | ~KeyFrameDisplay();
71 |
72 | // copies points from KF over to internal buffer,
73 | // keeping some additional information so we can render it differently.
74 | void setFromKF(FrameHessian* fh, CalibHessian* HCalib);
75 |
76 | // copies points from KF over to internal buffer,
77 | // keeping some additional information so we can render it differently.
78 | void setFromF(FrameShell* fs, CalibHessian* HCalib);
79 |
80 | // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing.
81 | bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity);
82 |
83 | // renders cam & pointcloud.
84 | void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1);
85 | void drawPC(float pointSize);
86 |
87 | int id;
88 | bool active;
89 | SE3 camToWorld;
90 |
91 | inline bool operator < (const KeyFrameDisplay& other) const
92 | {
93 | return (id < other.id);
94 | }
95 |
96 |
97 | private:
98 | float fx,fy,cx,cy;
99 | float fxi,fyi,cxi,cyi;
100 | int width, height;
101 |
102 | float my_scaledTH, my_absTH, my_scale;
103 | int my_sparsifyFactor;
104 | int my_displayMode;
105 | float my_minRelBS;
106 | bool needRefresh;
107 |
108 |
109 | int numSparsePoints;
110 | int numSparseBufferSize;
111 | InputPointSparse<MAX_RES_PER_POINT>* originalInputSparse;
112 |
113 |
114 | bool bufferValid;
115 | int numGLBufferPoints;
116 | int numGLBufferGoodPoints;
117 | pangolin::GlBuffer vertexBuffer;
118 | pangolin::GlBuffer colorBuffer;
119 | };
120 |
121 | }
122 | }
123 |
124 |
--------------------------------------------------------------------------------
/src/IOWrapper/Pangolin/PangolinDSOViewer.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 | #include <pangolin/pangolin.h>
27 | #include "boost/thread.hpp"
28 | #include "util/MinimalImage.h"
29 | #include "IOWrapper/Output3DWrapper.h"
30 | #include <map>
31 | #include <deque>
32 |
33 |
34 | namespace dso
35 | {
36 |
37 | class FrameHessian;
38 | class CalibHessian;
39 | class FrameShell;
40 |
41 |
42 | namespace IOWrap
43 | {
44 |
45 | class KeyFrameDisplay;
46 |
47 | struct GraphConnection
48 | {
49 | KeyFrameDisplay* from;
50 | KeyFrameDisplay* to;
51 | int fwdMarg, bwdMarg, fwdAct, bwdAct;
52 | };
53 |
54 |
55 | class PangolinDSOViewer : public Output3DWrapper
56 | {
57 | public:
58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
59 | PangolinDSOViewer(int w, int h, bool startRunThread=true);
60 | virtual ~PangolinDSOViewer();
61 |
62 | void run();
63 | void close();
64 |
65 | void addImageToDisplay(std::string name, MinimalImageB3* image);
66 | void clearAllImagesToDisplay();
67 |
68 |
69 | // ==================== Output3DWrapper Functionality ======================
70 | virtual void publishGraph(const std::map<uint64_t, Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>> &connectivity) override;
71 | virtual void publishKeyframes( std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) override;
72 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override;
73 |
74 |
75 | virtual void pushLiveFrame(FrameHessian* image) override;
76 | virtual void pushDepthImage(MinimalImageB3* image) override;
77 | virtual bool needPushDepthImage() override;
78 |
79 | virtual void join() override;
80 |
81 | virtual void reset() override;
82 | private:
83 |
84 | bool needReset;
85 | void reset_internal();
86 | void drawConstraints();
87 |
88 | boost::thread runThread;
89 | bool running;
90 | int w,h;
91 |
92 |
93 |
94 | // images rendering
95 | boost::mutex openImagesMutex;
96 | MinimalImageB3* internalVideoImg;
97 | MinimalImageB3* internalKFImg;
98 | MinimalImageB3* internalResImg;
99 | bool videoImgChanged, kfImgChanged, resImgChanged;
100 |
101 |
102 |
103 | // 3D model rendering
104 | boost::mutex model3DMutex;
105 | KeyFrameDisplay* currentCam;
106 | std::vector<KeyFrameDisplay*> keyframes;
107 | std::vector<Vec3f,Eigen::aligned_allocator<Vec3f>> allFramePoses;
108 | std::map<int, KeyFrameDisplay*> keyframesByKFID;
109 | std::vector<GraphConnection,Eigen::aligned_allocator<GraphConnection>> connections;
110 |
111 |
112 |
113 | // render settings
114 | bool settings_showKFCameras;
115 | bool settings_showCurrentCamera;
116 | bool settings_showTrajectory;
117 | bool settings_showFullTrajectory;
118 | bool settings_showActiveConstraints;
119 | bool settings_showAllConstraints;
120 |
121 | float settings_scaledVarTH;
122 | float settings_absVarTH;
123 | int settings_pointCloudMode;
124 | float settings_minRelBS;
125 | int settings_sparsity;
126 |
127 |
128 | // timings
129 | struct timeval last_track;
130 | struct timeval last_map;
131 |
132 |
133 | std::deque<float> lastNTrackingMs;
134 | std::deque<float> lastNMappingMs;
135 | };
136 |
137 |
138 |
139 | }
140 |
141 |
142 |
143 | }
144 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/AccumulatedSCHessian.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #include "OptimizationBackend/AccumulatedSCHessian.h"
26 | #include "OptimizationBackend/EnergyFunctional.h"
27 | #include "OptimizationBackend/EnergyFunctionalStructs.h"
28 |
29 | #include "FullSystem/HessianBlocks.h"
30 |
31 | namespace dso
32 | {
33 |
34 | void AccumulatedSCHessianSSE::addPoint(EFPoint* p, bool shiftPriorToZero, int tid)
35 | {
36 | int ngoodres = 0;
37 | for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++;
38 | if(ngoodres==0)
39 | {
40 | p->HdiF=0;
41 | p->bdSumF=0;
42 | p->data->idepth_hessian=0;
43 | p->data->maxRelBaseline=0;
44 | return;
45 | }
46 |
47 | float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF;
48 | if(H < 1e-10) H = 1e-10;
49 |
50 | p->data->idepth_hessian=H;
51 |
52 | p->HdiF = 1.0 / H;
53 | p->bdSumF = p->bd_accAF + p->bd_accLF;
54 | if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF;
55 | VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF;
56 | accHcc[tid].update(Hcd,Hcd,p->HdiF);
57 | accbc[tid].update(Hcd, p->bdSumF * p->HdiF);
58 |
59 | assert(std::isfinite((float)(p->HdiF)));
60 |
61 | int nFrames2 = nframes[tid]*nframes[tid];
62 | for(EFResidual* r1 : p->residualsAll)
63 | {
64 | if(!r1->isActive()) continue;
65 | int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid];
66 |
67 | for(EFResidual* r2 : p->residualsAll)
68 | {
69 | if(!r2->isActive()) continue;
70 |
71 | accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF);
72 | }
73 |
74 | accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF);
75 | accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF);
76 | }
77 | }
78 | void AccumulatedSCHessianSSE::stitchDoubleInternal(
79 | MatXX* H, VecX* b, EnergyFunctional const * const EF,
80 | int min, int max, Vec10* stats, int tid)
81 | {
82 | int toAggregate = NUM_THREADS;
83 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate.
84 | if(min==max) return;
85 |
86 |
87 | int nf = nframes[0];
88 | int nframes2 = nf*nf;
89 |
90 | for(int k=min;k<max;k++)
91 | {
92 | int i = k%nf;
93 | int j = k/nf;
94 |
95 | int iIdx = CPARS+i*8;
96 | int jIdx = CPARS+j*8;
97 | int ijIdx = i+nf*j;
98 |
99 | Mat8C Hpc = Mat8C::Zero();
100 | Vec8 bp = Vec8::Zero();
101 |
102 | for(int tid2=0;tid2 < toAggregate;tid2++)
103 | {
104 | accE[tid2][ijIdx].finish();
105 | accEB[tid2][ijIdx].finish();
106 | Hpc += accE[tid2][ijIdx].A1m.cast<double>();
107 | bp += accEB[tid2][ijIdx].A1m.cast<double>();
108 | }
109 |
110 | H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc;
111 | H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc;
112 | b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp;
113 | b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp;
114 |
115 |
116 |
117 | for(int k=0;k<nf;k++)
118 | {
119 | int kIdx = CPARS+k*8;
120 | int ijkIdx = ijIdx + k*nframes2;
121 | int ikIdx = i+nf*k;
122 |
123 | Mat88 accDM = Mat88::Zero();
124 |
125 | for(int tid2=0;tid2 < toAggregate;tid2++)
126 | {
127 | accD[tid2][ijkIdx].finish();
128 | if(accD[tid2][ijkIdx].num == 0) continue;
129 | accDM += accD[tid2][ijkIdx].A1m.cast<double>();
130 | }
131 |
132 | H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
133 | H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
134 | H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
135 | H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
136 | }
137 | }
138 |
139 | if(min==0)
140 | {
141 | for(int tid2=0;tid2 < toAggregate;tid2++)
142 | {
143 | accHcc[tid2].finish();
144 | accbc[tid2].finish();
145 | H[tid].topLeftCorner<CPARS,CPARS>() += accHcc[tid2].A1m.cast<double>();
146 | b[tid].head<CPARS>() += accbc[tid2].A1m.cast<double>();
147 | }
148 | }
149 |
150 |
151 | // // ----- new: copy transposed parts for calibration only.
152 | // for(int h=0;h<nf;h++)
153 | // {
154 | // int hIdx = 4+h*8;
155 | // H.block<4,8>(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose();
156 | // }
157 | }
158 |
159 | void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid)
160 | {
161 |
162 | int nf = nframes[0];
163 | int nframes2 = nf*nf;
164 |
165 | H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS);
166 | b = VecX::Zero(nf*8+CPARS);
167 |
168 |
169 | for(int i=0;i<nf;i++)
170 | for(int j=0;j<nf;j++)
171 | {
172 | int iIdx = CPARS+i*8;
173 | int jIdx = CPARS+j*8;
174 | int ijIdx = i+nf*j;
175 |
176 | accE[tid][ijIdx].finish();
177 | accEB[tid][ijIdx].finish();
178 |
179 | Mat8C accEM = accE[tid][ijIdx].A1m.cast<double>();
180 | Vec8 accEBV = accEB[tid][ijIdx].A1m.cast<double>();
181 |
182 | H.block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * accEM;
183 | H.block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * accEM;
184 |
185 | b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV;
186 | b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV;
187 |
188 | for(int k=0;k<nf;k++)
189 | {
190 | int kIdx = CPARS+k*8;
191 | int ijkIdx = ijIdx + k*nframes2;
192 | int ikIdx = i+nf*k;
193 |
194 | accD[tid][ijkIdx].finish();
195 | if(accD[tid][ijkIdx].num == 0) continue;
196 | Mat88 accDM = accD[tid][ijkIdx].A1m.cast<double>();
197 |
198 | H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
199 |
200 | H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
201 |
202 | H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose();
203 |
204 | H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose();
205 | }
206 | }
207 |
208 | accHcc[tid].finish();
209 | accbc[tid].finish();
210 | H.topLeftCorner<CPARS,CPARS>() = accHcc[tid].A1m.cast<double>();
211 | b.head<CPARS>() = accbc[tid].A1m.cast<double>();
212 |
213 | // ----- new: copy transposed parts for calibration only.
214 | for(int h=0;h<nf;h++)
215 | {
216 | int hIdx = CPARS+h*8;
217 | H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
218 | }
219 | }
220 |
221 | }
222 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/AccumulatedSCHessian.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 | #include "util/IndexThreadReduce.h"
30 | #include "OptimizationBackend/MatrixAccumulators.h"
31 | #include "vector"
32 | #include <math.h>
33 |
34 | namespace dso
35 | {
36 |
37 | class EFPoint;
38 | class EnergyFunctional;
39 |
40 |
41 | class AccumulatedSCHessianSSE
42 | {
43 | public:
44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
45 | inline AccumulatedSCHessianSSE()
46 | {
47 | for(int i=0;i<NUM_THREADS;i++)
48 | {
49 | accE[i]=0;
50 | accEB[i]=0;
51 | accD[i]=0;
52 | nframes[i]=0;
53 | }
54 | };
55 | inline ~AccumulatedSCHessianSSE()
56 | {
57 | for(int i=0;i<NUM_THREADS;i++)
58 | {
59 | if(accE[i] != 0) delete[] accE[i];
60 | if(accEB[i] != 0) delete[] accEB[i];
61 | if(accD[i] != 0) delete[] accD[i];
62 | }
63 | };
64 |
65 | inline void setZero(int n, int min=0, int max=1, Vec10* stats=0, int tid=0)
66 | {
67 | if(n != nframes[tid])
68 | {
69 | if(accE[tid] != 0) delete[] accE[tid];
70 | if(accEB[tid] != 0) delete[] accEB[tid];
71 | if(accD[tid] != 0) delete[] accD[tid];
72 | accE[tid] = new AccumulatorXX<8,CPARS>[n*n];
73 | accEB[tid] = new AccumulatorX<8>[n*n];
74 | accD[tid] = new AccumulatorXX<8,8>[n*n*n];
75 | }
76 | accbc[tid].initialize();
77 | accHcc[tid].initialize();
78 |
79 | for(int i=0;i<n*n;i++)
80 | {
81 | accE[tid][i].initialize();
82 | accEB[tid][i].initialize();
83 |
84 | for(int j=0;j<n;j++)
85 | accD[tid][i*n+j].initialize();
86 | }
87 | nframes[tid]=n;
88 | }
89 | void stitchDouble(MatXX &H_sc, VecX &b_sc, EnergyFunctional const * const EF, int tid=0);
90 | void addPoint(EFPoint* p, bool shiftPriorToZero, int tid=0);
91 |
92 |
93 | void stitchDoubleMT(IndexThreadReduce<Vec10>* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT)
94 | {
95 | // sum up, splitting by bock in square.
96 | if(MT)
97 | {
98 | MatXX Hs[NUM_THREADS];
99 | VecX bs[NUM_THREADS];
100 | for(int i=0;i<NUM_THREADS;i++)
101 | {
102 | assert(nframes[0] == nframes[i]);
103 | Hs[i] = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
104 | bs[i] = VecX::Zero(nframes[0]*8+CPARS);
105 | }
106 |
107 | red->reduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal,
108 | this,Hs, bs, EF, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0);
109 |
110 | // sum up results
111 | H = Hs[0];
112 | b = bs[0];
113 |
114 | for(int i=1;i<NUM_THREADS;i++)
115 | {
116 | H.noalias() += Hs[i];
117 | b.noalias() += bs[i];
118 | }
119 | }
120 | else
121 | {
122 | H = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
123 | b = VecX::Zero(nframes[0]*8+CPARS);
124 | stitchDoubleInternal(&H, &b, EF,0,nframes[0]*nframes[0],0,-1);
125 | }
126 |
127 | // make diagonal by copying over parts.
128 | for(int h=0;h<nframes[0];h++)
129 | {
130 | int hIdx = CPARS+h*8;
131 | H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
132 | }
133 | }
134 |
135 |
136 | AccumulatorXX<8,CPARS>* accE[NUM_THREADS];
137 | AccumulatorX<8>* accEB[NUM_THREADS];
138 | AccumulatorXX<8,8>* accD[NUM_THREADS];
139 | AccumulatorXX<CPARS,CPARS> accHcc[NUM_THREADS];
140 | AccumulatorX<CPARS> accbc[NUM_THREADS];
141 | int nframes[NUM_THREADS];
142 |
143 |
144 | void addPointsInternal(
145 | std::vector<EFPoint*>* points, bool shiftPriorToZero,
146 | int min=0, int max=1, Vec10* stats=0, int tid=0)
147 | {
148 | for(int i=min;i<max;i++) addPoint((*points)[i],shiftPriorToZero,tid);
149 | }
150 |
151 | private:
152 |
153 | void stitchDoubleInternal(
154 | MatXX* H, VecX* b, EnergyFunctional const * const EF,
155 | int min, int max, Vec10* stats, int tid);
156 | };
157 |
158 | }
159 |
160 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/AccumulatedTopHessian.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 | #include "OptimizationBackend/MatrixAccumulators.h"
30 | #include "vector"
31 | #include <math.h>
32 | #include "util/IndexThreadReduce.h"
33 |
34 |
35 | namespace dso
36 | {
37 |
38 | class EFPoint;
39 | class EnergyFunctional;
40 |
41 |
42 |
43 | class AccumulatedTopHessianSSE
44 | {
45 | public:
46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
47 | inline AccumulatedTopHessianSSE()
48 | {
49 | for(int tid=0;tid < NUM_THREADS; tid++)
50 | {
51 | nres[tid]=0;
52 | acc[tid]=0;
53 | nframes[tid]=0;
54 | }
55 |
56 | };
57 | inline ~AccumulatedTopHessianSSE()
58 | {
59 | for(int tid=0;tid < NUM_THREADS; tid++)
60 | {
61 | if(acc[tid] != 0) delete[] acc[tid];
62 | }
63 | };
64 |
65 | inline void setZero(int nFrames, int min=0, int max=1, Vec10* stats=0, int tid=0)
66 | {
67 |
68 | if(nFrames != nframes[tid])
69 | {
70 | if(acc[tid] != 0) delete[] acc[tid];
71 | #if USE_XI_MODEL
72 | acc[tid] = new Accumulator14[nFrames*nFrames];
73 | #else
74 | acc[tid] = new AccumulatorApprox[nFrames*nFrames];
75 | #endif
76 | }
77 |
78 | for(int i=0;i<nFrames*nFrames;i++)
79 | { acc[tid][i].initialize(); }
80 |
81 | nframes[tid]=nFrames;
82 | nres[tid]=0;
83 |
84 | }
85 | void stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool useDelta, int tid=0);
86 |
87 | template<int mode> void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0);
88 |
89 |
90 |
91 | void stitchDoubleMT(IndexThreadReduce<Vec10>* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT)
92 | {
93 | // sum up, splitting by bock in square.
94 | if(MT)
95 | {
96 | MatXX Hs[NUM_THREADS];
97 | VecX bs[NUM_THREADS];
98 | for(int i=0;i<NUM_THREADS;i++)
99 | {
100 | assert(nframes[0] == nframes[i]);
101 | Hs[i] = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
102 | bs[i] = VecX::Zero(nframes[0]*8+CPARS);
103 | }
104 |
105 | red->reduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal,
106 | this,Hs, bs, EF, usePrior, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0);
107 |
108 | // sum up results
109 | H = Hs[0];
110 | b = bs[0];
111 |
112 | for(int i=1;i<NUM_THREADS;i++)
113 | {
114 | H.noalias() += Hs[i];
115 | b.noalias() += bs[i];
116 | nres[0] += nres[i];
117 | }
118 | }
119 | else
120 | {
121 | H = MatXX::Zero(nframes[0]*8+CPARS, nframes[0]*8+CPARS);
122 | b = VecX::Zero(nframes[0]*8+CPARS);
123 | stitchDoubleInternal(&H, &b, EF, usePrior,0,nframes[0]*nframes[0],0,-1);
124 | }
125 |
126 | // make diagonal by copying over parts.
127 | for(int h=0;h<nframes[0];h++)
128 | {
129 | int hIdx = CPARS+h*8;
130 | H.block<CPARS,8>(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose();
131 |
132 | for(int t=h+1;t<nframes[0];t++)
133 | {
134 | int tIdx = CPARS+t*8;
135 | H.block<8,8>(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose();
136 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose();
137 | }
138 | }
139 | }
140 |
141 |
142 |
143 |
144 | int nframes[NUM_THREADS];
145 |
146 | EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS];
147 |
148 |
149 | int nres[NUM_THREADS];
150 |
151 |
152 | template<int mode> void addPointsInternal(
153 | std::vector<EFPoint*>* points, EnergyFunctional const * const ef,
154 | int min=0, int max=1, Vec10* stats=0, int tid=0)
155 | {
156 | for(int i=min;i<max;i++) addPoint<mode>((*points)[i],ef,tid);
157 | }
158 |
159 |
160 |
161 | private:
162 |
163 | void stitchDoubleInternal(
164 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior,
165 | int min, int max, Vec10* stats, int tid);
166 | };
167 | }
168 |
169 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/EnergyFunctional.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 | #include "util/IndexThreadReduce.h"
30 | #include "vector"
31 | #include <math.h>
32 | #include "map"
33 |
34 |
35 | namespace dso
36 | {
37 |
38 | class PointFrameResidual;
39 | class CalibHessian;
40 | class FrameHessian;
41 | class PointHessian;
42 |
43 |
44 | class EFResidual;
45 | class EFPoint;
46 | class EFFrame;
47 | class EnergyFunctional;
48 | class AccumulatedTopHessian;
49 | class AccumulatedTopHessianSSE;
50 | class AccumulatedSCHessian;
51 | class AccumulatedSCHessianSSE;
52 |
53 |
54 | extern bool EFAdjointsValid;
55 | extern bool EFIndicesValid;
56 | extern bool EFDeltaValid;
57 |
58 |
59 |
60 | class EnergyFunctional {
61 | public:
62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
63 | friend class EFFrame;
64 | friend class EFPoint;
65 | friend class EFResidual;
66 | friend class AccumulatedTopHessian;
67 | friend class AccumulatedTopHessianSSE;
68 | friend class AccumulatedSCHessian;
69 | friend class AccumulatedSCHessianSSE;
70 |
71 | EnergyFunctional();
72 | ~EnergyFunctional();
73 |
74 |
75 | EFResidual* insertResidual(PointFrameResidual* r);
76 | EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib);
77 | EFPoint* insertPoint(PointHessian* ph);
78 |
79 | void dropResidual(EFResidual* r);
80 | void marginalizeFrame(EFFrame* fh);
81 | void removePoint(EFPoint* ph);
82 |
83 |
84 |
85 | void marginalizePointsF();
86 | void dropPointsF();
87 | void solveSystemF(int iteration, double lambda, CalibHessian* HCalib);
88 | double calcMEnergyF();
89 | double calcLEnergyF_MT();
90 |
91 |
92 | void makeIDX();
93 |
94 | void setDeltaF(CalibHessian* HCalib);
95 |
96 | void setAdjointsF(CalibHessian* Hcalib);
97 |
98 | std::vector<EFFrame*> frames;
99 | int nPoints, nFrames, nResiduals;
100 |
101 | MatXX HM;
102 | VecX bM;
103 |
104 | int resInA, resInL, resInM;
105 | MatXX lastHS;
106 | VecX lastbS;
107 | VecX lastX;
108 | std::vector<VecX> lastNullspaces_forLogging;
109 | std::vector<VecX> lastNullspaces_pose;
110 | std::vector<VecX> lastNullspaces_scale;
111 | std::vector<VecX> lastNullspaces_affA;
112 | std::vector<VecX> lastNullspaces_affB;
113 |
114 | IndexThreadReduce<Vec10>* red;
115 |
116 |
117 | std::map<uint64_t,
118 | Eigen::Vector2i,
119 | std::less<uint64_t>,
120 | Eigen::aligned_allocator<std::pair<const uint64_t, Eigen::Vector2i>>
121 | > connectivityMap;
122 |
123 | private:
124 |
125 | VecX getStitchedDeltaF() const;
126 |
127 | void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT);
128 | void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid);
129 |
130 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT);
131 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT);
132 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT);
133 |
134 | void calcLEnergyPt(int min, int max, Vec10* stats, int tid);
135 |
136 | void orthogonalize(VecX* b, MatXX* H);
137 | Mat18f* adHTdeltaF;
138 |
139 | Mat88* adHost;
140 | Mat88* adTarget;
141 |
142 | Mat88f* adHostF;
143 | Mat88f* adTargetF;
144 |
145 |
146 | VecC cPrior;
147 | VecCf cDeltaF;
148 | VecCf cPriorF;
149 |
150 | AccumulatedTopHessianSSE* accSSE_top_L;
151 | AccumulatedTopHessianSSE* accSSE_top_A;
152 |
153 |
154 | AccumulatedSCHessianSSE* accSSE_bot;
155 |
156 | std::vector<EFPoint*> allPoints;
157 | std::vector<EFPoint*> allPointsToMarg;
158 |
159 | float currentLambda;
160 | };
161 | }
162 |
163 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/EnergyFunctionalStructs.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #include "OptimizationBackend/EnergyFunctionalStructs.h"
26 | #include "OptimizationBackend/EnergyFunctional.h"
27 | #include "FullSystem/FullSystem.h"
28 | #include "FullSystem/HessianBlocks.h"
29 | #include "FullSystem/Residuals.h"
30 |
31 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__)
32 | #include "SSE2NEON.h"
33 | #endif
34 |
35 | namespace dso
36 | {
37 |
38 |
39 | void EFResidual::takeDataF()
40 | {
41 | std::swap<RawResidualJacobian*>(J, data->J);
42 |
43 | Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd;
44 |
45 | for(int i=0;i<6;i++)
46 | JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1];
47 |
48 | JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd;
49 | }
50 |
51 |
52 | void EFFrame::takeData()
53 | {
54 | prior = data->getPrior().head<8>();
55 | delta = data->get_state_minus_stateZero().head<8>();
56 | delta_prior = (data->get_state() - data->getPriorZero()).head<8>();
57 |
58 |
59 |
60 | // Vec10 state_zero = data->get_state_zero();
61 | // state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0);
62 | // state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3);
63 | // state_zero[6] = SCALE_A * state_zero[6];
64 | // state_zero[7] = SCALE_B * state_zero[7];
65 | // state_zero[8] = SCALE_A * state_zero[8];
66 | // state_zero[9] = SCALE_B * state_zero[9];
67 | //
68 | // std::cout << "state_zero: " << state_zero.transpose() << "\n";
69 |
70 |
71 | assert(data->frameID != -1);
72 |
73 | frameID = data->frameID;
74 | }
75 |
76 |
77 |
78 |
79 | void EFPoint::takeData()
80 | {
81 | priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0;
82 | if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0;
83 |
84 | deltaF = data->idepth-data->idepth_zero;
85 | }
86 |
87 |
88 | void EFResidual::fixLinearizationF(EnergyFunctional* ef)
89 | {
90 | Vec8f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX];
91 |
92 | // compute Jp*delta
93 | __m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>())
94 | +J->Jpdc[0].dot(ef->cDeltaF)
95 | +J->Jpdd[0]*point->deltaF);
96 | __m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>())
97 | +J->Jpdc[1].dot(ef->cDeltaF)
98 | +J->Jpdd[1]*point->deltaF);
99 | __m128 delta_a = _mm_set1_ps((float)(dp[6]));
100 | __m128 delta_b = _mm_set1_ps((float)(dp[7]));
101 |
102 | for(int i=0;i<patternNum;i+=4)
103 | {
104 | // PATTERN: rtz = resF - [JI*Jp Ja]*delta.
105 | __m128 rtz = _mm_load_ps(((float*)&J->resF)+i);
106 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x));
107 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y));
108 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a));
109 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b));
110 | _mm_store_ps(((float*)&res_toZeroF)+i, rtz);
111 | }
112 |
113 | isLinearized = true;
114 | }
115 |
116 | }
117 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/EnergyFunctionalStructs.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 | #include "vector"
30 | #include <math.h>
31 | #include "OptimizationBackend/RawResidualJacobian.h"
32 |
33 | namespace dso
34 | {
35 |
36 | class PointFrameResidual;
37 | class CalibHessian;
38 | class FrameHessian;
39 | class PointHessian;
40 |
41 | class EFResidual;
42 | class EFPoint;
43 | class EFFrame;
44 | class EnergyFunctional;
45 |
46 |
47 |
48 |
49 |
50 |
51 | class EFResidual
52 | {
53 | public:
54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
55 |
56 | inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) :
57 | data(org), point(point_), host(host_), target(target_)
58 | {
59 | isLinearized=false;
60 | isActiveAndIsGoodNEW=false;
61 | J = new RawResidualJacobian();
62 | assert(((long)this)%16==0);
63 | assert(((long)J)%16==0);
64 | }
65 | inline ~EFResidual()
66 | {
67 | delete J;
68 | }
69 |
70 |
71 | void takeDataF();
72 |
73 |
74 | void fixLinearizationF(EnergyFunctional* ef);
75 |
76 |
77 | // structural pointers
78 | PointFrameResidual* data;
79 | int hostIDX, targetIDX;
80 | EFPoint* point;
81 | EFFrame* host;
82 | EFFrame* target;
83 | int idxInAll;
84 |
85 | RawResidualJacobian* J;
86 |
87 | VecNRf res_toZeroF;
88 | Vec8f JpJdF;
89 |
90 |
91 | // status.
92 | bool isLinearized;
93 |
94 | // if residual is not OOB & not OUTLIER & should be used during accumulations
95 | bool isActiveAndIsGoodNEW;
96 | inline const bool &isActive() const {return isActiveAndIsGoodNEW;}
97 | };
98 |
99 |
100 | enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP};
101 |
102 | class EFPoint
103 | {
104 | public:
105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
106 | EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_)
107 | {
108 | takeData();
109 | stateFlag=EFPointStatus::PS_GOOD;
110 | }
111 | void takeData();
112 |
113 | PointHessian* data;
114 |
115 |
116 |
117 | float priorF;
118 | float deltaF;
119 |
120 |
121 | // constant info (never changes in-between).
122 | int idxInPoints;
123 | EFFrame* host;
124 |
125 | // contains all residuals.
126 | std::vector<EFResidual*> residualsAll;
127 |
128 | float bdSumF;
129 | float HdiF;
130 | float Hdd_accLF;
131 | VecCf Hcd_accLF;
132 | float bd_accLF;
133 | float Hdd_accAF;
134 | VecCf Hcd_accAF;
135 | float bd_accAF;
136 |
137 |
138 | EFPointStatus stateFlag;
139 | };
140 |
141 |
142 |
143 | class EFFrame
144 | {
145 | public:
146 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
147 | EFFrame(FrameHessian* d) : data(d)
148 | {
149 | takeData();
150 | }
151 | void takeData();
152 |
153 |
154 | Vec8 prior; // prior hessian (diagonal)
155 | Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior)
156 | Vec8 delta; // state - state_zero.
157 |
158 |
159 |
160 | std::vector<EFPoint*> points;
161 | FrameHessian* data;
162 | int idx; // idx in frames.
163 |
164 | int frameID;
165 | };
166 |
167 | }
168 |
169 |
--------------------------------------------------------------------------------
/src/OptimizationBackend/RawResidualJacobian.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 |
28 | #include "util/NumType.h"
29 |
30 | namespace dso
31 | {
32 | struct RawResidualJacobian
33 | {
34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
35 | // ================== new structure: save independently =============.
36 | VecNRf resF;
37 |
38 | // the two rows of d[x,y]/d[xi].
39 | Vec6f Jpdxi[2]; // 2x6
40 |
41 | // the two rows of d[x,y]/d[C].
42 | VecCf Jpdc[2]; // 2x4
43 |
44 | // the two rows of d[x,y]/d[idepth].
45 | Vec2f Jpdd; // 2x1
46 |
47 | // the two columns of d[r]/d[x,y].
48 | VecNRf JIdx[2]; // 9x2
49 |
50 | // = the two columns of d[r] / d[ab]
51 | VecNRf JabF[2]; // 9x2
52 |
53 |
54 | // = JIdx^T * JIdx (inner product). Only as a shorthand.
55 | Mat22f JIdx2; // 2x2
56 | // = Jab^T * JIdx (inner product). Only as a shorthand.
57 | Mat22f JabJIdx; // 2x2
58 | // = Jab^T * Jab (inner product). Only as a shorthand.
59 | Mat22f Jab2; // 2x2
60 |
61 | };
62 | }
63 |
64 |
--------------------------------------------------------------------------------
/src/util/FrameShell.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "util/NumType.h"
28 | #include "algorithm"
29 |
30 | namespace dso
31 | {
32 |
33 |
34 | class FrameShell
35 | {
36 | public:
37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
38 | int id; // INTERNAL ID, starting at zero.
39 | int incoming_id; // ID passed into DSO
40 | double timestamp; // timestamp passed into DSO.
41 |
42 | // set once after tracking
43 | SE3 camToTrackingRef;
44 | FrameShell* trackingRef;
45 |
46 | // constantly adapted.
47 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex].
48 | AffLight aff_g2l;
49 | bool poseValid;
50 |
51 | // statisitcs
52 | int statistics_outlierResOnThis;
53 | int statistics_goodResOnThis;
54 | int marginalizedAt;
55 | double movedByOpt;
56 |
57 |
58 | inline FrameShell()
59 | {
60 | id=0;
61 | poseValid=true;
62 | camToWorld = SE3();
63 | timestamp=0;
64 | marginalizedAt=-1;
65 | movedByOpt=0;
66 | statistics_outlierResOnThis=statistics_goodResOnThis=0;
67 | trackingRef=0;
68 | camToTrackingRef = SE3();
69 | }
70 | };
71 |
72 |
73 | }
74 |
75 |
--------------------------------------------------------------------------------
/src/util/ImageAndExposure.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 | #include <cstring>
27 | #include <iostream>
28 |
29 |
30 | namespace dso
31 | {
32 |
33 |
34 | class ImageAndExposure
35 | {
36 | public:
37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
38 | float* image; // irradiance. between 0 and 256
39 | int w,h; // width and height;
40 | double timestamp;
41 | float exposure_time; // exposure time in ms.
42 | inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_)
43 | {
44 | image = new float[w*h];
45 | exposure_time=1;
46 | }
47 | inline ~ImageAndExposure()
48 | {
49 | delete[] image;
50 | }
51 |
52 | inline void copyMetaTo(ImageAndExposure &other)
53 | {
54 | other.exposure_time = exposure_time;
55 | }
56 |
57 | inline ImageAndExposure* getDeepCopy()
58 | {
59 | ImageAndExposure* img = new ImageAndExposure(w,h,timestamp);
60 | img->exposure_time = exposure_time;
61 | memcpy(img->image, image, w*h*sizeof(float));
62 | return img;
63 | }
64 | };
65 |
66 |
67 | }
68 |
--------------------------------------------------------------------------------
/src/util/IndexThreadReduce.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #pragma once
27 | #include "util/settings.h"
28 | #include "boost/thread.hpp"
29 | #include <stdio.h>
30 | #include <iostream>
31 |
32 |
33 |
34 | namespace dso
35 | {
36 |
37 | template<typename Running>
38 | class IndexThreadReduce
39 | {
40 |
41 | public:
42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
43 |
44 | inline IndexThreadReduce()
45 | {
46 | nextIndex = 0;
47 | maxIndex = 0;
48 | stepSize = 1;
49 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4);
50 |
51 | running = true;
52 | for(int i=0;i<NUM_THREADS;i++)
53 | {
54 | isDone[i] = false;
55 | gotOne[i] = true;
56 | workerThreads[i] = boost::thread(&IndexThreadReduce::workerLoop, this, i);
57 | }
58 |
59 | }
60 | inline ~IndexThreadReduce()
61 | {
62 | running = false;
63 |
64 | exMutex.lock();
65 | todo_signal.notify_all();
66 | exMutex.unlock();
67 |
68 | for(int i=0;i<NUM_THREADS;i++)
69 | workerThreads[i].join();
70 |
71 |
72 | printf("destroyed ThreadReduce\n");
73 |
74 | }
75 |
76 | inline void reduce(boost::function<void(int,int,Running*,int)> callPerIndex, int first, int end, int stepSize = 0)
77 | {
78 |
79 | memset(&stats, 0, sizeof(Running));
80 |
81 | // if(!multiThreading)
82 | // {
83 | // callPerIndex(first, end, &stats, 0);
84 | // return;
85 | // }
86 |
87 |
88 |
89 | if(stepSize == 0)
90 | stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS;
91 |
92 |
93 | //printf("reduce called\n");
94 |
95 | boost::unique_lock<boost::mutex> lock(exMutex);
96 |
97 | // save
98 | this->callPerIndex = callPerIndex;
99 | nextIndex = first;
100 | maxIndex = end;
101 | this->stepSize = stepSize;
102 |
103 | // go worker threads!
104 | for(int i=0;i<NUM_THREADS;i++)
105 | {
106 | isDone[i] = false;
107 | gotOne[i] = false;
108 | }
109 |
110 | // let them start!
111 | todo_signal.notify_all();
112 |
113 |
114 | //printf("reduce waiting for threads to finish\n");
115 | // wait for all worker threads to signal they are done.
116 | while(true)
117 | {
118 | // wait for at least one to finish
119 | done_signal.wait(lock);
120 | //printf("thread finished!\n");
121 |
122 | // check if actually all are finished.
123 | bool allDone = true;
124 | for(int i=0;i<NUM_THREADS;i++)
125 | allDone = allDone && isDone[i];
126 |
127 | // all are finished! exit.
128 | if(allDone)
129 | break;
130 | }
131 |
132 | nextIndex = 0;
133 | maxIndex = 0;
134 | this->callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4);
135 |
136 | //printf("reduce done (all threads finished)\n");
137 | }
138 |
139 | Running stats;
140 |
141 | private:
142 | boost::thread workerThreads[NUM_THREADS];
143 | bool isDone[NUM_THREADS];
144 | bool gotOne[NUM_THREADS];
145 |
146 | boost::mutex exMutex;
147 | boost::condition_variable todo_signal;
148 | boost::condition_variable done_signal;
149 |
150 | int nextIndex;
151 | int maxIndex;
152 | int stepSize;
153 |
154 | bool running;
155 |
156 | boost::function<void(int,int,Running*,int)> callPerIndex;
157 |
158 | void callPerIndexDefault(int i, int j,Running* k, int tid)
159 | {
160 | printf("ERROR: should never be called....\n");
161 | assert(false);
162 | }
163 |
164 | void workerLoop(int idx)
165 | {
166 | boost::unique_lock<boost::mutex> lock(exMutex);
167 |
168 | while(running)
169 | {
170 | // try to get something to do.
171 | int todo = 0;
172 | bool gotSomething = false;
173 | if(nextIndex < maxIndex)
174 | {
175 | // got something!
176 | todo = nextIndex;
177 | nextIndex+=stepSize;
178 | gotSomething = true;
179 | }
180 |
181 | // if got something: do it (unlock in the meantime)
182 | if(gotSomething)
183 | {
184 | lock.unlock();
185 |
186 | assert(callPerIndex != 0);
187 |
188 | Running s; memset(&s, 0, sizeof(Running));
189 | callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx);
190 | gotOne[idx] = true;
191 | lock.lock();
192 | stats += s;
193 | }
194 |
195 | // otherwise wait on signal, releasing lock in the meantime.
196 | else
197 | {
198 | if(!gotOne[idx])
199 | {
200 | lock.unlock();
201 | assert(callPerIndex != 0);
202 | Running s; memset(&s, 0, sizeof(Running));
203 | callPerIndex(0, 0, &s, idx);
204 | gotOne[idx] = true;
205 | lock.lock();
206 | stats += s;
207 | }
208 | isDone[idx] = true;
209 | //printf("worker %d waiting..\n", idx);
210 | done_signal.notify_all();
211 | todo_signal.wait(lock);
212 | }
213 | }
214 | }
215 | };
216 | }
217 |
--------------------------------------------------------------------------------
/src/util/MinimalImage.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "util/NumType.h"
28 | #include "algorithm"
29 |
30 | namespace dso
31 | {
32 |
33 | template<typename T>
34 | class MinimalImage
35 | {
36 | public:
37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 | int w;
39 | int h;
40 | T* data;
41 |
42 | /*
43 | * creates minimal image with own memory
44 | */
45 | inline MinimalImage(int w_, int h_) : w(w_), h(h_)
46 | {
47 | data = new T[w*h];
48 | ownData=true;
49 | }
50 |
51 | /*
52 | * creates minimal image wrapping around existing memory
53 | */
54 | inline MinimalImage(int w_, int h_, T* data_) : w(w_), h(h_)
55 | {
56 | data = data_;
57 | ownData=false;
58 | }
59 |
60 | inline ~MinimalImage()
61 | {
62 | if(ownData) delete [] data;
63 | }
64 |
65 | inline MinimalImage* getClone()
66 | {
67 | MinimalImage* clone = new MinimalImage(w,h);
68 | memcpy(clone->data, data, sizeof(T)*w*h);
69 | return clone;
70 | }
71 |
72 |
73 | inline T& at(int x, int y) {return data[(int)x+((int)y)*w];}
74 | inline T& at(int i) {return data[i];}
75 |
76 | inline void setBlack()
77 | {
78 | memset(data, 0, sizeof(T)*w*h);
79 | }
80 |
81 | inline void setConst(T val)
82 | {
83 | for(int i=0;i<w*h;i++) data[i] = val;
84 | }
85 |
86 | inline void setPixel1(const float &u, const float &v, T val)
87 | {
88 | at(u+0.5f,v+0.5f) = val;
89 | }
90 |
91 | inline void setPixel4(const float &u, const float &v, T val)
92 | {
93 | at(u+1.0f,v+1.0f) = val;
94 | at(u+1.0f,v) = val;
95 | at(u,v+1.0f) = val;
96 | at(u,v) = val;
97 | }
98 |
99 | inline void setPixel9(const int &u, const int &v, T val)
100 | {
101 | at(u+1,v-1) = val;
102 | at(u+1,v) = val;
103 | at(u+1,v+1) = val;
104 | at(u,v-1) = val;
105 | at(u,v) = val;
106 | at(u,v+1) = val;
107 | at(u-1,v-1) = val;
108 | at(u-1,v) = val;
109 | at(u-1,v+1) = val;
110 | }
111 |
112 | inline void setPixelCirc(const int &u, const int &v, T val)
113 | {
114 | for(int i=-3;i<=3;i++)
115 | {
116 | at(u+3,v+i) = val;
117 | at(u-3,v+i) = val;
118 | at(u+2,v+i) = val;
119 | at(u-2,v+i) = val;
120 |
121 | at(u+i,v-3) = val;
122 | at(u+i,v+3) = val;
123 | at(u+i,v-2) = val;
124 | at(u+i,v+2) = val;
125 | }
126 | }
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 | private:
156 | bool ownData;
157 | };
158 |
159 | typedef Eigen::Matrix<unsigned char,3,1> Vec3b;
160 | typedef MinimalImage<float> MinimalImageF;
161 | typedef MinimalImage<Vec3f> MinimalImageF3;
162 | typedef MinimalImage<unsigned char> MinimalImageB;
163 | typedef MinimalImage<Vec3b> MinimalImageB3;
164 | typedef MinimalImage<unsigned short> MinimalImageB16;
165 |
166 | }
167 |
168 |
--------------------------------------------------------------------------------
/src/util/NumType.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "Eigen/Core"
28 | #include "sophus/sim3.hpp"
29 | #include "sophus/se3.hpp"
30 |
31 |
32 | namespace dso
33 | {
34 |
35 | // CAMERA MODEL TO USE
36 |
37 |
38 | #define SSEE(val,idx) (*(((float*)&val)+idx))
39 |
40 |
41 | #define MAX_RES_PER_POINT 8
42 | #define NUM_THREADS 6
43 |
44 |
45 | #define todouble(x) (x).cast<double>()
46 |
47 |
48 | typedef Sophus::SE3d SE3;
49 | typedef Sophus::Sim3d Sim3;
50 | typedef Sophus::SO3d SO3;
51 |
52 |
53 |
54 | #define CPARS 4
55 |
56 |
57 | typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatXX;
58 | typedef Eigen::Matrix<double,CPARS,CPARS> MatCC;
59 | #define MatToDynamic(x) MatXX(x)
60 |
61 |
62 |
63 | typedef Eigen::Matrix<double,CPARS,10> MatC10;
64 | typedef Eigen::Matrix<double,10,10> Mat1010;
65 | typedef Eigen::Matrix<double,13,13> Mat1313;
66 |
67 | typedef Eigen::Matrix<double,8,10> Mat810;
68 | typedef Eigen::Matrix<double,8,3> Mat83;
69 | typedef Eigen::Matrix<double,6,6> Mat66;
70 | typedef Eigen::Matrix<double,5,3> Mat53;
71 | typedef Eigen::Matrix<double,4,3> Mat43;
72 | typedef Eigen::Matrix<double,4,2> Mat42;
73 | typedef Eigen::Matrix<double,3,3> Mat33;
74 | typedef Eigen::Matrix<double,2,2> Mat22;
75 | typedef Eigen::Matrix<double,8,CPARS> Mat8C;
76 | typedef Eigen::Matrix<double,CPARS,8> MatC8;
77 | typedef Eigen::Matrix<float,8,CPARS> Mat8Cf;
78 | typedef Eigen::Matrix<float,CPARS,8> MatC8f;
79 |
80 | typedef Eigen::Matrix<double,8,8> Mat88;
81 | typedef Eigen::Matrix<double,7,7> Mat77;
82 |
83 | typedef Eigen::Matrix<double,CPARS,1> VecC;
84 | typedef Eigen::Matrix<float,CPARS,1> VecCf;
85 | typedef Eigen::Matrix<double,13,1> Vec13;
86 | typedef Eigen::Matrix<double,10,1> Vec10;
87 | typedef Eigen::Matrix<double,9,1> Vec9;
88 | typedef Eigen::Matrix<double,8,1> Vec8;
89 | typedef Eigen::Matrix<double,7,1> Vec7;
90 | typedef Eigen::Matrix<double,6,1> Vec6;
91 | typedef Eigen::Matrix<double,5,1> Vec5;
92 | typedef Eigen::Matrix<double,4,1> Vec4;
93 | typedef Eigen::Matrix<double,3,1> Vec3;
94 | typedef Eigen::Matrix<double,2,1> Vec2;
95 | typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;
96 |
97 | typedef Eigen::Matrix<float,3,3> Mat33f;
98 | typedef Eigen::Matrix<float,10,3> Mat103f;
99 | typedef Eigen::Matrix<float,2,2> Mat22f;
100 | typedef Eigen::Matrix<float,3,1> Vec3f;
101 | typedef Eigen::Matrix<float,2,1> Vec2f;
102 | typedef Eigen::Matrix<float,6,1> Vec6f;
103 |
104 |
105 |
106 | typedef Eigen::Matrix<double,4,9> Mat49;
107 | typedef Eigen::Matrix<double,8,9> Mat89;
108 |
109 | typedef Eigen::Matrix<double,9,4> Mat94;
110 | typedef Eigen::Matrix<double,9,8> Mat98;
111 |
112 | typedef Eigen::Matrix<double,8,1> Mat81;
113 | typedef Eigen::Matrix<double,1,8> Mat18;
114 | typedef Eigen::Matrix<double,9,1> Mat91;
115 | typedef Eigen::Matrix<double,1,9> Mat19;
116 |
117 |
118 | typedef Eigen::Matrix<double,8,4> Mat84;
119 | typedef Eigen::Matrix<double,4,8> Mat48;
120 | typedef Eigen::Matrix<double,4,4> Mat44;
121 |
122 |
123 | typedef Eigen::Matrix<float,MAX_RES_PER_POINT,1> VecNRf;
124 | typedef Eigen::Matrix<float,12,1> Vec12f;
125 | typedef Eigen::Matrix<float,1,8> Mat18f;
126 | typedef Eigen::Matrix<float,6,6> Mat66f;
127 | typedef Eigen::Matrix<float,8,8> Mat88f;
128 | typedef Eigen::Matrix<float,8,4> Mat84f;
129 | typedef Eigen::Matrix<float,8,1> Vec8f;
130 | typedef Eigen::Matrix<float,10,1> Vec10f;
131 | typedef Eigen::Matrix<float,6,6> Mat66f;
132 | typedef Eigen::Matrix<float,4,1> Vec4f;
133 | typedef Eigen::Matrix<float,4,4> Mat44f;
134 | typedef Eigen::Matrix<float,12,12> Mat1212f;
135 | typedef Eigen::Matrix<float,12,1> Vec12f;
136 | typedef Eigen::Matrix<float,13,13> Mat1313f;
137 | typedef Eigen::Matrix<float,10,10> Mat1010f;
138 | typedef Eigen::Matrix<float,13,1> Vec13f;
139 | typedef Eigen::Matrix<float,9,9> Mat99f;
140 | typedef Eigen::Matrix<float,9,1> Vec9f;
141 |
142 | typedef Eigen::Matrix<float,4,2> Mat42f;
143 | typedef Eigen::Matrix<float,6,2> Mat62f;
144 | typedef Eigen::Matrix<float,1,2> Mat12f;
145 |
146 | typedef Eigen::Matrix<float,Eigen::Dynamic,1> VecXf;
147 | typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> MatXXf;
148 |
149 |
150 | typedef Eigen::Matrix<double,8+CPARS+1,8+CPARS+1> MatPCPC;
151 | typedef Eigen::Matrix<float,8+CPARS+1,8+CPARS+1> MatPCPCf;
152 | typedef Eigen::Matrix<double,8+CPARS+1,1> VecPC;
153 | typedef Eigen::Matrix<float,8+CPARS+1,1> VecPCf;
154 |
155 | typedef Eigen::Matrix<float,14,14> Mat1414f;
156 | typedef Eigen::Matrix<float,14,1> Vec14f;
157 | typedef Eigen::Matrix<double,14,14> Mat1414;
158 | typedef Eigen::Matrix<double,14,1> Vec14;
159 |
160 |
161 |
162 |
163 |
164 |
165 | // transforms points from one frame to another.
166 | struct AffLight
167 | {
168 | AffLight(double a_, double b_) : a(a_), b(b_) {};
169 | AffLight() : a(0), b(0) {};
170 |
171 | // Affine Parameters:
172 | double a,b; // I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b).
173 |
174 | static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T)
175 | {
176 | if(exposureF==0 || exposureT==0)
177 | {
178 | exposureT = exposureF = 1;
179 | //printf("got exposure value of 0! please choose the correct model.\n");
180 | //assert(setting_brightnessTransferFunc < 2);
181 | }
182 |
183 | double a = exp(g2T.a-g2F.a) * exposureT / exposureF;
184 | double b = g2T.b - a*g2F.b;
185 | return Vec2(a,b);
186 | }
187 |
188 | Vec2 vec()
189 | {
190 | return Vec2(a,b);
191 | }
192 | };
193 |
194 | }
195 |
196 |
--------------------------------------------------------------------------------
/src/util/Undistort.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 | #pragma once
26 |
27 | #include "util/ImageAndExposure.h"
28 | #include "util/MinimalImage.h"
29 | #include "util/NumType.h"
30 | #include "Eigen/Core"
31 |
32 |
33 |
34 |
35 |
36 | namespace dso
37 | {
38 |
39 |
40 | class PhotometricUndistorter
41 | {
42 | public:
43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
44 | PhotometricUndistorter(std::string file, std::string noiseImage, std::string vignetteImage, int w_, int h_);
45 | ~PhotometricUndistorter();
46 |
47 | // removes readout noise, and converts to irradiance.
48 | // affine normalizes values to 0 <= I < 256.
49 | // raw irradiance = a*I + b.
50 | // output will be written in [output].
51 | template<typename T> void processFrame(T* image_in, float exposure_time, float factor=1);
52 | void unMapFloatImage(float* image);
53 |
54 | ImageAndExposure* output;
55 |
56 | float* getG() {if(!valid) return 0; else return G;};
57 | private:
58 | float G[256*256];
59 | int GDepth;
60 | float* vignetteMap;
61 | float* vignetteMapInv;
62 | int w,h;
63 | bool valid;
64 | };
65 |
66 |
67 | class Undistort
68 | {
69 | public:
70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
71 | virtual ~Undistort();
72 |
73 | virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0;
74 |
75 |
76 | inline const Mat33 getK() const {return K;};
77 | inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);};
78 | inline const VecX getOriginalParameter() const {return parsOrg;};
79 | inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);};
80 | inline bool isValid() {return valid;};
81 |
82 | template<typename T>
83 | ImageAndExposure* undistort(const MinimalImage<T>* image_raw, float exposure=0, double timestamp=0, float factor=1) const;
84 | static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename);
85 |
86 | void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage);
87 |
88 | PhotometricUndistorter* photometricUndist;
89 |
90 | protected:
91 | int w, h, wOrg, hOrg, wUp, hUp;
92 | int upsampleUndistFactor;
93 | Mat33 K;
94 | VecX parsOrg;
95 | bool valid;
96 | bool passthrough;
97 |
98 | float* remapX;
99 | float* remapY;
100 |
101 | void applyBlurNoise(float* img) const;
102 |
103 | void makeOptimalK_crop();
104 | void makeOptimalK_full();
105 |
106 | void readFromFile(const char* configFileName, int nPars, std::string prefix = "");
107 | };
108 |
109 | class UndistortFOV : public Undistort
110 | {
111 | public:
112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
113 |
114 | UndistortFOV(const char* configFileName, bool noprefix);
115 | ~UndistortFOV();
116 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
117 |
118 | };
119 |
120 | class UndistortRadTan : public Undistort
121 | {
122 | public:
123 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
124 | UndistortRadTan(const char* configFileName, bool noprefix);
125 | ~UndistortRadTan();
126 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
127 |
128 | };
129 |
130 | class UndistortEquidistant : public Undistort
131 | {
132 | public:
133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
134 | UndistortEquidistant(const char* configFileName, bool noprefix);
135 | ~UndistortEquidistant();
136 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
137 |
138 | };
139 |
140 | class UndistortPinhole : public Undistort
141 | {
142 | public:
143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
144 | UndistortPinhole(const char* configFileName, bool noprefix);
145 | ~UndistortPinhole();
146 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
147 |
148 | private:
149 | float inputCalibration[8];
150 | };
151 |
152 | class UndistortKB : public Undistort
153 | {
154 | public:
155 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
156 | UndistortKB(const char* configFileName, bool noprefix);
157 | ~UndistortKB();
158 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const;
159 |
160 | };
161 |
162 | }
163 |
164 |
--------------------------------------------------------------------------------
/src/util/globalCalib.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #include "util/globalCalib.h"
27 | #include "stdio.h"
28 | #include <iostream>
29 |
30 | namespace dso
31 | {
32 | int wG[PYR_LEVELS], hG[PYR_LEVELS];
33 | float fxG[PYR_LEVELS], fyG[PYR_LEVELS],
34 | cxG[PYR_LEVELS], cyG[PYR_LEVELS];
35 |
36 | float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS],
37 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS];
38 |
39 | Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS];
40 |
41 |
42 | float wM3G;
43 | float hM3G;
44 |
45 | void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K)
46 | {
47 | int wlvl=w;
48 | int hlvl=h;
49 | pyrLevelsUsed=1;
50 | while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS)
51 | {
52 | wlvl /=2;
53 | hlvl /=2;
54 | pyrLevelsUsed++;
55 | }
56 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n",
57 | pyrLevelsUsed-1, wlvl, hlvl);
58 | if(wlvl>100 && hlvl > 100)
59 | {
60 | printf("\n\n===============WARNING!===================\n "
61 | "using not enough pyramid levels.\n"
62 | "Consider scaling to a resolution that is a multiple of a power of 2.\n");
63 | }
64 | if(pyrLevelsUsed < 3)
65 | {
66 | printf("\n\n===============WARNING!===================\n "
67 | "I need higher resolution.\n"
68 | "I will probably segfault.\n");
69 | }
70 |
71 | wM3G = w-3;
72 | hM3G = h-3;
73 |
74 | wG[0] = w;
75 | hG[0] = h;
76 | KG[0] = K;
77 | fxG[0] = K(0,0);
78 | fyG[0] = K(1,1);
79 | cxG[0] = K(0,2);
80 | cyG[0] = K(1,2);
81 | KiG[0] = KG[0].inverse();
82 | fxiG[0] = KiG[0](0,0);
83 | fyiG[0] = KiG[0](1,1);
84 | cxiG[0] = KiG[0](0,2);
85 | cyiG[0] = KiG[0](1,2);
86 |
87 | for (int level = 1; level < pyrLevelsUsed; ++ level)
88 | {
89 | wG[level] = w >> level;
90 | hG[level] = h >> level;
91 |
92 | fxG[level] = fxG[level-1] * 0.5;
93 | fyG[level] = fyG[level-1] * 0.5;
94 | cxG[level] = (cxG[0] + 0.5) / ((int)1<<level) - 0.5;
95 | cyG[level] = (cyG[0] + 0.5) / ((int)1<<level) - 0.5;
96 |
97 | KG[level] << fxG[level], 0.0, cxG[level], 0.0, fyG[level], cyG[level], 0.0, 0.0, 1.0; // synthetic
98 | KiG[level] = KG[level].inverse();
99 |
100 | fxiG[level] = KiG[level](0,0);
101 | fyiG[level] = KiG[level](1,1);
102 | cxiG[level] = KiG[level](0,2);
103 | cyiG[level] = KiG[level](1,2);
104 | }
105 | }
106 |
107 |
108 | }
109 |
--------------------------------------------------------------------------------
/src/util/globalCalib.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #pragma once
27 | #include "util/settings.h"
28 | #include "util/NumType.h"
29 |
30 | namespace dso
31 | {
32 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS];
33 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS],
34 | cxG[PYR_LEVELS], cyG[PYR_LEVELS];
35 |
36 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS],
37 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS];
38 |
39 | extern Eigen::Matrix3f KG[PYR_LEVELS],KiG[PYR_LEVELS];
40 |
41 | extern float wM3G;
42 | extern float hM3G;
43 |
44 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K );
45 | }
46 |
--------------------------------------------------------------------------------
/src/util/settings.h:
--------------------------------------------------------------------------------
1 | /**
2 | * This file is part of DSO.
3 | *
4 | * Copyright 2016 Technical University of Munich and Intel.
5 | * Developed by Jakob Engel <engelj at in dot tum dot de>,
6 | * for more information see <http://vision.in.tum.de/dso>.
7 | * If you use this code, please cite the respective publications as
8 | * listed on the above website.
9 | *
10 | * DSO is free software: you can redistribute it and/or modify
11 | * it under the terms of the GNU General Public License as published by
12 | * the Free Software Foundation, either version 3 of the License, or
13 | * (at your option) any later version.
14 | *
15 | * DSO is distributed in the hope that it will be useful,
16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 | * GNU General Public License for more details.
19 | *
20 | * You should have received a copy of the GNU General Public License
21 | * along with DSO. If not, see <http://www.gnu.org/licenses/>.
22 | */
23 |
24 |
25 |
26 | #pragma once
27 |
28 | #include <string.h>
29 | #include <string>
30 | #include <cmath>
31 |
32 |
33 | namespace dso
34 | {
35 | #define SOLVER_SVD (int)1
36 | #define SOLVER_ORTHOGONALIZE_SYSTEM (int)2
37 | #define SOLVER_ORTHOGONALIZE_POINTMARG (int)4
38 | #define SOLVER_ORTHOGONALIZE_FULL (int)8
39 | #define SOLVER_SVD_CUT7 (int)16
40 | #define SOLVER_REMOVE_POSEPRIOR (int)32
41 | #define SOLVER_USE_GN (int)64
42 | #define SOLVER_FIX_LAMBDA (int)128
43 | #define SOLVER_ORTHOGONALIZE_X (int)256
44 | #define SOLVER_MOMENTUM (int)512
45 | #define SOLVER_STEPMOMENTUM (int)1024
46 | #define SOLVER_ORTHOGONALIZE_X_LATER (int)2048
47 |
48 |
49 | // ============== PARAMETERS TO BE DECIDED ON COMPILE TIME =================
50 | #define PYR_LEVELS 6
51 | extern int pyrLevelsUsed;
52 |
53 |
54 |
55 | extern float setting_keyframesPerSecond;
56 | extern bool setting_realTimeMaxKF;
57 | extern float setting_maxShiftWeightT;
58 | extern float setting_maxShiftWeightR;
59 | extern float setting_maxShiftWeightRT;
60 | extern float setting_maxAffineWeight;
61 | extern float setting_kfGlobalWeight;
62 |
63 |
64 |
65 | extern float setting_idepthFixPrior;
66 | extern float setting_idepthFixPriorMargFac;
67 | extern float setting_initialRotPrior;
68 | extern float setting_initialTransPrior;
69 | extern float setting_initialAffBPrior;
70 | extern float setting_initialAffAPrior;
71 | extern float setting_initialCalibHessian;
72 |
73 | extern int setting_solverMode;
74 | extern double setting_solverModeDelta;
75 |
76 |
77 | extern float setting_minIdepthH_act;
78 | extern float setting_minIdepthH_marg;
79 |
80 |
81 |
82 | extern float setting_maxIdepth;
83 | extern float setting_maxPixSearch;
84 | extern float setting_desiredImmatureDensity; // done
85 | extern float setting_desiredPointDensity; // done
86 | extern float setting_minPointsRemaining;
87 | extern float setting_maxLogAffFacInWindow;
88 | extern int setting_minFrames;
89 | extern int setting_maxFrames;
90 | extern int setting_minFrameAge;
91 | extern int setting_maxOptIterations;
92 | extern int setting_minOptIterations;
93 | extern float setting_thOptIterations;
94 | extern float setting_outlierTH;
95 | extern float setting_outlierTHSumComponent;
96 |
97 |
98 |
99 | extern int setting_pattern;
100 | extern float setting_margWeightFac;
101 | extern int setting_GNItsOnPointActivation;
102 |
103 |
104 | extern float setting_minTraceQuality;
105 | extern int setting_minTraceTestRadius;
106 | extern float setting_reTrackThreshold;
107 |
108 |
109 | extern int setting_minGoodActiveResForMarg;
110 | extern int setting_minGoodResForMarg;
111 | extern int setting_minInlierVotesForMarg;
112 |
113 |
114 |
115 |
116 | extern int setting_photometricCalibration;
117 | extern bool setting_useExposure;
118 | extern float setting_affineOptModeA;
119 | extern float setting_affineOptModeB;
120 | extern int setting_gammaWeightsPixelSelect;
121 |
122 |
123 |
124 | extern bool setting_forceAceptStep;
125 |
126 |
127 |
128 | extern float setting_huberTH;
129 |
130 |
131 | extern bool setting_logStuff;
132 | extern float benchmarkSetting_fxfyfac;
133 | extern int benchmarkSetting_width;
134 | extern int benchmarkSetting_height;
135 | extern float benchmark_varNoise;
136 | extern float benchmark_varBlurNoise;
137 | extern int benchmark_noiseGridsize;
138 | extern float benchmark_initializerSlackFactor;
139 |
140 | extern float setting_frameEnergyTHConstWeight;
141 | extern float setting_frameEnergyTHN;
142 |
143 | extern float setting_frameEnergyTHFacMedian;
144 | extern float setting_overallEnergyTHWeight;
145 | extern float setting_coarseCutoffTH;
146 |
147 | extern float setting_minGradHistCut;
148 | extern float setting_minGradHistAdd;
149 | extern float setting_gradDownweightPerLevel;
150 | extern bool setting_selectDirectionDistribution;
151 |
152 |
153 |
154 | extern float setting_trace_stepsize;
155 | extern int setting_trace_GNIterations;
156 | extern float setting_trace_GNThreshold;
157 | extern float setting_trace_extraSlackOnTH;
158 | extern float setting_trace_slackInterval;
159 | extern float setting_trace_minImprovementFactor;
160 |
161 |
162 | extern bool setting_render_displayCoarseTrackingFull;
163 | extern bool setting_render_renderWindowFrames;
164 | extern bool setting_render_plotTrackingFull;
165 | extern bool setting_render_display3D;
166 | extern bool setting_render_displayResidual;
167 | extern bool setting_render_displayVideo;
168 | extern bool setting_render_displayDepth;
169 |
170 | extern bool setting_fullResetRequested;
171 |
172 | extern bool setting_debugout_runquiet;
173 |
174 | extern bool disableAllDisplay;
175 | extern bool disableReconfigure;
176 |
177 |
178 | extern bool setting_onlyLogKFPoses;
179 |
180 |
181 |
182 |
183 | extern bool debugSaveImages;
184 |
185 |
186 | extern int sparsityFactor;
187 | extern bool goStepByStep;
188 | extern bool plotStereoImages;
189 | extern bool multiThreading;
190 |
191 | extern float freeDebugParam1;
192 | extern float freeDebugParam2;
193 | extern float freeDebugParam3;
194 | extern float freeDebugParam4;
195 | extern float freeDebugParam5;
196 |
197 |
198 | void handleKey(char k);
199 |
200 |
201 |
202 |
203 | extern int staticPattern[10][40][2];
204 | extern int staticPatternNum[10];
205 | extern int staticPatternPadding[10];
206 |
207 |
208 |
209 |
210 | //#define patternNum staticPatternNum[setting_pattern]
211 | //#define patternP staticPattern[setting_pattern]
212 | //#define patternPadding staticPatternPadding[setting_pattern]
213 |
214 | //
215 | #define patternNum 8
216 | #define patternP staticPattern[8]
217 | #define patternPadding 2
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 | }
232 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | SET(PROJECT_NAME Sophus)
2 |
3 | PROJECT(${PROJECT_NAME})
4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
5 |
6 | SET (CMAKE_VERBOSE_MAKEFILE ON)
7 |
8 | # Release by default
9 | # Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug"
10 | IF( NOT CMAKE_BUILD_TYPE )
11 | SET( CMAKE_BUILD_TYPE Release )
12 | ENDIF()
13 |
14 | IF (CMAKE_COMPILER_IS_GNUCXX )
15 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
16 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ")
17 |
18 | ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable
19 | -Wno-unused-but-set-variable -Wno-unknown-pragmas ")
20 | ENDIF()
21 |
22 | ################################################################################
23 | # Add local path for finding packages, set the local version first
24 | set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" )
25 | list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" )
26 |
27 | ################################################################################
28 | # Create variables used for exporting in SophusConfig.cmake
29 | set( Sophus_LIBRARIES "" )
30 | set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} )
31 |
32 | ################################################################################
33 |
34 | include(FindEigen3.cmake)
35 | #find_package( Eigen3 REQUIRED )
36 | INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} )
37 | SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} )
38 |
39 | SET (SOURCE_DIR "sophus")
40 |
41 | SET (TEMPLATES tests
42 | so2
43 | se2
44 | so3
45 | se3
46 | rxso3
47 | sim3
48 | )
49 |
50 | SET (SOURCES ${SOURCE_DIR}/sophus.hpp)
51 |
52 | FOREACH(templ ${TEMPLATES})
53 | LIST(APPEND SOURCES ${SOURCE_DIR}/${templ}.hpp)
54 | ENDFOREACH(templ)
55 |
56 |
57 | INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
58 |
59 | # Added ${SOURCES} to executables so they show up in QtCreator (and possibly
60 | # other IDEs).
61 | # ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp ${SOURCES})
62 | # ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp ${SOURCES})
63 | # ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp ${SOURCES})
64 | # ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp ${SOURCES})
65 | # ADD_EXECUTABLE(test_rxso3 sophus/test_rxso3.cpp ${SOURCES})
66 | # ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp ${SOURCES})
67 | # ENABLE_TESTING()
68 | #
69 | # ADD_TEST(test_so2 test_so2)
70 | # ADD_TEST(test_se2 test_se2)
71 | # ADD_TEST(test_so3 test_so3)
72 | # ADD_TEST(test_se3 test_se3)
73 | # ADD_TEST(test_rxso3 test_rxso3)
74 | # ADD_TEST(test_sim3 test_sim3)
75 |
76 | ################################################################################
77 | # Create the SophusConfig.cmake file for other cmake projects.
78 | CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in
79 | ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE )
80 | export( PACKAGE Sophus )
81 |
82 | INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include
83 | FILES_MATCHING PATTERN "*.hpp" )
--------------------------------------------------------------------------------
/thirdparty/Sophus/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, <montel@kde.org>
14 | # Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
15 | # Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
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 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
65 | PATHS
66 | ${CMAKE_INSTALL_PREFIX}/include
67 | ${KDE4_INCLUDE_DIR}
68 | PATH_SUFFIXES eigen3 eigen
69 | )
70 |
71 | if(EIGEN3_INCLUDE_DIR)
72 | _eigen3_check_version()
73 | endif(EIGEN3_INCLUDE_DIR)
74 |
75 | include(FindPackageHandleStandardArgs)
76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
77 |
78 | mark_as_advanced(EIGEN3_INCLUDE_DIR)
79 |
80 | endif(EIGEN3_INCLUDE_DIR)
81 |
82 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/README:
--------------------------------------------------------------------------------
1 | Sophus (version 0.9a)
2 |
3 | C++ implementation of Lie Groups using Eigen.
4 |
5 | Thanks to Steven Lovegrove, Sophus is now fully templated - using the Curiously Recurring Template Pattern (CRTP).
6 |
7 | (In order to go back to the non-templated/double-only version "git checkout a621ff".)
8 |
9 | Installation guide:
10 |
11 | >>>
12 | cd Sophus
13 | mkdir build
14 | cd build
15 | cmake ..
16 | make
17 | <<<
18 |
19 |
20 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/SophusConfig.cmake.in:
--------------------------------------------------------------------------------
1 | ################################################################################
2 | # Sophus source dir
3 | set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@")
4 |
5 | ################################################################################
6 | # Sophus build dir
7 | set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@")
8 |
9 | ################################################################################
10 | set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" )
11 | set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" )
--------------------------------------------------------------------------------
/thirdparty/Sophus/cmake_modules/FindEigen3.cmake:
--------------------------------------------------------------------------------
1 | # - Try to find Eigen3 lib
2 | # Once done this will define
3 | #
4 | # EIGEN3_FOUND - system has eigen lib
5 | # EIGEN3_INCLUDE_DIR - the eigen include directory
6 |
7 | # Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
8 | # Redistribution and use is allowed according to the terms of the BSD license.
9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file.
10 |
11 | if( EIGEN3_INCLUDE_DIR )
12 | # in cache already
13 | set( EIGEN3_FOUND TRUE )
14 | else (EIGEN3_INCLUDE_DIR)
15 | find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core
16 | PATH_SUFFIXES eigen3/
17 | HINTS
18 | ${INCLUDE_INSTALL_DIR}
19 | /usr/local/include
20 | ${KDE4_INCLUDE_DIR}
21 | )
22 | include( FindPackageHandleStandardArgs )
23 | find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR )
24 | mark_as_advanced( EIGEN3_INCLUDE_DIR )
25 | endif(EIGEN3_INCLUDE_DIR)
26 |
27 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/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 <stdexcept>
27 |
28 | // fix log1p not being found on Android in Eigen
29 | #if defined( ANDROID )
30 | #include <cmath>
31 | namespace std {
32 | using ::log1p;
33 | }
34 | #endif
35 |
36 | #include <Eigen/Eigen>
37 | #include <Eigen/Geometry>
38 |
39 | namespace Sophus {
40 | using namespace Eigen;
41 |
42 | template<typename Scalar>
43 | struct SophusConstants {
44 | EIGEN_ALWAYS_INLINE static
45 | const Scalar epsilon() {
46 | return static_cast<Scalar>(1e-10);
47 | }
48 |
49 | EIGEN_ALWAYS_INLINE static
50 | const Scalar pi() {
51 | return static_cast<Scalar>(M_PI);
52 | }
53 | };
54 |
55 | template<>
56 | struct SophusConstants<float> {
57 | EIGEN_ALWAYS_INLINE static
58 | float epsilon() {
59 | return static_cast<float>(1e-5);
60 | }
61 |
62 | EIGEN_ALWAYS_INLINE static
63 | float pi() {
64 | return static_cast<float>(M_PI);
65 | }
66 | };
67 |
68 | class SophusException : public std::runtime_error {
69 | public:
70 | SophusException (const std::string& str)
71 | : runtime_error("Sophus exception: " + str) {
72 | }
73 | };
74 |
75 | }
76 |
77 | #endif
78 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/test_rxso3.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of Sophus.
2 | //
3 | // Copyright 2012-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 | #include <iostream>
24 | #include <vector>
25 |
26 |
27 | #include "rxso3.hpp"
28 | #include "tests.hpp"
29 |
30 | using namespace Sophus;
31 | using namespace std;
32 |
33 | template<class Scalar>
34 | void tests() {
35 |
36 | typedef RxSO3Group<Scalar> RxSO3Type;
37 | typedef typename RxSO3Group<Scalar>::Point Point;
38 | typedef typename RxSO3Group<Scalar>::Tangent Tangent;
39 |
40 | vector<RxSO3Type> rxso3_vec;
41 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.)));
42 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1)));
43 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1)));
44 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.)));
45 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001)));
46 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0)));
47 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9)));
48 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0))
49 | *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0))
50 | *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0)));
51 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0))
52 | *RxSO3Type::exp(Tangent(M_PI, 0, 0,0))
53 | *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0)));
54 |
55 | vector<Tangent> tangent_vec;
56 | Tangent tmp;
57 | tmp << 0,0,0,0;
58 | tangent_vec.push_back(tmp);
59 | tmp << 1,0,0,0;
60 | tangent_vec.push_back(tmp);
61 | tmp << 1,0,0,0.1;
62 | tangent_vec.push_back(tmp);
63 | tmp << 0,1,0,0.1;
64 | tangent_vec.push_back(tmp);
65 | tmp << 0,0,1,-0.1;
66 | tangent_vec.push_back(tmp);
67 | tmp << -1,1,0,-0.1;
68 | tangent_vec.push_back(tmp);
69 | tmp << 20,-1,0,2;
70 | tangent_vec.push_back(tmp);
71 |
72 | vector<Point> point_vec;
73 | point_vec.push_back(Point(1,2,4));
74 |
75 | Tests<RxSO3Type> tests;
76 | tests.setGroupElements(rxso3_vec);
77 | tests.setTangentVectors(tangent_vec);
78 | tests.setPoints(point_vec);
79 |
80 | tests.runAllTests();
81 | }
82 |
83 | int main() {
84 | cerr << "Test RxSO3" << endl << endl;
85 |
86 | cerr << "Double tests: " << endl;
87 | tests<double>();
88 |
89 | cerr << "Float tests: " << endl;
90 | tests<float>();
91 | return 0;
92 | }
93 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/test_se2.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of Sophus.
2 | //
3 | // Copyright 2012-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 | #include <iostream>
24 | #include <vector>
25 |
26 | #include <unsupported/Eigen/MatrixFunctions>
27 | #include "se2.hpp"
28 | #include "tests.hpp"
29 |
30 | using namespace Sophus;
31 | using namespace std;
32 |
33 | template<class Scalar>
34 | void tests() {
35 |
36 | typedef SO2Group<Scalar> SO2Type;
37 | typedef SE2Group<Scalar> SE2Type;
38 | typedef typename SE2Group<Scalar>::Point Point;
39 | typedef typename SE2Group<Scalar>::Tangent Tangent;
40 |
41 | vector<SE2Type> se2_vec;
42 | se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0)));
43 | se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0)));
44 | se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100)));
45 | se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1)));
46 | se2_vec.push_back(SE2Type(SO2Type(0.00001),
47 | Point(-0.00000001,0.0000000001)));
48 | se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0))
49 | *SE2Type(SO2Type(M_PI),Point(0,0))
50 | *SE2Type(SO2Type(-0.2),Point(0,0)));
51 | se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0))
52 | *SE2Type(SO2Type(M_PI),Point(0,0))
53 | *SE2Type(SO2Type(-0.3),Point(0,6)));
54 |
55 | vector<Tangent> tangent_vec;
56 | Tangent tmp;
57 | tmp << 0,0,0;
58 | tangent_vec.push_back(tmp);
59 | tmp << 1,0,0;
60 | tangent_vec.push_back(tmp);
61 | tmp << 0,1,1;
62 | tangent_vec.push_back(tmp);
63 | tmp << -1,1,0;
64 | tangent_vec.push_back(tmp);
65 | tmp << 20,-1,-1;
66 | tangent_vec.push_back(tmp);
67 | tmp << 30,5,20;
68 | tangent_vec.push_back(tmp);
69 |
70 | vector<Point> point_vec;
71 | point_vec.push_back(Point(1,2));
72 |
73 | Tests<SE2Type> tests;
74 | tests.setGroupElements(se2_vec);
75 | tests.setTangentVectors(tangent_vec);
76 | tests.setPoints(point_vec);
77 |
78 | tests.runAllTests();
79 | }
80 |
81 | int main() {
82 | cerr << "Test SE2" << endl << endl;
83 |
84 | cerr << "Double tests: " << endl;
85 | tests<double>();
86 |
87 | cerr << "Float tests: " << endl;
88 | tests<float>();
89 | return 0;
90 | }
91 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/test_se3.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of Sophus.
2 | //
3 | // Copyright 2012-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 | #include <iostream>
24 | #include <vector>
25 |
26 | #include "se3.hpp"
27 | #include "tests.hpp"
28 |
29 | using namespace Sophus;
30 | using namespace std;
31 |
32 | template<class Scalar>
33 | void tests() {
34 |
35 | typedef SO3Group<Scalar> SO3Type;
36 | typedef SE3Group<Scalar> SE3Type;
37 | typedef typename SE3Group<Scalar>::Point Point;
38 | typedef typename SE3Group<Scalar>::Tangent Tangent;
39 |
40 | vector<SE3Type> se3_vec;
41 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)),
42 | Point(0,0,0)));
43 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)),
44 | Point(10,0,0)));
45 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)),
46 | Point(0,100,5)));
47 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
48 | Point(0,0,0)));
49 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
50 | Point(0,-0.00000001,0.0000000001)));
51 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
52 | Point(0.01,0,0)));
53 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(M_PI, 0, 0)),
54 | Point(4,-5,0)));
55 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)),
56 | Point(0,0,0))
57 | *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)),
58 | Point(0,0,0))
59 | *SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)),
60 | Point(0,0,0)));
61 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)),
62 | Point(2,0,-7))
63 | *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)),
64 | Point(0,0,0))
65 | *SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)),
66 | Point(0,6,0)));
67 | vector<Tangent> tangent_vec;
68 | Tangent tmp;
69 | tmp << 0,0,0,0,0,0;
70 | tangent_vec.push_back(tmp);
71 | tmp << 1,0,0,0,0,0;
72 | tangent_vec.push_back(tmp);
73 | tmp << 0,1,0,1,0,0;
74 | tangent_vec.push_back(tmp);
75 | tmp << 0,-5,10,0,0,0;
76 | tangent_vec.push_back(tmp);
77 | tmp << -1,1,0,0,0,1;
78 | tangent_vec.push_back(tmp);
79 | tmp << 20,-1,0,-1,1,0;
80 | tangent_vec.push_back(tmp);
81 | tmp << 30,5,-1,20,-1,0;
82 | tangent_vec.push_back(tmp);
83 |
84 | vector<Point> point_vec;
85 | point_vec.push_back(Point(1,2,4));
86 |
87 | Tests<SE3Type> tests;
88 | tests.setGroupElements(se3_vec);
89 | tests.setTangentVectors(tangent_vec);
90 | tests.setPoints(point_vec);
91 |
92 | tests.runAllTests();
93 | cerr << "passed." << endl << endl;
94 | }
95 |
96 | int main() {
97 | cerr << "Test SE3" << endl << endl;
98 |
99 | cerr << "Double tests: " << endl;
100 | tests<double>();
101 |
102 | cerr << "Float tests: " << endl;
103 | tests<float>();
104 | return 0;
105 | }
106 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/test_sim3.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of Sophus.
2 | //
3 | // Copyright 2012-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 | #include <iostream>
24 | #include <vector>
25 |
26 | #include <unsupported/Eigen/MatrixFunctions>
27 |
28 | #include "sim3.hpp"
29 | #include "tests.hpp"
30 |
31 | using namespace Sophus;
32 | using namespace std;
33 |
34 | template<class Scalar>
35 | void tests() {
36 |
37 | typedef Sim3Group<Scalar> Sim3Type;
38 | typedef RxSO3Group<Scalar> RxSO3Type;
39 | typedef typename Sim3Group<Scalar>::Point Point;
40 | typedef typename Sim3Group<Scalar>::Tangent Tangent;
41 | typedef Matrix<Scalar,4,1> Vector4Type;
42 |
43 | vector<Sim3Type> sim3_vec;
44 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,1.)),
45 | Point(0,0,0)));
46 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, -1.0,1.1)),
47 | Point(10,0,0)));
48 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.,1.1)),
49 | Point(0,10,5)));
50 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0.)),
51 | Point(0,0,0)));
52 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(
53 | Vector4Type(0., 0., 0.00001, 0.0000001)),
54 | Point(1,-1.00000001,2.0000000001)));
55 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0)),
56 | Point(0.01,0,0)));
57 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0.9)),
58 | Point(4,-5,0)));
59 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,0)),
60 | Point(0,0,0))
61 | *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)),
62 | Point(0,0,0))
63 | *Sim3Type(RxSO3Type::exp(Vector4Type(-0.2, -0.5, -0.0,0)),
64 | Point(0,0,0)));
65 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.3, 0.5, 0.1,0)),
66 | Point(2,0,-7))
67 | *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)),
68 | Point(0,0,0))
69 | *Sim3Type(RxSO3Type::exp(Vector4Type(-0.3, -0.5, -0.1,0)),
70 | Point(0,6,0)));
71 | vector<Tangent> tangent_vec;
72 | Tangent tmp;
73 | tmp << 0,0,0,0,0,0,0;
74 | tangent_vec.push_back(tmp);
75 | tmp << 1,0,0,0,0,0,0;
76 | tangent_vec.push_back(tmp);
77 | tmp << 0,1,0,1,0,0,0.1;
78 | tangent_vec.push_back(tmp);
79 | tmp << 0,0,1,0,1,0,0.1;
80 | tangent_vec.push_back(tmp);
81 | tmp << -1,1,0,0,0,1,-0.1;
82 | tangent_vec.push_back(tmp);
83 | tmp << 20,-1,0,-1,1,0,-0.1;
84 | tangent_vec.push_back(tmp);
85 | tmp << 30,5,-1,20,-1,0,1.5;
86 | tangent_vec.push_back(tmp);
87 |
88 |
89 | vector<Point> point_vec;
90 | point_vec.push_back(Point(1,2,4));
91 |
92 | Tests<Sim3Type> tests;
93 | tests.setGroupElements(sim3_vec);
94 | tests.setTangentVectors(tangent_vec);
95 | tests.setPoints(point_vec);
96 |
97 | tests.runAllTests();
98 | }
99 |
100 | int main() {
101 | cerr << "Test Sim3" << endl << endl;
102 |
103 | cerr << "Double tests: " << endl;
104 | tests<double>();
105 |
106 | cerr << "Float tests: " << endl;
107 | tests<float>();
108 | return 0;
109 | }
110 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/test_so2.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of Sophus.
2 | //
3 | // Copyright 2012-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 |
24 | #include <iostream>
25 | #include <vector>
26 |
27 | #include "so2.hpp"
28 | #include "tests.hpp"
29 |
30 | using namespace Sophus;
31 | using namespace std;
32 |
33 | template<class Scalar>
34 | void tests() {
35 |
36 | typedef SO2Group<Scalar> SO2Type;
37 | typedef typename SO2Group<Scalar>::Point Point;
38 | typedef typename SO2Group<Scalar>::Tangent Tangent;
39 |
40 | vector<SO2Type> so2_vec;
41 | so2_vec.push_back(SO2Type::exp(0.0));
42 | so2_vec.push_back(SO2Type::exp(0.2));
43 | so2_vec.push_back(SO2Type::exp(10.));
44 | so2_vec.push_back(SO2Type::exp(0.00001));
45 | so2_vec.push_back(SO2Type::exp(M_PI));
46 | so2_vec.push_back(SO2Type::exp(0.2)
47 | *SO2Type::exp(M_PI)
48 | *SO2Type::exp(-0.2));
49 | so2_vec.push_back(SO2Type::exp(-0.3)
50 | *SO2Type::exp(M_PI)
51 | *SO2Type::exp(0.3));
52 |
53 | vector<Tangent> tangent_vec;
54 | tangent_vec.push_back(Tangent(0));
55 | tangent_vec.push_back(Tangent(1));
56 | tangent_vec.push_back(Tangent(M_PI_2));
57 | tangent_vec.push_back(Tangent(-1));
58 | tangent_vec.push_back(Tangent(20));
59 | tangent_vec.push_back(Tangent(M_PI_2+0.0001));
60 |
61 | vector<Point> point_vec;
62 | point_vec.push_back(Point(1,2));
63 |
64 | Tests<SO2Type> tests;
65 | tests.setGroupElements(so2_vec);
66 | tests.setTangentVectors(tangent_vec);
67 | tests.setPoints(point_vec);
68 |
69 | tests.runAllTests();
70 |
71 | cerr << "Exception test: ";
72 | try {
73 | SO2Type so2(0., 0.);
74 | } catch(SophusException & e) {
75 | cerr << "passed." << endl << endl;
76 | return;
77 | }
78 | cerr << "failed!" << endl << endl;
79 | exit(-1);
80 | }
81 |
82 | int main() {
83 | cerr << "Test SO2" << endl << endl;
84 |
85 | cerr << "Double tests: " << endl;
86 | tests<double>();
87 |
88 | cerr << "Float tests: " << endl;
89 | tests<float>();
90 | return 0;
91 | }
92 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/test_so3.cpp:
--------------------------------------------------------------------------------
1 | // This file is part of Sophus.
2 | //
3 | // Copyright 2012-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 | #include <iostream>
24 | #include <vector>
25 |
26 | #include "so3.hpp"
27 | #include "tests.hpp"
28 |
29 | using namespace Sophus;
30 | using namespace std;
31 |
32 | template<class Scalar>
33 | void tests() {
34 |
35 | typedef SO3Group<Scalar> SO3Type;
36 | typedef typename SO3Group<Scalar>::Point Point;
37 | typedef typename SO3Group<Scalar>::Tangent Tangent;
38 |
39 | vector<SO3Type> so3_vec;
40 |
41 | so3_vec.push_back(SO3Type(Quaternion<Scalar>(0.1e-11, 0., 1., 0.)));
42 | so3_vec.push_back(SO3Type(Quaternion<Scalar>(-1,0.00001,0.0,0.0)));
43 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0)));
44 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, -1.0)));
45 | so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.)));
46 | so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.00001)));
47 | so3_vec.push_back(SO3Type::exp(Point(M_PI, 0, 0)));
48 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0))
49 | *SO3Type::exp(Point(M_PI, 0, 0))
50 | *SO3Type::exp(Point(-0.2, -0.5, -0.0)));
51 | so3_vec.push_back(SO3Type::exp(Point(0.3, 0.5, 0.1))
52 | *SO3Type::exp(Point(M_PI, 0, 0))
53 | *SO3Type::exp(Point(-0.3, -0.5, -0.1)));
54 |
55 | vector<Tangent> tangent_vec;
56 | tangent_vec.push_back(Tangent(0,0,0));
57 | tangent_vec.push_back(Tangent(1,0,0));
58 | tangent_vec.push_back(Tangent(0,1,0));
59 | tangent_vec.push_back(Tangent(M_PI_2,M_PI_2,0.0));
60 | tangent_vec.push_back(Tangent(-1,1,0));
61 | tangent_vec.push_back(Tangent(20,-1,0));
62 | tangent_vec.push_back(Tangent(30,5,-1));
63 |
64 | vector<Point> point_vec;
65 | point_vec.push_back(Point(1,2,4));
66 |
67 | Tests<SO3Type> tests;
68 | tests.setGroupElements(so3_vec);
69 | tests.setTangentVectors(tangent_vec);
70 | tests.setPoints(point_vec);
71 |
72 | tests.runAllTests();
73 | }
74 |
75 | int main() {
76 | cerr << "Test SO3" << endl << endl;
77 |
78 | cerr << "Double tests: " << endl;
79 | tests<double>();
80 |
81 | cerr << "Float tests: " << endl;
82 | tests<float>();
83 | return 0;
84 | }
85 |
--------------------------------------------------------------------------------
/thirdparty/Sophus/sophus/tests.hpp:
--------------------------------------------------------------------------------
1 | #ifndef SOPUHS_TESTS_HPP
2 | #define SOPUHS_TESTS_HPP
3 |
4 | #include <vector>
5 | #include <unsupported/Eigen/MatrixFunctions>
6 |
7 | #include "sophus.hpp"
8 |
9 | namespace Sophus {
10 |
11 | using namespace std;
12 | using namespace Eigen;
13 |
14 | template <class LieGroup>
15 | class Tests {
16 |
17 | public:
18 | typedef typename LieGroup::Scalar Scalar;
19 | typedef typename LieGroup::Transformation Transformation;
20 | typedef typename LieGroup::Tangent Tangent;
21 | typedef typename LieGroup::Point Point;
22 | typedef typename LieGroup::Adjoint Adjoint;
23 | static const int N = LieGroup::N;
24 | static const int DoF = LieGroup::DoF;
25 |
26 | const Scalar SMALL_EPS;
27 |
28 | Tests() : SMALL_EPS(SophusConstants<Scalar>::epsilon()) {
29 | }
30 |
31 | void setGroupElements(const vector<LieGroup> & group_vec) {
32 | group_vec_ = group_vec;
33 | }
34 |
35 | void setTangentVectors(const vector<Tangent> & tangent_vec) {
36 | tangent_vec_ = tangent_vec;
37 | }
38 |
39 | void setPoints(const vector<Point> & point_vec) {
40 | point_vec_ = point_vec;
41 | }
42 |
43 | bool adjointTest() {
44 | bool passed = true;
45 | for (size_t i=0; i<group_vec_.size(); ++i) {
46 | Transformation T = group_vec_[i].matrix();
47 | Adjoint Ad = group_vec_[i].Adj();
48 | for (size_t j=0; j<tangent_vec_.size(); ++j) {
49 | Tangent x = tangent_vec_[j];
50 |
51 | Transformation I;
52 | I.setIdentity();
53 | Tangent ad1 = Ad*x;
54 | Tangent ad2 = LieGroup::vee(T*LieGroup::hat(x)
55 | *group_vec_[i].inverse().matrix());
56 | Scalar nrm = norm(ad1-ad2);
57 |
58 | if (isnan(nrm) || nrm>20.*SMALL_EPS) {
59 | cerr << "Adjoint" << endl;
60 | cerr << "Test case: " << i << "," << j <<endl;
61 | cerr << (ad1-ad2) <<endl;
62 | cerr << endl;
63 | passed = false;
64 | }
65 | }
66 | }
67 | return passed;
68 | }
69 |
70 | bool expLogTest() {
71 | bool passed = true;
72 |
73 | for (size_t i=0; i<group_vec_.size(); ++i) {
74 | Transformation T1 = group_vec_[i].matrix();
75 | Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix();
76 | Transformation DiffT = T1-T2;
77 | Scalar nrm = DiffT.norm();
78 |
79 | if (isnan(nrm) || nrm>SMALL_EPS) {
80 | cerr << "G - exp(log(G))" << endl;
81 | cerr << "Test case: " << i << endl;
82 | cerr << DiffT <<endl;
83 | cerr << endl;
84 | passed = false;
85 | }
86 | }
87 | return passed;
88 | }
89 |
90 | bool expMapTest() {
91 | bool passed = true;
92 | for (size_t i=0; i<tangent_vec_.size(); ++i) {
93 |
94 | Tangent omega = tangent_vec_[i];
95 | Transformation exp_x = LieGroup::exp(omega).matrix();
96 | Transformation expmap_hat_x = (LieGroup::hat(omega)).exp();
97 | Transformation DiffR = exp_x-expmap_hat_x;
98 | Scalar nrm = DiffR.norm();
99 |
100 | if (isnan(nrm) || nrm>10.*SMALL_EPS) {
101 | cerr << "expmap(hat(x)) - exp(x)" << endl;
102 | cerr << "Test case: " << i << endl;
103 | cerr << exp_x <<endl;
104 | cerr << expmap_hat_x <<endl;
105 | cerr << DiffR <<endl;
106 | cerr << endl;
107 | passed = false;
108 | }
109 | }
110 | return passed;
111 | }
112 |
113 | bool groupActionTest() {
114 | bool passed = true;
115 |
116 | for (size_t i=0; i<group_vec_.size(); ++i) {
117 | for (size_t j=0; j<point_vec_.size(); ++j) {
118 | const Point & p = point_vec_[j];
119 | Transformation T = group_vec_[i].matrix();
120 | Point res1 = group_vec_[i]*p;
121 | Point res2 = map(T, p);
122 | Scalar nrm = (res1-res2).norm();
123 | if (isnan(nrm) || nrm>SMALL_EPS) {
124 | cerr << "Transform vector" << endl;
125 | cerr << "Test case: " << i << endl;
126 | cerr << (res1-res2) <<endl;
127 | cerr << endl;
128 | passed = false;
129 | }
130 | }
131 | }
132 | return passed;
133 | }
134 |
135 |
136 | bool lieBracketTest() {
137 | bool passed = true;
138 | for (size_t i=0; i<tangent_vec_.size(); ++i) {
139 | for (size_t j=0; j<tangent_vec_.size(); ++j) {
140 | Tangent res1 = LieGroup::lieBracket(tangent_vec_[i],tangent_vec_[j]);
141 | Transformation hati = LieGroup::hat(tangent_vec_[i]);
142 | Transformation hatj = LieGroup::hat(tangent_vec_[j]);
143 |
144 | Tangent res2 = LieGroup::vee(hati*hatj-hatj*hati);
145 | Tangent resDiff = res1-res2;
146 | if (norm(resDiff)>SMALL_EPS) {
147 | cerr << "Lie Bracket Test" << endl;
148 | cerr << "Test case: " << i << ", " <<j<< endl;
149 | cerr << resDiff << endl;
150 | cerr << endl;
151 | passed = false;
152 | }
153 | }
154 | }
155 | return passed;
156 | }
157 |
158 | bool mapAndMultTest() {
159 | bool passed = true;
160 | for (size_t i=0; i<group_vec_.size(); ++i) {
161 | for (size_t j=0; j<group_vec_.size(); ++j) {
162 | Transformation mul_resmat = (group_vec_[i]*group_vec_[j]).matrix();
163 | Scalar fastmul_res_raw[LieGroup::num_parameters];
164 | Eigen::Map<LieGroup> fastmul_res(fastmul_res_raw);
165 | Eigen::Map<const LieGroup> group_j_constmap(group_vec_[j].data());
166 | fastmul_res = group_vec_[i];
167 | fastmul_res.fastMultiply(group_j_constmap);
168 | Transformation diff = mul_resmat-fastmul_res.matrix();
169 | Scalar nrm = diff.norm();
170 | if (isnan(nrm) || nrm>SMALL_EPS) {
171 | cerr << "Map & Multiply" << endl;
172 | cerr << "Test case: " << i << "," << j << endl;
173 | cerr << diff <<endl;
174 | cerr << endl;
175 | passed = false;
176 | }
177 | }
178 | }
179 | return passed;
180 | }
181 |
182 | bool veeHatTest() {
183 | bool passed = true;
184 | for (size_t i=0; i<tangent_vec_.size(); ++i) {
185 | Tangent resDiff
186 | = tangent_vec_[i] - LieGroup::vee(LieGroup::hat(tangent_vec_[i]));
187 | if (norm(resDiff)>SMALL_EPS) {
188 | cerr << "Hat-vee Test" << endl;
189 | cerr << "Test case: " << i << endl;
190 | cerr << resDiff << endl;
191 | cerr << endl;
192 | passed = false;
193 | }
194 | }
195 | return passed;
196 | }
197 |
198 |
199 |
200 | void runAllTests() {
201 | bool passed = adjointTest();
202 | if (!passed) {
203 | cerr << "failed!" << endl << endl;
204 | exit(-1);
205 | }
206 | passed = expLogTest();
207 | if (!passed) {
208 | cerr << "failed!" << endl << endl;
209 | exit(-1);
210 | }
211 | passed = expMapTest();
212 | if (!passed) {
213 | cerr << "failed!" << endl << endl;
214 | exit(-1);
215 | }
216 | passed = groupActionTest();
217 | if (!passed) {
218 | cerr << "failed!" << endl << endl;
219 | exit(-1);
220 | }
221 | passed = lieBracketTest();
222 | if (!passed) {
223 | cerr << "failed!" << endl << endl;
224 | exit(-1);
225 | }
226 | passed = mapAndMultTest();
227 | if (!passed) {
228 | cerr << "failed!" << endl << endl;
229 | exit(-1);
230 | }
231 | passed = veeHatTest();
232 | if (!passed) {
233 | cerr << "failed!" << endl << endl;
234 | exit(-1);
235 | }
236 | cerr << "passed." << endl << endl;
237 | }
238 |
239 | private:
240 | Matrix<Scalar,N-1,1> map(const Matrix<Scalar,N,N> & T,
241 | const Matrix<Scalar,N-1,1> & p) {
242 | return T.template topLeftCorner<N-1,N-1>()*p
243 | + T.template topRightCorner<N-1,1>();
244 | }
245 |
246 | Matrix<Scalar,N,1> map(const Matrix<Scalar,N,N> & T,
247 | const Matrix<Scalar,N,1> & p) {
248 | return T*p;
249 | }
250 |
251 | Scalar norm(const Scalar & v) {
252 | return std::abs(v);
253 | }
254 |
255 | Scalar norm(const Matrix<Scalar,DoF,1> & T) {
256 | return T.norm();
257 | }
258 |
259 | std::vector<LieGroup> group_vec_;
260 | std::vector<Tangent> tangent_vec_;
261 | std::vector<Point> point_vec_;
262 | };
263 | }
264 | #endif // TESTS_HPP
265 |
--------------------------------------------------------------------------------
/thirdparty/libzip-1.1.1.tar.gz:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/JakobEngel/dso/7b0c99f01d238f801c625beaff90240bcb007198/thirdparty/libzip-1.1.1.tar.gz
--------------------------------------------------------------------------------