├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── dataset ├── indoor_flying1 │ ├── calib.txt │ └── events.txt ├── poster_6dof │ ├── calib.txt │ └── events.txt ├── poster_rotation │ ├── calib.txt │ ├── events.txt │ └── sequence_error.m └── poster_translation │ ├── calib.txt │ └── events.txt ├── include ├── EventEMin.h └── EventEMin │ ├── convolution.h │ ├── data_stats.h │ ├── dispersion.h │ ├── dispersion │ ├── approximate_dispersion │ │ ├── dispersion.h │ │ ├── potential.h │ │ ├── renyi.h │ │ ├── shannon.h │ │ ├── sharma_mittal.h │ │ └── tsallis.h │ ├── dispersion.h │ ├── dispersion │ │ ├── dispersion.h │ │ ├── potential.h │ │ ├── renyi.h │ │ ├── shannon.h │ │ ├── sharma_mittal.h │ │ └── tsallis.h │ ├── incremental_dispersion.h │ └── incremental_dispersion │ │ ├── dispersion.h │ │ ├── potential.h │ │ └── tsallis.h │ ├── event.h │ ├── event │ ├── conversion.h │ ├── io.h │ ├── show.h │ ├── transform.h │ ├── type.h │ └── undistort.h │ ├── gauss_kernel.h │ ├── image.h │ ├── image │ ├── conversion.h │ ├── show.h │ └── undistort.h │ ├── model.h │ ├── model │ ├── incremental_model.h │ ├── incremental_model │ │ ├── affinity.h │ │ ├── isometry.h │ │ ├── rotation.h │ │ ├── similarity.h │ │ ├── six_dof.h │ │ ├── translation2d.h │ │ ├── translation3d.h │ │ └── translation_normal.h │ ├── model.h │ └── model │ │ ├── affinity.h │ │ ├── homography.h │ │ ├── isometry.h │ │ ├── rotation.h │ │ ├── similarity.h │ │ ├── six_dof.h │ │ ├── translation.h │ │ ├── translation2d.h │ │ ├── translation3d.h │ │ └── translation_normal.h │ ├── optimiser.h │ ├── optimiser │ └── gsl_fdf_optimiser.h │ ├── test.h │ ├── types_def.h │ └── utilities.h ├── src └── CMakeLists.txt └── test ├── CMakeLists.txt ├── main_6dof.cpp ├── main_homography.cpp ├── main_incremental_6dof.cpp ├── main_incremental_rotation.cpp ├── main_incremental_test_sequence.cpp ├── main_incremental_test_sequence_3d.cpp ├── main_incremental_translation2d.cpp ├── main_rotation.cpp ├── main_test_sequence.cpp ├── main_test_sequence_3d.cpp └── main_translation2d.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AlwaysBreakAfterDefinitionReturnType: All 4 | AlwaysBreakAfterReturnType: All 5 | BreakBeforeBraces: Allman 6 | DerivePointerAlignment: 'false' 7 | ... 8 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | bin 35 | build 36 | data 37 | lib 38 | *~ 39 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | 3 | set(LIB_NAME EventEMin) 4 | project(${LIB_NAME} LANGUAGES CXX) 5 | 6 | # C++ version 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_CXX_EXTENSIONS OFF) 10 | 11 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin) 12 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) 13 | set(${LIB_NAME}_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include) 14 | set(${LIB_NAME}_SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src) 15 | set(${LIB_NAME}_TEST_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/test) 16 | 17 | # CXX flags 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -march=native -mtune=native") 19 | 20 | # Default to Release 21 | set(DEFAULT_BUILD_TYPE "Release") 22 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 23 | message(STATUS "Setting build type to '${DEFAULT_BUILD_TYPE}' as none was specified.") 24 | set(CMAKE_BUILD_TYPE "${DEFAULT_BUILD_TYPE}" CACHE STRING "Choose the type of build." FORCE) 25 | # Set the possible values of build type for cmake-gui 26 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS 27 | "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 28 | endif() 29 | 30 | # Dependencies 31 | 32 | # OpenMP 33 | option(${LIB_NAME}_USE_OPENMP "If available, use OpenMP for parallelization." ON) 34 | if(${LIB_NAME}_USE_OPENMP) 35 | find_package(OpenMP) 36 | endif() 37 | 38 | if(OPENMP_FOUND) 39 | message("OpenMP found") 40 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 41 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 42 | else() 43 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unknown-pragmas") 44 | endif() 45 | 46 | # Eigen 47 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 48 | 49 | # OpenCV 50 | find_package(OpenCV REQUIRED) 51 | 52 | # Build library 53 | add_library(${LIB_NAME} INTERFACE) 54 | 55 | # Build batch mode 56 | option(${LIB_NAME}_BATCH_MODE "Use batch mode." OFF) 57 | if(${LIB_NAME}_BATCH_MODE) 58 | # gsl 59 | find_package(GSL REQUIRED) 60 | 61 | # interface batch lib 62 | add_library(${LIB_NAME}_BATCH_LIB INTERFACE) 63 | target_include_directories(${LIB_NAME}_BATCH_LIB INTERFACE ${${LIB_NAME}_INCLUDE_DIRS} ${GSL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 64 | target_link_libraries(${LIB_NAME}_BATCH_LIB INTERFACE ${GSL_LIBRARIES} Eigen3::Eigen ${OpenCV_LIBS}) 65 | 66 | target_compile_definitions(${LIB_NAME} INTERFACE ${LIB_NAME}_BATCH_MODE) 67 | 68 | target_link_libraries(${LIB_NAME} INTERFACE ${LIB_NAME}_BATCH_LIB) 69 | endif() 70 | 71 | # Build incremental mode 72 | option(${LIB_NAME}_INCREMENTAL_MODE "Use incremental mode." OFF) 73 | if(${LIB_NAME}_INCREMENTAL_MODE) 74 | # interface incremental lib 75 | add_library(${LIB_NAME}_INCREMENTAL_LIB INTERFACE) 76 | target_include_directories(${LIB_NAME}_INCREMENTAL_LIB INTERFACE ${${LIB_NAME}_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 77 | target_link_libraries(${LIB_NAME}_INCREMENTAL_LIB INTERFACE Eigen3::Eigen ${OpenCV_LIBS}) 78 | 79 | target_compile_definitions(${LIB_NAME} INTERFACE ${LIB_NAME}_INCREMENTAL_MODE) 80 | 81 | target_link_libraries(${LIB_NAME} INTERFACE ${LIB_NAME}_INCREMENTAL_LIB) 82 | endif() 83 | 84 | # Option for fast exp computation 85 | option(${LIB_NAME}_FAST_EXP "Enable fast exp" ON) 86 | if(${LIB_NAME}_FAST_EXP) 87 | target_compile_definitions(${LIB_NAME} INTERFACE ${LIB_NAME}_FAST_EXP) 88 | endif() 89 | 90 | add_subdirectory(${${LIB_NAME}_SOURCE_DIRS}) 91 | add_subdirectory(${${LIB_NAME}_TEST_DIRS}) 92 | -------------------------------------------------------------------------------- /dataset/indoor_flying1/calib.txt: -------------------------------------------------------------------------------- 1 | 226.38018519795807 226.15002947047415 173.6470807871759 133.73271487507847 -0.048031442223833355 0.011330957517194437 -0.055378166304281135 0.021500973881459395 0 2 | 346 260 3 | -------------------------------------------------------------------------------- /dataset/poster_6dof/calib.txt: -------------------------------------------------------------------------------- 1 | 199.092366542 198.82882047 132.192071378 110.712660011 -0.368436311798 0.150947243557 -0.000296130534385 -0.000759431726241 0.0 2 | 240 180 3 | -------------------------------------------------------------------------------- /dataset/poster_rotation/calib.txt: -------------------------------------------------------------------------------- 1 | 199.092366542 198.82882047 132.192071378 110.712660011 -0.368436311798 0.150947243557 -0.000296130534385 -0.000759431726241 0.0 2 | 240 180 3 | -------------------------------------------------------------------------------- /dataset/poster_rotation/sequence_error.m: -------------------------------------------------------------------------------- 1 | clear, clc, close all; 2 | 3 | %% read ground-truth vars 4 | disp('please select the file containing the ground-truth (it should be the imu.txt file)'); 5 | [file, folder] = uigetfile({'*.txt';'*.*'}, 'File Selector'); 6 | fimu = fullfile(folder, file); 7 | 8 | A = readmatrix(fimu); 9 | Aw = [A(:, 1) rad2deg(A(:, 5)) rad2deg(A(:, 6)) rad2deg(A(:, 7))]; 10 | 11 | %% read estimated vars 12 | disp('please select the file containing the estimates'); 13 | [file, folder] = uigetfile({'*.txt';'*.*'}, 'File Selector'); 14 | festimates = fullfile(folder, file); 15 | 16 | B = readmatrix(festimates); 17 | delay = 2.4e-3; 18 | Bw = [(B(:, 1) - delay) rad2deg(B(:, 2)) -rad2deg(B(:, 3)) rad2deg(B(:, 4))]; 19 | maxT = 59.75; 20 | Aw = Aw(Aw(:, 1) <= maxT, :); 21 | Bw = Bw(Bw(:, 1) <= maxT, :); 22 | 23 | %% error computation 24 | Awi = interp1(Aw(:, 1), Aw(:, 2:4), Bw(:, 1), 'spline'); 25 | erw = Awi - Bw(:, 2:4); 26 | 27 | % error statistics 28 | erwAbsMean = mean(abs(erw)); 29 | fprintf('\nmean (ex, ey, ez): %f %f %f\n', erwAbsMean); 30 | fprintf('mean (ew): %f\n', mean(erwAbsMean)); 31 | erwStd = std(erw); 32 | fprintf('std (ex, ey, ez): %f %f %f\n', erwStd); 33 | fprintf('std (ew): %f\n', mean(erwStd)); 34 | erwRms = rms(erw); 35 | erwMeanRms = mean(erwRms); 36 | fprintf('rms (ex, ey, ez): %f %f %f\n', erwRms); 37 | fprintf('rms (ew, percentage): %f %f\n', erwMeanRms, ... 38 | 100 * erwMeanRms/max(abs(Aw(:, 2:4)), [], 'all')); 39 | -------------------------------------------------------------------------------- /dataset/poster_translation/calib.txt: -------------------------------------------------------------------------------- 1 | 199.092366542 198.82882047 132.192071378 110.712660011 -0.368436311798 0.150947243557 -0.000296130534385 -0.000759431726241 0.0 2 | 240 180 3 | -------------------------------------------------------------------------------- /include/EventEMin.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_H 2 | #define EVENT_EMIN_H 3 | 4 | #include "EventEMin/convolution.h" 5 | #include "EventEMin/data_stats.h" 6 | #include "EventEMin/dispersion.h" 7 | #include "EventEMin/event.h" 8 | #include "EventEMin/gauss_kernel.h" 9 | #include "EventEMin/image.h" 10 | #include "EventEMin/model.h" 11 | #include "EventEMin/optimiser.h" 12 | #include "EventEMin/test.h" 13 | #include "EventEMin/types_def.h" 14 | 15 | #endif // EVENT_EMIN_H 16 | -------------------------------------------------------------------------------- /include/EventEMin/convolution.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_CONVOLUTION_H 2 | #define EVENT_EMIN_CONVOLUTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "EventEMin/types_def.h" 10 | 11 | namespace EventEMin 12 | { 13 | template 14 | struct OffsetOp 15 | { 16 | T 17 | operator()(const T& val) const 18 | { 19 | return (val - 1) >> 1; 20 | } 21 | 22 | T 23 | operator()(const T& val1, const T& val2) const 24 | { 25 | return val1 + val2 - 1; 26 | } 27 | }; 28 | 29 | template 30 | struct AddOffsetOp 31 | { 32 | T 33 | operator()(const T& val1, const T& val2) const 34 | { 35 | return val1 + val2; 36 | } 37 | }; 38 | 39 | template 40 | struct SubOffsetOp 41 | { 42 | T 43 | operator()(const T& val1, const T& val2) const 44 | { 45 | return val1 - val2 - 1; 46 | } 47 | }; 48 | 49 | template 50 | Container 51 | transform(const Container& c, Functor&& op) 52 | { 53 | Container container; 54 | std::transform(c.begin(), c.end(), container.begin(), op); 55 | return container; 56 | } 57 | 58 | template 59 | Container 60 | transform(const Container& c1, const Container& c2, Functor&& op) 61 | { 62 | Container container; 63 | std::transform(c1.begin(), c1.end(), c2.begin(), container.begin(), op); 64 | return container; 65 | } 66 | 67 | template 68 | class Convolution 69 | { 70 | public: 71 | typedef Tensor Kernel; 72 | 73 | private: 74 | const Array dim_; 75 | const Array kdim_; 76 | const Array offset_; 77 | 78 | protected: 79 | const U lambda_; 80 | 81 | Tensor val_; 82 | Tensor ts_; 83 | 84 | public: 85 | Convolution(const Array& dim, const Array& kdim, 86 | const U& lambda = U(2.0 * M_PI), const U& tsRef = U(0.0)) 87 | : dim_(dim), 88 | kdim_(kdim), 89 | offset_(std::move(transform(kdim_, OffsetOp()))), 90 | lambda_(lambda), 91 | val_(std::move(transform(dim_, kdim_, OffsetOp()))), 92 | ts_(dim_) 93 | { 94 | reset(tsRef); 95 | } 96 | 97 | U 98 | lambda(void) const 99 | { 100 | return lambda_; 101 | } 102 | const TensorRef> 103 | val(void) const 104 | { 105 | return val_; 106 | } 107 | T 108 | val(const Array& ind) const 109 | { 110 | return val_(std::move(transform(ind, offset_, AddOffsetOp()))); 111 | } 112 | const TensorRef> 113 | ts(void) const 114 | { 115 | return ts_; 116 | } 117 | U 118 | ts(const Array& ind) const 119 | { 120 | return ts_(ind); 121 | } 122 | 123 | void 124 | conv(const Array& ind, const U& ts, const T& val, 125 | const Kernel& kernel) 126 | { 127 | // found no better way of doing this using Tensor 128 | const U expts = std::exp(-lambda_ * (ts - ts_(ind))); 129 | const Array dim( 130 | std::move(transform(ind, kdim_, AddOffsetOp()))); 131 | Array indTemp; 132 | iterate(expts, val, kernel, 0, dim, ind, indTemp); 133 | ts_(ind) = ts; 134 | } 135 | 136 | // found no efficient way of implementing update() using Tensor 137 | 138 | void 139 | reset(const U& ti = U(0.0)) 140 | { 141 | val_.setZero(); 142 | ts_.setConstant(ti); 143 | } 144 | 145 | protected: 146 | void 147 | iterate(const U& expts, const T& val, const Tensor& kernel, const int d, 148 | const Array& dim, const Array& iind, 149 | Array& ind) 150 | { 151 | if (d >= N) 152 | { 153 | const Array kind( 154 | std::move(transform(dim, ind, SubOffsetOp()))); 155 | val_(ind) *= expts; 156 | val_(ind) += val * kernel(kind); 157 | return; 158 | } 159 | 160 | for (ind[d] = iind[d]; ind[d] < dim[d]; ++ind[d]) 161 | { 162 | iterate(expts, val, kernel, d + 1, dim, iind, ind); 163 | } 164 | } 165 | 166 | private: 167 | }; 168 | 169 | template 170 | class Convolution 171 | { 172 | public: 173 | typedef Matrix Kernel; 174 | 175 | private: 176 | const Array dim_; 177 | const Array kdim_; 178 | const Array offset_; 179 | 180 | protected: 181 | const U lambda_; 182 | 183 | Matrix val_; 184 | Matrix ts_; 185 | 186 | public: 187 | Convolution(const Array& dim, const Array& kdim, 188 | const U& lambda = U(2.0 * M_PI), const U& tsRef = U(0.0)) 189 | : dim_(dim), 190 | kdim_(kdim), 191 | offset_(std::move(transform(kdim_, OffsetOp()))), 192 | lambda_(lambda), 193 | val_(dim_[0] + kdim_[0] - 1, dim_[1] + kdim_[1] - 1), 194 | ts_(dim_[0], dim_[1]) 195 | { 196 | reset(tsRef); 197 | } 198 | 199 | int 200 | width(void) const 201 | { 202 | return dim_[0]; 203 | } 204 | int 205 | height(void) const 206 | { 207 | return dim_[1]; 208 | } 209 | U 210 | lambda(void) const 211 | { 212 | return lambda_; 213 | } 214 | const Ref> 215 | val(void) const 216 | { 217 | return val_.block(offset_[0], offset_[1], width(), height()); 218 | } 219 | T 220 | val(const Array& ind) const 221 | { 222 | assert(-offset_[0] <= ind[0] && ind[0] < width() + offset_[0]); 223 | assert(-offset_[1] <= ind[1] && ind[1] < height() + offset_[1]); 224 | return val_(ind[0] + offset_[0], ind[1] + offset_[1]); 225 | } 226 | const Ref> 227 | ts(void) const 228 | { 229 | return ts_; 230 | } 231 | U 232 | ts(const Array& ind) const 233 | { 234 | assert(0 <= ind[0] && ind[0] < width()); 235 | assert(0 <= ind[1] && ind[1] < height()); 236 | return ts_(ind[0], ind[1]); 237 | } 238 | 239 | void 240 | conv(const Array& ind, const U& ts, const U& lambda, const T& val, 241 | const Kernel& kernel) 242 | { 243 | assert(0 <= ind[0] && ind[0] < width()); 244 | assert(0 <= ind[1] && ind[1] < height()); 245 | assert(kernel.rows() <= kdim_[0]); 246 | assert(kernel.cols() <= kdim_[1]); 247 | 248 | val_.block(ind[0], ind[1], kernel.rows(), kernel.cols()) *= 249 | std::exp(-lambda * (ts - ts_(ind[0], ind[1]))); 250 | val_.block(ind[0], ind[1], kernel.rows(), kernel.cols()) += 251 | val * kernel.reverse(); 252 | ts_(ind[0], ind[1]) = ts; 253 | } 254 | 255 | void 256 | conv(const Array& ind, const U& ts, const T& val, 257 | const Kernel& kernel) 258 | { 259 | conv(ind, ts, lambda(), val, kernel); 260 | } 261 | 262 | void 263 | update(const U& ts) 264 | { 265 | const Matrix tsimg((-lambda() * (ts - ts_.array())).exp()); 266 | 267 | // core image block 268 | val_.block(offset_[0], offset_[1], width(), height()).array() *= 269 | tsimg.array(); 270 | 271 | /* reflective time update */ 272 | 273 | // top left corner image block 274 | val_.topLeftCorner(offset_[0], offset_[1]).array() *= 275 | tsimg.block(1, 1, offset_[0], offset_[1]).reverse().array(); 276 | // top right corner image block 277 | val_.topRightCorner(offset_[0], offset_[1]).array() *= 278 | tsimg.block(1, height() - 1 - offset_[1], offset_[0], offset_[1]) 279 | .reverse() 280 | .array(); 281 | // bottom left corner image block 282 | val_.bottomLeftCorner(offset_[0], offset_[1]).array() *= 283 | tsimg.block(width() - 1 - offset_[0], 1, offset_[0], offset_[1]) 284 | .reverse() 285 | .array(); 286 | // bottom right corner image block 287 | val_.bottomRightCorner(offset_[0], offset_[1]).array() *= 288 | tsimg 289 | .block(width() - 1 - offset_[0], height() - 1 - offset_[1], 290 | offset_[0], offset_[1]) 291 | .reverse() 292 | .array(); 293 | 294 | // top image rows 295 | val_.topRows(offset_[0]).middleCols(offset_[1], height()).array() *= 296 | tsimg.middleRows(offset_[0], offset_[0]).colwise().reverse().array(); 297 | // bottom image rows 298 | val_.bottomRows(offset_[0]).middleCols(offset_[1], height()).array() *= 299 | tsimg.middleRows(width() - 1 - offset_[0], offset_[0]) 300 | .colwise() 301 | .reverse() 302 | .array(); 303 | // left image columns 304 | val_.leftCols(offset_[1]).middleRows(offset_[0], width()).array() *= 305 | tsimg.middleCols(offset_[1], offset_[1]).rowwise().reverse().array(); 306 | // right image columns 307 | val_.rightCols(offset_[1]).middleRows(offset_[0], width()).array() *= 308 | tsimg.middleCols(height() - 1 - offset_[1], offset_[1]) 309 | .rowwise() 310 | .reverse() 311 | .array(); 312 | 313 | resetTime(ts); 314 | } 315 | 316 | void 317 | reset(const U& ti = U(0.0)) 318 | { 319 | val_.setZero(); 320 | resetTime(ti); 321 | } 322 | void 323 | resetTime(const U& ti = U(0.0)) 324 | { 325 | ts_.setConstant(ti); 326 | } 327 | }; 328 | } // namespace EventEMin 329 | 330 | #endif // EVENT_EMIN_CONVOLUTION_H 331 | -------------------------------------------------------------------------------- /include/EventEMin/data_stats.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_DATA_STATS_H 2 | #define EVENT_EMIN_DATA_STATS_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/types_def.h" 8 | 9 | namespace EventEMin 10 | { 11 | template 12 | void 13 | computeWhitening(const MatrixBase& cov, MatrixBase& w) 14 | { 15 | const Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); 16 | w.noalias() = svd.matrixU() * 17 | (svd.singularValues().array() + 1.0e-8) 18 | .sqrt() 19 | .inverse() 20 | .matrix() 21 | .asDiagonal() * 22 | svd.matrixU().transpose(); 23 | } 24 | template 25 | void 26 | computeWhitening(const MatrixBase& cov, MatrixBase& w, 27 | MatrixBase& sValues) 28 | { 29 | const Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); 30 | sValues = (svd.singularValues().array() + 1.0e-8).sqrt(); 31 | w.noalias() = svd.matrixU() * 32 | sValues.array().inverse().matrix().asDiagonal() * 33 | svd.matrixU().transpose(); 34 | } 35 | template 36 | void 37 | colourPoints(const MatrixBase& cWhitened, 38 | const MatrixBase& w, MatrixBase& cCentred) 39 | { 40 | cCentred.noalias() = w.inverse() * cWhitened; 41 | } 42 | template 43 | void 44 | whitenPoints(const MatrixBase& cCentred, 45 | const MatrixBase& w, MatrixBase& cWhitened) 46 | { 47 | cWhitened.noalias() = w * cCentred; 48 | } 49 | 50 | template 51 | class DataStats 52 | { 53 | protected: 54 | Vector min_, max_; 55 | Vector mean_; 56 | Matrix centred_, cov_; 57 | 58 | public: 59 | DataStats(void) = default; 60 | 61 | const Vector& 62 | min(void) const 63 | { 64 | return min_; 65 | } 66 | const Vector& 67 | max(void) const 68 | { 69 | return max_; 70 | } 71 | const Vector& 72 | mean(void) const 73 | { 74 | return mean_; 75 | } 76 | const Matrix& 77 | centred(void) const 78 | { 79 | return centred_; 80 | } 81 | const Matrix& 82 | cov(void) const 83 | { 84 | return cov_; 85 | } 86 | 87 | void 88 | computeAll(const Ref >& data) 89 | { 90 | computeLimits(data); 91 | computeMoments(data); 92 | } 93 | void 94 | computeAll(const Ref >& data, 95 | const Ref >& weights) 96 | { 97 | assert(data.cols() == weights.size()); 98 | computeLimits(data); 99 | computeMoments(data, weights, weights.sum() + T(1.0e-32)); 100 | } 101 | 102 | void 103 | computeCentred(const Ref >& data) 104 | { 105 | centred_ = data.colwise() - mean(); 106 | } 107 | void 108 | computeCov(void) 109 | { 110 | assert(centred().cols() > 1); 111 | cov_.noalias() = centred() * centred().transpose() / (centred().cols() - 1); 112 | } 113 | void 114 | computeCov(const Ref >& weights, const T& weightsSum) 115 | { 116 | cov_.noalias() = 117 | (centred().array().rowwise() * weights.transpose().array()).matrix() * 118 | centred().transpose() / weightsSum; 119 | } 120 | void 121 | computeMean(const Ref >& data) 122 | { 123 | mean_ = data.rowwise().mean(); 124 | } 125 | void 126 | computeMean(const Ref >& data, 127 | const Ref >& weights, const T& weightsSum) 128 | { 129 | mean_.noalias() = (data.array().rowwise() * weights.transpose().array()) 130 | .matrix() 131 | .rowwise() 132 | .sum() / 133 | weightsSum; 134 | } 135 | void 136 | computeMoments(const Ref >& data) 137 | { 138 | computeMean(data); 139 | computeCentred(data); 140 | computeCov(); 141 | } 142 | void 143 | computeMoments(const Ref >& data, 144 | const Ref >& weights, const T& weightsSum) 145 | { 146 | computeMean(data, weights, weightsSum); 147 | computeCentred(data); 148 | computeCov(weights, weightsSum); 149 | } 150 | 151 | void 152 | computeMin(const Ref >& data) 153 | { 154 | min_ = data.rowwise().minCoeff(); 155 | } 156 | void 157 | computeMax(const Ref >& data) 158 | { 159 | max_ = data.rowwise().maxCoeff(); 160 | } 161 | void 162 | computeLimits(const Ref >& data) 163 | { 164 | computeMin(data); 165 | computeMax(data); 166 | } 167 | }; 168 | } // namespace EventEMin 169 | 170 | #endif // EVENT_EMIN_DATA_STATS_H 171 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_DISPERSION_ALL_H 2 | #define EVENT_EMIN_DISPERSION_ALL_H 3 | 4 | // batch-based approximate measures 5 | #include "EventEMin/dispersion/approximate_dispersion/potential.h" 6 | #include "EventEMin/dispersion/approximate_dispersion/renyi.h" 7 | #include "EventEMin/dispersion/approximate_dispersion/shannon.h" 8 | #include "EventEMin/dispersion/approximate_dispersion/sharma_mittal.h" 9 | #include "EventEMin/dispersion/approximate_dispersion/tsallis.h" 10 | 11 | // batch-based exact measures 12 | #include "EventEMin/dispersion/dispersion/potential.h" 13 | #include "EventEMin/dispersion/dispersion/renyi.h" 14 | #include "EventEMin/dispersion/dispersion/shannon.h" 15 | #include "EventEMin/dispersion/dispersion/sharma_mittal.h" 16 | #include "EventEMin/dispersion/dispersion/tsallis.h" 17 | 18 | // incremental-based measures 19 | #include "EventEMin/dispersion/incremental_dispersion/potential.h" 20 | #include "EventEMin/dispersion/incremental_dispersion/tsallis.h" 21 | 22 | #endif // EVENT_EMIN_DISPERSION_ALL_H 23 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/approximate_dispersion/dispersion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_APPROXIMATE_DISPERSION_IMPL_H 2 | #define EVENT_EMIN_APPROXIMATE_DISPERSION_IMPL_H 3 | 4 | #include "EventEMin/convolution.h" 5 | #include "EventEMin/dispersion/dispersion.h" 6 | #include "EventEMin/event/transform.h" 7 | 8 | namespace EventEMin 9 | { 10 | namespace batch 11 | { 12 | namespace approximate 13 | { 14 | template 15 | class Dispersion : public DispersionBase > 16 | { 17 | public: 18 | typedef typename DispersionTraits >::Model Model; 19 | typedef typename DispersionTraits >::T T; 20 | 21 | enum 22 | { 23 | NVars = Model::NVars, 24 | NDims = Model::NDims 25 | }; 26 | 27 | typedef typename Convolution::Kernel Kernel; 28 | 29 | private: 30 | const T dimScaleMax_; 31 | Array cMin_, cMax_, dim_; 32 | 33 | protected: 34 | Array kdim_; 35 | const T halfOffset_, offset_; 36 | const T lambda_; 37 | Kernel kernel_; 38 | 39 | public: 40 | Dispersion(const T& dimScaleMax, const T& offset = T(0.0), 41 | const T& lambda = T(0.0), const int ksize = 3) 42 | : DispersionBase >(), 43 | dimScaleMax_(dimScaleMax), 44 | halfOffset_(T(0.5) * offset), 45 | offset_(offset), 46 | lambda_(lambda) 47 | { 48 | assert(T(0.0) <= offset_); 49 | assert(0 < ksize && ksize % 2 == 1); 50 | assert(T(0.0) <= lambda_); 51 | 52 | for (int d = 0; d < NDims; ++d) 53 | { 54 | cMin_[d] = 0; 55 | } 56 | 57 | kdim_.fill(ksize); 58 | if constexpr (NDims == 2) 59 | { 60 | kernel_.resize(kdim_[0], kdim_[1]); 61 | kernel::gaussKernel(ksize, kernel_); 62 | kernel_ /= kernel_.sum(); 63 | } 64 | else 65 | { 66 | kernel_.resize(kdim_); 67 | kernel::gaussKernel(ksize, kernel_); 68 | const Tensor gaussKernelSum = kernel_.sum(); 69 | kernel_ /= kernel_.constant(gaussKernelSum(0)); 70 | } 71 | } 72 | 73 | T 74 | dimScale(const int d) const 75 | { 76 | assert(0 <= d && d < NDims); 77 | return static_cast(cMax_[d]); 78 | } 79 | int 80 | dim(const int d) const 81 | { 82 | assert(0 <= d && d < NDims); 83 | return dim_[d]; 84 | } 85 | const Array& 86 | dim(void) const 87 | { 88 | return dim_; 89 | } 90 | T 91 | halfOffset(void) const 92 | { 93 | return halfOffset_; 94 | } 95 | T 96 | offset(void) const 97 | { 98 | return offset_; 99 | } 100 | const Array& 101 | kdim(void) const 102 | { 103 | return kdim_; 104 | } 105 | T 106 | lambda(void) const 107 | { 108 | return lambda_; 109 | } 110 | const Kernel& 111 | kernel(void) const 112 | { 113 | return kernel_; 114 | } 115 | 116 | template 117 | U 118 | compute(const Matrix& c) const 119 | { 120 | Convolution conv(dim(), kdim(), lambda(), this->tsRef()); 121 | add(c, conv); 122 | return this->underlying().score(c, conv); 123 | } 124 | 125 | void 126 | computeDimScale(void) 127 | { 128 | const Vector cLimDiff(this->c().rowwise().maxCoeff() - 129 | this->c().rowwise().minCoeff()); 130 | const T limDiffMax = cLimDiff.maxCoeff(); 131 | for (int d = 0; d < NDims; ++d) 132 | { 133 | dim_[d] = std::round(dimScaleMax_ * cLimDiff(d) / limDiffMax); 134 | cMax_[d] = dim_[d] - 1; 135 | } 136 | } 137 | 138 | protected: 139 | template 140 | void 141 | add(const Matrix& c, Convolution& conv) const 142 | { 143 | const computeBoundaries computeBoundaries; 144 | Vector val; 145 | Array cl, lcMin, lcMax; 146 | 147 | for (int k = 0; k < this->nPoints(); ++k) 148 | { 149 | computeBoundaries(c.col(k), cMin_, cMax_, lcMin, lcMax); 150 | if constexpr (NDims == 2) 151 | { 152 | for (cl[0] = lcMin[0]; cl[0] <= lcMax[0]; ++cl[0]) 153 | { 154 | val(0) = 155 | T(1.0) - ((c(0, k) > cl[0]) ? c(0, k) - cl[0] : cl[0] - c(0, k)); 156 | for (cl[1] = lcMin[1]; cl[1] <= lcMax[1]; ++cl[1]) 157 | { 158 | val(1) = T(1.0) - 159 | ((c(1, k) > cl[1]) ? c(1, k) - cl[1] : cl[1] - c(1, k)); 160 | conv.conv(cl, this->ts(k), val.prod(), kernel()); 161 | } 162 | } 163 | } 164 | else 165 | { 166 | addIterate(this->ts(k), c.col(k), lcMin, lcMax, val, cl, conv); 167 | } 168 | } 169 | // no conv update 170 | } 171 | 172 | template 173 | void 174 | addIterate(const T& ts, const Ref >& c, 175 | const Array& lcMin, const Array& lcMax, 176 | Vector& val, Array& cl, 177 | Convolution& conv) const 178 | { 179 | addIterate(ts, 0, c, lcMin, lcMax, val, cl, conv); 180 | } 181 | 182 | template 183 | void 184 | addIterate(const T& ts, const int d, const Ref >& c, 185 | const Array& lcMin, const Array& lcMax, 186 | Vector& val, Array& cl, 187 | Convolution& conv) const 188 | { 189 | if (d >= NDims) 190 | { 191 | conv.conv(cl, ts, val.prod(), kernel()); 192 | return; 193 | } 194 | 195 | for (cl[d] = lcMin[d]; cl[d] <= lcMax[d]; ++cl[d]) 196 | { 197 | val(d) = T(1.0) - ((c(d) > cl[d]) ? c(d) - cl[d] : cl[d] - c(d)); 198 | addIterate(ts, d + 1, c, lcMin, lcMax, val, cl, conv); 199 | } 200 | } 201 | 202 | template 203 | U 204 | interpolate(const Matrix& c, const Convolution& conv) const 205 | { 206 | const computeBoundaries computeBoundaries; 207 | Vector val; 208 | Array cl, lcMin, lcMax; 209 | U f = U(0.0); 210 | 211 | for (int k = 0; k < this->nPoints(); ++k) 212 | { 213 | computeBoundaries(c.col(k), cMin_, cMax_, lcMin, lcMax); 214 | if constexpr (NDims == 2) 215 | { 216 | for (cl[0] = lcMin[0]; cl[0] <= lcMax[0]; ++cl[0]) 217 | { 218 | val(0) = 219 | T(1.0) - ((c(0, k) > cl[0]) ? c(0, k) - cl[0] : cl[0] - c(0, k)); 220 | for (cl[1] = lcMin[1]; cl[1] <= lcMax[1]; ++cl[1]) 221 | { 222 | val(1) = T(1.0) - 223 | ((c(1, k) > cl[1]) ? c(1, k) - cl[1] : cl[1] - c(1, k)); 224 | f += val.prod() * 225 | std::exp(-lambda() * (this->tsEnd() - this->ts(k))) * 226 | conv.val(cl); 227 | } 228 | } 229 | } 230 | else 231 | { 232 | interpolateIterate(this->ts(k), c.col(k), lcMin, lcMax, conv, val, 233 | cl, f); 234 | } 235 | } 236 | return f; 237 | } 238 | 239 | template 240 | void 241 | interpolateIterate(const T& ts, const Ref >& c, 242 | const Array& lcMin, 243 | const Array& lcMax, 244 | const Convolution& conv, 245 | Vector& val, Array& cl, U& f) const 246 | { 247 | interpolateIterate(ts, 0, c, lcMin, lcMax, conv, val, cl, f); 248 | } 249 | 250 | template 251 | void 252 | interpolateIterate(const T& ts, const int d, 253 | const Ref >& c, 254 | const Array& lcMin, 255 | const Array& lcMax, 256 | const Convolution& conv, 257 | Vector& val, Array& cl, U& f) const 258 | { 259 | if (d >= NDims) 260 | { 261 | f += val.prod() * std::exp(-lambda() * (this->tsEnd() - ts)) * 262 | conv.val(cl); 263 | return; 264 | } 265 | 266 | for (cl[d] = lcMin[d]; cl[d] <= lcMax[d]; ++cl[d]) 267 | { 268 | val(d) = T(1.0) - ((c(d) > cl[d]) ? c(d) - cl[d] : cl[d] - c(d)); 269 | interpolateIterate(ts, d + 1, c, lcMin, lcMax, conv, val, cl, f); 270 | } 271 | } 272 | 273 | private: 274 | Derived& 275 | underlying(void) 276 | { 277 | return static_cast(*this); 278 | } 279 | const Derived& 280 | underlying(void) const 281 | { 282 | return static_cast(*this); 283 | } 284 | }; 285 | } // namespace approximate 286 | } // namespace batch 287 | } // namespace EventEMin 288 | 289 | #endif // EVENT_EMIN_APPROXIMATE_DISPERSION_IMPL_H 290 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/approximate_dispersion/potential.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_APPROXIMATE_POTENTIAL_H 2 | #define EVENT_EMIN_APPROXIMATE_POTENTIAL_H 3 | 4 | #include "EventEMin/dispersion/approximate_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace approximate 11 | { 12 | template 13 | class Potential; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace approximate 24 | { 25 | template 26 | class Potential : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | public: 39 | Potential(const T& dimScaleMax, const T& offset = T(5.0), 40 | const T& lambda = T(0.0), const int ksize = 3) 41 | : Dispersion >(dimScaleMax, offset, lambda, ksize) 42 | { 43 | } 44 | 45 | template 46 | U 47 | score(const Matrix& c, const Convolution& conv) const 48 | { 49 | return -this->interpolate(c, conv) / this->nPoints(); 50 | } 51 | }; 52 | } // namespace approximate 53 | } // namespace batch 54 | 55 | template 56 | using ApproximatePotential = batch::approximate::Potential; 57 | } // namespace EventEMin 58 | 59 | #endif // EVENT_EMIN_APPROXIMATE_POTENTIAL_H 60 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/approximate_dispersion/renyi.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_APPROXIMATE_RENYI_H 2 | #define EVENT_EMIN_APPROXIMATE_RENYI_H 3 | 4 | #include "EventEMin/dispersion/approximate_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace approximate 11 | { 12 | template 13 | class Renyi; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace approximate 24 | { 25 | template 26 | class Renyi : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T alpha_; 40 | const T scale_; 41 | 42 | public: 43 | Renyi(const T& dimScaleMax, const T& offset = T(5.0), 44 | const T& lambda = T(0.0), const int ksize = 3, const T& alpha = T(2.0)) 45 | : Dispersion >(dimScaleMax, offset, lambda, ksize), 46 | alpha_(alpha), 47 | scale_(T(1.0) / (T(1.0) - alpha_)) 48 | { 49 | assert(T(0.0) < alpha_ && alpha_ != T(1.0)); 50 | if constexpr (NDims == 2) 51 | { 52 | this->kernel_ = this->kernel_.array().pow(alpha_); 53 | } 54 | else 55 | { 56 | this->kernel_ = this->kernel_.pow(alpha_); 57 | } 58 | } 59 | 60 | T 61 | alpha(void) const 62 | { 63 | return alpha_; 64 | } 65 | T 66 | scale(void) const 67 | { 68 | return scale_; 69 | } 70 | 71 | template 72 | U 73 | score(const Matrix& c, const Convolution& conv) const 74 | { 75 | return scale() * log(this->interpolate(c, conv) / this->nPoints()); 76 | } 77 | }; 78 | } // namespace approximate 79 | } // namespace batch 80 | 81 | template 82 | using ApproximateRenyi = batch::approximate::Renyi; 83 | } // namespace EventEMin 84 | 85 | #endif // EVENT_EMIN_APPROXIMATE_RENYI_H 86 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/approximate_dispersion/shannon.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_APPROXIMATE_SHANNON_H 2 | #define EVENT_EMIN_APPROXIMATE_SHANNON_H 3 | 4 | #include "EventEMin/dispersion/approximate_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace approximate 11 | { 12 | template 13 | class Shannon; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace approximate 24 | { 25 | template 26 | class Shannon : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | public: 39 | Shannon(const T& dimScaleMax, const T& offset = T(5.0), 40 | const T& lambda = T(0.0), const int ksize = 3) 41 | : Dispersion >(dimScaleMax, offset, lambda, ksize) 42 | { 43 | if constexpr (NDims == 2) 44 | { 45 | this->kernel_ = this->kernel_.array() * this->kernel_.array().log(); 46 | } 47 | else 48 | { 49 | this->kernel_ = this->kernel_ * this->kernel_.log(); 50 | } 51 | } 52 | 53 | template 54 | U 55 | score(const Matrix& c, const Convolution& conv) const 56 | { 57 | return this->interpolate(c, conv) / this->nPoints(); 58 | } 59 | }; 60 | } // namespace approximate 61 | } // namespace batch 62 | 63 | template 64 | using ApproximateShannon = batch::approximate::Shannon; 65 | } // namespace EventEMin 66 | 67 | #endif // EVENT_EMIN_APPROX_SHANNON_H 68 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/approximate_dispersion/sharma_mittal.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_APPROXIMATE_SHARMA_MITTAL_H 2 | #define EVENT_EMIN_APPROXIMATE_SHARMA_MITTAL_H 3 | 4 | #include "EventEMin/dispersion/approximate_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace approximate 11 | { 12 | template 13 | class SharmaMittal; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace approximate 24 | { 25 | template 26 | class SharmaMittal : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T alpha_, beta_, gamma_; 40 | const T scale_; 41 | 42 | public: 43 | SharmaMittal(const T& dimScaleMax, const T& offset = T(5.0), 44 | const T& lambda = T(0.0), const int ksize = 3, 45 | const T& alpha = T(2.0), const T& beta = T(0.5)) 46 | : Dispersion >(dimScaleMax, offset, lambda, ksize), 47 | alpha_(alpha), 48 | beta_(beta), 49 | gamma_((T(1.0) - beta_) / (T(1.0) - alpha_)), 50 | scale_(T(1.0) / (T(1.0) - beta_)) 51 | { 52 | assert(T(0.0) < alpha_ && alpha_ != T(1.0)); 53 | assert(beta_ != T(1.0)); 54 | if constexpr (NDims == 2) 55 | { 56 | this->kernel_ = this->kernel_.array().pow(alpha_); 57 | } 58 | else 59 | { 60 | this->kernel_ = this->kernel_.pow(alpha_); 61 | } 62 | } 63 | 64 | T 65 | alpha(void) const 66 | { 67 | return alpha_; 68 | } 69 | T 70 | beta(void) const 71 | { 72 | return beta_; 73 | } 74 | T 75 | gamma(void) const 76 | { 77 | return gamma_; 78 | } 79 | T 80 | scale(void) const 81 | { 82 | return scale_; 83 | } 84 | 85 | template 86 | U 87 | score(const Matrix& c, const Convolution& conv) const 88 | { 89 | return scale() * pow(this->interpolate(c, conv) / this->nPoints(), gamma()); 90 | } 91 | }; 92 | } // namespace approximate 93 | } // namespace batch 94 | 95 | template 96 | using ApproximateSharmaMittal = batch::approximate::SharmaMittal; 97 | } // namespace EventEMin 98 | 99 | #endif // EVENT_EMIN_APPROX_SHARMA_MITTAL_H 100 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/approximate_dispersion/tsallis.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_APPROXIMATE_TSALLIS_H 2 | #define EVENT_EMIN_APPROXIMATE_TSALLIS_H 3 | 4 | #include "EventEMin/dispersion/approximate_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace approximate 11 | { 12 | template 13 | class Tsallis; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace approximate 24 | { 25 | template 26 | class Tsallis : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T alpha_; 40 | const T scale_; 41 | 42 | public: 43 | Tsallis(const T& dimScaleMax, const T& offset = T(5.0), 44 | const T& lambda = T(0.0), const int ksize = 3, 45 | const T& alpha = T(2.0)) 46 | : Dispersion >(dimScaleMax, offset, lambda, ksize), 47 | alpha_(alpha), 48 | scale_(T(1.0) / (T(1.0) - alpha_)) 49 | { 50 | assert(T(0.0) < alpha_ && alpha_ != T(1.0)); 51 | if constexpr (NDims == 2) 52 | { 53 | this->kernel_ = this->kernel_.array().pow(alpha_); 54 | } 55 | else 56 | { 57 | this->kernel_ = this->kernel_.pow(alpha_); 58 | } 59 | } 60 | 61 | T 62 | alpha(void) const 63 | { 64 | return alpha_; 65 | } 66 | T 67 | scale(void) const 68 | { 69 | return scale_; 70 | } 71 | 72 | template 73 | U 74 | score(const Matrix& c, const Convolution& conv) const 75 | { 76 | return scale() * this->interpolate(c, conv) / this->nPoints(); 77 | } 78 | }; 79 | } // namespace approximate 80 | } // namespace batch 81 | 82 | template 83 | using ApproximateTsallis = batch::approximate::Tsallis; 84 | } // namespace EventEMin 85 | 86 | #endif // EVENT_EMIN_APPROXIMATE_TSALLIS_H 87 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_DISPERSION_H 2 | #define EVENT_EMIN_DISPERSION_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/data_stats.h" 8 | #include "EventEMin/gauss_kernel.h" 9 | #include "EventEMin/types_def.h" 10 | 11 | namespace EventEMin 12 | { 13 | namespace batch 14 | { 15 | template 16 | struct DispersionTraits; 17 | 18 | template 19 | class DispersionBase 20 | { 21 | public: 22 | typedef typename DispersionTraits::Model Model; 23 | typedef typename DispersionTraits::T T; 24 | 25 | enum 26 | { 27 | NVars = Model::NVars, 28 | NDims = Model::NDims, 29 | InputsAtCompileTime = NVars, 30 | ValuesAtCompileTime = 1 31 | }; 32 | 33 | typedef Vector InputType; 34 | typedef Vector ValueType; 35 | 36 | private: 37 | int nPoints_; 38 | 39 | Vector cLimDiff_; 40 | DataStats cStats_; 41 | 42 | protected: 43 | const Model model_; 44 | Map > c_; 45 | Map > ts_; 46 | Map > polarity_; 47 | bool whiten_; 48 | 49 | public: 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | 52 | DispersionBase(void) 53 | : nPoints_(0), 54 | c_(nullptr, 0, 0), 55 | ts_(nullptr, 0), 56 | polarity_(nullptr, 0), 57 | whiten_(true) 58 | { 59 | } 60 | 61 | int 62 | nPoints(void) const 63 | { 64 | return nPoints_; 65 | } 66 | const Ref > 67 | c(void) const 68 | { 69 | return c_; 70 | } 71 | const Ref > 72 | c(const int k) const 73 | { 74 | assert(0 <= k && k < nPoints()); 75 | return c_.col(k); 76 | } 77 | T 78 | cLimDiff(const int d) const 79 | { 80 | assert(0 <= d && d < NDims); 81 | return cLimDiff_(d); 82 | } 83 | const Vector& 84 | cLimDiff(void) const 85 | { 86 | return cLimDiff_; 87 | } 88 | const Ref > 89 | ts(void) const 90 | { 91 | return ts_; 92 | } 93 | T 94 | ts(const int k) const 95 | { 96 | assert(0 <= k && k < nPoints()); 97 | return ts_(k); 98 | } 99 | T 100 | tsRef(void) const 101 | { 102 | return ts_(0); 103 | } 104 | T 105 | tsMiddle(void) const 106 | { 107 | return ts_(nPoints() >> 1); 108 | } 109 | T 110 | tsEnd(void) const 111 | { 112 | return ts_(nPoints() - 1); 113 | } 114 | const Ref > 115 | polarity(void) const 116 | { 117 | return polarity_; 118 | } 119 | int 120 | polarity(const int k) const 121 | { 122 | assert(0 <= k && k < nPoints()); 123 | return polarity_(k); 124 | } 125 | 126 | void 127 | assignPoints(const Ref >& c, const Ref >& ts, 128 | const Ref >& polarity, 129 | const bool whiten = true) 130 | { 131 | assert(c.rows() == NDims); 132 | nPoints_ = c.cols(); 133 | assert(ts.size() == nPoints()); 134 | assert(polarity.size() == nPoints()); 135 | 136 | new (&c_) Map >(c.data(), NDims, nPoints()); 137 | new (&ts_) Map >(ts.data(), nPoints()); 138 | new (&polarity_) Map >(polarity.data(), nPoints()); 139 | whiten_ = whiten; 140 | 141 | if (whiten_) 142 | { 143 | cStats_.computeMoments(c_); 144 | Matrix w; 145 | computeWhitening(cStats_.cov(), w); 146 | Matrix cScaled; 147 | whitenPoints(cStats_.centred(), w, cScaled); 148 | cStats_.computeLimits(cScaled); 149 | } 150 | else 151 | { 152 | cStats_.computeLimits(c_); 153 | } 154 | cLimDiff_ = (cStats_.max() - cStats_.min()).array() + T(1.0e-8); 155 | 156 | this->underlying().computeDimScale(); 157 | } 158 | 159 | template 160 | void 161 | operator()(const Vector& vars, Vector* f) const 162 | { 163 | Matrix cm(static_cast(NDims), nPoints()), 164 | cmScaled(static_cast(NDims), nPoints()); 165 | 166 | modelPoints(vars, cm); 167 | if (whiten_) 168 | { 169 | DataStats cmStats; 170 | cmStats.computeMoments(cm); 171 | Matrix w; 172 | computeWhitening(cmStats.cov(), w); 173 | whitenPoints(cmStats.centred(), w, cm); 174 | } 175 | scalePoints(cm, cmScaled); 176 | 177 | (*f)(0) = this->underlying().compute(cmScaled); 178 | } 179 | 180 | protected: 181 | template 182 | void 183 | modelPoints(const Vector& vars, Matrix& cm) const 184 | { 185 | model_(vars, c(), ts(), cm); 186 | } 187 | 188 | template 189 | void 190 | scalePoints(const Ref >& c, Ref > cScaled) const 191 | { 192 | for (int d = 0; d < NDims; ++d) 193 | { 194 | cScaled.row(d) = 195 | ((this->underlying().dimScale(d) - this->underlying().offset()) / 196 | cLimDiff_(d)) * 197 | (c.row(d).array() - cStats_.min()(d)) + 198 | this->underlying().halfOffset(); 199 | } 200 | } 201 | 202 | private: 203 | Derived& 204 | underlying(void) 205 | { 206 | return static_cast(*this); 207 | } 208 | const Derived& 209 | underlying(void) const 210 | { 211 | return static_cast(*this); 212 | } 213 | }; 214 | } // namespace batch 215 | } // namespace EventEMin 216 | 217 | #endif // EVENT_EMIN_DISPERSION_H 218 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion/dispersion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_DISPERSION_IMPL_H 2 | #define EVENT_EMIN_DISPERSION_IMPL_H 3 | 4 | #ifdef _OPENMP 5 | #include 6 | #endif 7 | 8 | #include "EventEMin/dispersion/dispersion.h" 9 | 10 | namespace EventEMin 11 | { 12 | namespace batch 13 | { 14 | namespace exact 15 | { 16 | template 17 | class Dispersion : public DispersionBase > 18 | { 19 | public: 20 | typedef typename DispersionTraits >::Model Model; 21 | typedef typename DispersionTraits >::T T; 22 | 23 | enum 24 | { 25 | NVars = Model::NVars, 26 | NDims = Model::NDims 27 | }; 28 | 29 | private: 30 | const T dimScaleMax_; 31 | Array dim_; 32 | 33 | public: 34 | Dispersion(const T& dimScaleMax) 35 | : DispersionBase >(), dimScaleMax_(dimScaleMax) 36 | { 37 | } 38 | 39 | T 40 | dimScale(const int d) const 41 | { 42 | assert(0 <= d && d < NDims); 43 | return static_cast(dim_[d]); 44 | } 45 | T 46 | halfOffset(void) const 47 | { 48 | return T(0.0); 49 | } 50 | T 51 | offset(void) const 52 | { 53 | return T(0.0); 54 | } 55 | 56 | template 57 | U 58 | compute(const Matrix& c) const 59 | { 60 | U s = U(0.0); 61 | #ifdef _OPENMP 62 | #pragma omp parallel shared(c, s) 63 | #endif 64 | { 65 | Matrix cDiff; 66 | Vector cDiffPow; 67 | 68 | #ifdef _OPENMP 69 | #pragma omp declare reduction(sum:U : omp_out += omp_in) initializer(omp_priv = U(0.0)) 70 | #pragma omp for reduction(sum : s) 71 | #endif 72 | for (int k = 0; k < this->nPoints(); ++k) 73 | { 74 | pointsDiff(k, c, cDiff); 75 | pointsPow(cDiff, cDiffPow); 76 | s += this->underlying().partialScore(cDiffPow); 77 | } 78 | } 79 | 80 | return this->underlying().score(s); 81 | } 82 | 83 | void 84 | computeDimScale(void) 85 | { 86 | const Vector cLimDiff(this->c().rowwise().maxCoeff() - 87 | this->c().rowwise().minCoeff()); 88 | const T limDiffMax = cLimDiff.maxCoeff(); 89 | for (int d = 0; d < NDims; ++d) 90 | { 91 | dim_[d] = std::round(dimScaleMax_ * cLimDiff(d) / limDiffMax); 92 | } 93 | } 94 | 95 | protected: 96 | template 97 | void 98 | pointsDiff(const int k, const Matrix& p, Matrix& pDiff) const 99 | { 100 | pDiff = p.colwise() - p.col(k); 101 | } 102 | 103 | template 104 | void 105 | pointsPow(const Matrix& p, Vector& pPow) const 106 | { 107 | pPow = T(-0.5) * p.array().square().colwise().sum(); 108 | } 109 | 110 | private: 111 | Derived& 112 | underlying(void) 113 | { 114 | return static_cast(*this); 115 | } 116 | const Derived& 117 | underlying(void) const 118 | { 119 | return static_cast(*this); 120 | } 121 | }; 122 | } // namespace exact 123 | } // namespace batch 124 | } // namespace EventEMin 125 | 126 | #endif // EVENT_EMIN_DISPERSION_IMPL_H 127 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion/potential.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_POTENTIAL_H 2 | #define EVENT_EMIN_POTENTIAL_H 3 | 4 | #include "EventEMin/dispersion/dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace exact 11 | { 12 | template 13 | class Potential; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace exact 24 | { 25 | template 26 | class Potential : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | public: 39 | Potential(const T& dimScale) : Dispersion >(dimScale) {} 40 | template 41 | U 42 | partialScore(const Vector& cDiffPow) const 43 | { 44 | return cDiffPow.array().exp().sum(); 45 | } 46 | 47 | template 48 | U 49 | score(const U& s) const 50 | { 51 | return -s / this->nPoints(); 52 | } 53 | }; 54 | } // namespace exact 55 | } // namespace batch 56 | 57 | template 58 | using Potential = batch::exact::Potential; 59 | } // namespace EventEMin 60 | 61 | #endif // EVENT_EMIN_POTENTIAL_H 62 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion/renyi.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_RENYI_H 2 | #define EVENT_EMIN_RENYI_H 3 | 4 | #include "EventEMin/dispersion/dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace exact 11 | { 12 | template 13 | class Renyi; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace exact 24 | { 25 | template 26 | class Renyi : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T alpha_; 40 | const T scale_; 41 | 42 | public: 43 | Renyi(const T& dimScale, const T& alpha = T(2.0)) 44 | : Dispersion >(dimScale), 45 | alpha_(alpha), 46 | scale_(T(1.0) / (T(1.0) - alpha_)) 47 | { 48 | assert(T(0.0) < alpha_ && alpha_ != T(1.0)); 49 | } 50 | 51 | T 52 | alpha(void) const 53 | { 54 | return alpha_; 55 | } 56 | T 57 | scale(void) const 58 | { 59 | return scale_; 60 | } 61 | 62 | template 63 | U 64 | partialScore(const Vector& cDiffPow) const 65 | { 66 | return (alpha() * cDiffPow).array().exp().sum(); 67 | } 68 | 69 | template 70 | U 71 | score(const U& s) const 72 | { 73 | return scale() * log(s / this->nPoints()); 74 | } 75 | }; 76 | } // namespace exact 77 | } // namespace batch 78 | 79 | template 80 | using Renyi = batch::exact::Renyi; 81 | } // namespace EventEMin 82 | 83 | #endif // EVENT_EMIN_RENYI_H 84 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion/shannon.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_SHANNON_H 2 | #define EVENT_EMIN_SHANNON_H 3 | 4 | #include "EventEMin/dispersion/dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace exact 11 | { 12 | template 13 | class Shannon; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace exact 24 | { 25 | template 26 | class Shannon : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T logDen_; 40 | 41 | public: 42 | Shannon(const T& dimScale) 43 | : Dispersion >(dimScale), 44 | logDen_(std::log(std::pow(T(2.0 * M_PI), T(0.5) * NDims))) 45 | { 46 | } 47 | template 48 | U 49 | partialScore(const Vector& cDiffPow) const 50 | { 51 | return (cDiffPow.array().exp() * (cDiffPow.array() - logDen_)).sum(); 52 | } 53 | 54 | template 55 | U 56 | score(const U& s) const 57 | { 58 | return s / this->nPoints(); 59 | } 60 | }; 61 | } // namespace exact 62 | } // namespace batch 63 | 64 | template 65 | using Shannon = batch::exact::Shannon; 66 | } // namespace EventEMin 67 | 68 | #endif // EVENT_EMIN_SHANNON_H 69 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion/sharma_mittal.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_SHARMA_MITTAL_H 2 | #define EVENT_EMIN_SHARMA_MITTAL_H 3 | 4 | #include "EventEMin/dispersion/dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace exact 11 | { 12 | template 13 | class SharmaMittal; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace exact 24 | { 25 | template 26 | class SharmaMittal : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T alpha_, beta_, gamma_; 40 | const T scale_; 41 | 42 | public: 43 | SharmaMittal(const T& dimScale, const T& alpha = T(2.0), 44 | const T& beta = T(0.5)) 45 | : Dispersion >(dimScale), 46 | alpha_(alpha), 47 | beta_(beta), 48 | gamma_((T(1.0) - beta_) / (T(1.0) - alpha_)), 49 | scale_(T(1.0) / (T(1.0) - beta_)) 50 | { 51 | assert(T(0.0) < alpha_ && alpha_ != T(1.0)); 52 | assert(beta_ != T(1.0)); 53 | } 54 | 55 | T 56 | alpha(void) const 57 | { 58 | return alpha_; 59 | } 60 | T 61 | beta(void) const 62 | { 63 | return beta_; 64 | } 65 | T 66 | gamma(void) const 67 | { 68 | return gamma_; 69 | } 70 | T 71 | scale(void) const 72 | { 73 | return scale_; 74 | } 75 | 76 | template 77 | U 78 | partialScore(const Vector& cDiffPow) const 79 | { 80 | return (alpha() * cDiffPow).array().exp().sum(); 81 | } 82 | 83 | template 84 | U 85 | score(const U& s) const 86 | { 87 | return scale() * pow(s / this->nPoints(), gamma()); 88 | } 89 | }; 90 | } // namespace exact 91 | } // namespace batch 92 | 93 | template 94 | using SharmaMittal = batch::exact::SharmaMittal; 95 | } // namespace EventEMin 96 | 97 | #endif // EVENT_EMIN_SHARMA_MITTAL_H 98 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/dispersion/tsallis.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TSALLIS_H 2 | #define EVENT_EMIN_TSALLIS_H 3 | 4 | #include "EventEMin/dispersion/dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | namespace exact 11 | { 12 | template 13 | class Tsallis; 14 | } 15 | 16 | template 17 | struct DispersionTraits > > 18 | { 19 | typedef M Model; 20 | typedef typename Model::T T; 21 | }; 22 | 23 | namespace exact 24 | { 25 | template 26 | class Tsallis : public Dispersion > 27 | { 28 | public: 29 | typedef M Model; 30 | typedef typename Model::T T; 31 | 32 | enum 33 | { 34 | NVars = Model::NVars, 35 | NDims = Model::NDims 36 | }; 37 | 38 | private: 39 | const T alpha_; 40 | const T scale_; 41 | 42 | public: 43 | Tsallis(const T& dimScale, const T& alpha = T(2.0)) 44 | : Dispersion >(dimScale), 45 | alpha_(alpha), 46 | scale_(T(1.0) / (T(1.0) - alpha_)) 47 | { 48 | assert(T(0.0) < alpha_ && alpha_ != T(1.0)); 49 | } 50 | 51 | T 52 | alpha(void) const 53 | { 54 | return alpha_; 55 | } 56 | T 57 | scale(void) const 58 | { 59 | return scale_; 60 | } 61 | 62 | template 63 | U 64 | partialScore(const Vector& cDiffPow) const 65 | { 66 | return (alpha() * cDiffPow).array().exp().sum(); 67 | } 68 | 69 | template 70 | U 71 | score(const U& s) const 72 | { 73 | return scale() * s / this->nPoints(); 74 | } 75 | }; 76 | } // namespace exact 77 | } // namespace batch 78 | 79 | template 80 | using Tsallis = batch::exact::Tsallis; 81 | } // namespace EventEMin 82 | 83 | #endif // EVENT_EMIN_TSALLIS_H 84 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/incremental_dispersion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_DISPERSION_H 2 | #define EVENT_EMIN_INCREMENTAL_DISPERSION_H 3 | 4 | #include 5 | 6 | #include "EventEMin/data_stats.h" 7 | #include "EventEMin/types_def.h" 8 | #include "EventEMin/utilities.h" 9 | 10 | namespace EventEMin 11 | { 12 | namespace incremental 13 | { 14 | template 15 | struct DispersionParams 16 | { 17 | T minStep; 18 | int maxIter, wSize; 19 | 20 | DispersionParams(const T& minStep = T(1.0e-6), const int maxIter = 10, 21 | const int wSize = 4) 22 | : minStep(minStep), maxIter(maxIter), wSize(wSize) 23 | { 24 | } 25 | }; 26 | 27 | class Cell 28 | { 29 | private: 30 | int n_, nMax_, nMaxMinus1_; 31 | int first_, last_; 32 | 33 | StdVector ind_; 34 | 35 | public: 36 | Cell(const int nMax = 2) 37 | : n_(0), 38 | nMax_(nMax), 39 | nMaxMinus1_(nMax_ - 1), 40 | first_(0), 41 | last_(0), 42 | ind_(nMax_) 43 | { 44 | assert(0 < nMaxMinus1_ && (nMax_ & nMaxMinus1_) == 0); 45 | } 46 | 47 | int 48 | n(void) const 49 | { 50 | return n_; 51 | } 52 | int 53 | ind(const int k) const 54 | { 55 | return ind_[fastAddCyclic(first_, k, nMaxMinus1_)]; 56 | } 57 | 58 | void 59 | add(const int ind) 60 | { 61 | if (n_ >= nMax_) 62 | { 63 | nMax_ <<= 1; 64 | nMaxMinus1_ = nMax_ - 1; 65 | ind_.resize(nMax_); 66 | if (0 < first_) 67 | { 68 | std::copy_n(ind_.begin(), last_, ind_.begin() + n_); 69 | } 70 | last_ += n_; 71 | } 72 | 73 | ind_[last_] = ind; 74 | ++n_; 75 | last_ = fastIncrementCyclic(last_, nMaxMinus1_); 76 | } 77 | 78 | int 79 | remove(void) 80 | { 81 | if (0 < n_) 82 | { 83 | first_ = fastIncrementCyclic(first_, nMaxMinus1_); 84 | --n_; 85 | } 86 | return n_; 87 | } 88 | }; 89 | 90 | template 91 | struct Stats 92 | { 93 | typedef typename M::Point Point; 94 | typedef typename M::CMatrix CMatrix; 95 | 96 | Point mean, singularValues; 97 | CMatrix cov, w; 98 | 99 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 100 | 101 | Stats(void) 102 | : mean(Point::Zero()), 103 | singularValues(Point::Ones()), 104 | cov(CMatrix::Identity()), 105 | w(CMatrix::Identity()) 106 | { 107 | } 108 | 109 | void 110 | update(const Ref& c, const int n) 111 | { 112 | const Point cDiff(c - mean); 113 | mean += cDiff / n; 114 | cov.noalias() += (cDiff * (c - mean).transpose() - cov) / n; 115 | 116 | computeWhitening(cov, w, singularValues); 117 | } 118 | }; 119 | 120 | template 121 | class Whitening 122 | { 123 | public: 124 | typedef M Model; 125 | typedef typename Model::T T; 126 | 127 | typedef typename Model::Point Point; 128 | typedef typename Model::Vars Vars; 129 | 130 | private: 131 | projectEvent projectEvent_; 132 | 133 | Model model_; 134 | 135 | Stats istats_; 136 | 137 | public: 138 | Whitening(void) = default; 139 | 140 | void 141 | processEvent(const Matrix& camParams, const Ref& scale, 142 | const Ref& c, Ref cs) const 143 | { 144 | if constexpr (W) 145 | { 146 | Point cw; 147 | whitenPoints(c - istats_.mean, istats_.w, cw); 148 | projectEvent_( 149 | camParams, 150 | ((istats_.singularValues.array() * cw.array() + istats_.mean.array()) 151 | .matrix()), 152 | cs); 153 | } 154 | else 155 | { 156 | projectEvent_(camParams, c, cs); 157 | } 158 | cs.array() *= scale.array(); 159 | } 160 | 161 | void 162 | updateStats([[maybe_unused]] const Ref& vars, 163 | [[maybe_unused]] const Ref& c, 164 | [[maybe_unused]] const T& tsDiffRef, [[maybe_unused]] const int n) 165 | { 166 | if constexpr (W) 167 | { 168 | Point cm; 169 | model_(vars, c, tsDiffRef, cm); 170 | istats_.update(cm, n); 171 | } 172 | } 173 | }; 174 | 175 | template 176 | struct VarsEstimate 177 | { 178 | typedef M Model; 179 | typedef typename Model::T T; 180 | 181 | enum 182 | { 183 | NVars = Model::NVars 184 | }; 185 | 186 | typedef typename Model::Vars Vars; 187 | typedef Matrix VarsMatrix; 188 | 189 | T val, valCum; 190 | Vars vNum, vNumCum; 191 | VarsMatrix vDen, vDenCum; 192 | 193 | Vars vars; 194 | 195 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 196 | 197 | VarsEstimate(void) 198 | : valCum(T(0.0)), 199 | vNumCum(Vars::Zero()), 200 | vDenCum(VarsMatrix::Zero()), 201 | vars(Vars::Zero()) 202 | { 203 | } 204 | 205 | void 206 | decay(const T& dec) 207 | { 208 | valCum *= dec; 209 | vNumCum *= dec; 210 | vDenCum *= dec; 211 | } 212 | 213 | void 214 | update(void) 215 | { 216 | valCum += val; 217 | vNumCum += vNum; 218 | vDenCum += vDen; 219 | } 220 | }; 221 | } // namespace incremental 222 | } // namespace EventEMin 223 | 224 | #endif // EVENT_EMIN_INCREMENTAL_DISPERSION_H 225 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/incremental_dispersion/dispersion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_DISPERSION_IMPL_H 2 | #define EVENT_EMIN_INCREMENTAL_DISPERSION_IMPL_H 3 | 4 | #include "EventEMin/dispersion/incremental_dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | class Dispersion 12 | { 13 | public: 14 | typedef typename DispersionImpl::Model Model; 15 | typedef typename DispersionImpl::T T; 16 | 17 | enum 18 | { 19 | NVars = Model::NVars, 20 | NDims = Model::NDims 21 | }; 22 | 23 | typedef typename Model::Point Point; 24 | typedef typename Model::Vars Vars; 25 | typedef typename Model::CMatrix CMatrix; 26 | typedef typename Model::DMatrix DMatrix; 27 | typedef typename Model::PMatrix PMatrix; 28 | 29 | typedef DispersionParams Params; 30 | 31 | typedef StdVector Grid; 32 | 33 | private: 34 | Matrix camParams_; 35 | projectEvent projectEvent_; 36 | 37 | Point scale_; 38 | 39 | Array cMin_, cMax_; 40 | Array dim_; 41 | 42 | Params params_; 43 | 44 | int nPointsCur_, nPointsMax_, nBuffer_; 45 | int curInd_, prevInd_, refInd_, nPoints_; 46 | 47 | StdVector inds_; 48 | StdVector c_; 49 | StdVector ts_; 50 | StdVector tsDiffRef_; 51 | StdVector > ij_; 52 | 53 | T nDecay_; 54 | 55 | int iter_; 56 | 57 | DispersionImpl dispersionImpl_; 58 | Whitening whitening_; 59 | 60 | protected: 61 | Grid grid_; 62 | 63 | Model model_; 64 | VarsEstimate varse_; 65 | 66 | public: 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | 69 | Dispersion(void) = delete; 70 | Dispersion(const Matrix& camParams, const Point& scale, 71 | const Params& params, const int nPoints, 72 | const Array& dim) 73 | : camParams_(camParams), 74 | scale_(scale), 75 | dim_(dim), 76 | params_(params), 77 | nPointsCur_(nPoints), 78 | nPointsMax_(nPoints), 79 | nBuffer_(nextPower2(nPointsMax_)), 80 | curInd_(0), 81 | prevInd_(0), 82 | refInd_(0), 83 | nPoints_(0), 84 | c_(nBuffer_), 85 | ts_(nBuffer_), 86 | ij_(nBuffer_), 87 | nDecay_(T(0.0)), 88 | iter_(0) 89 | { 90 | --nBuffer_; 91 | int gridSize = 1; 92 | for (int d = 0; d < 2; ++d) 93 | { 94 | cMin_[d] = 0; 95 | cMax_[d] = dim_[d] - 1; 96 | gridSize *= dim_[d]; 97 | } 98 | grid_.resize(gridSize, Cell()); 99 | } 100 | 101 | T 102 | minStep(void) const 103 | { 104 | return params_.minStep; 105 | } 106 | int 107 | maxIter(void) const 108 | { 109 | return params_.maxIter; 110 | } 111 | int 112 | wSize(void) const 113 | { 114 | return params_.wSize; 115 | } 116 | int 117 | iter(void) const 118 | { 119 | return iter_; 120 | } 121 | const Vars& 122 | vars(void) const 123 | { 124 | return varse_.vars; 125 | } 126 | 127 | void 128 | decay(const T& ts) 129 | { 130 | const T dec = computeExp(-nDecay_ * (ts - ts_[prevInd_])); 131 | varse_.decay(dec); 132 | nDecay_ *= dec; 133 | } 134 | 135 | void 136 | run(const Ref& c, const T& ts) 137 | { 138 | Point cu; 139 | projectEvent_(camParams_, c, cu); 140 | const Vector ij( 141 | cu.template head<2>().array().round().template cast()); 142 | const int ind = ij2ind(ij); 143 | Vector ijMin, ijMax; 144 | ijBoundary(ij, ijMin, ijMax); 145 | ij2ind(ijMin, ijMax, grid_, inds_); 146 | 147 | // circular shift 148 | T nProp; 149 | if (nPoints_ < nPointsCur_) 150 | { 151 | ++nPoints_; 152 | nProp = T(nPoints_) / nPointsCur_; 153 | } 154 | else 155 | { 156 | nProp = T(1.0); 157 | refInd_ = fastIncrementCyclic(refInd_, nBuffer_); 158 | // remove old event 159 | grid_[ij2ind(ij_[refInd_])].remove(); 160 | } 161 | // compute difference in times 162 | tsDiffRef_.clear(); 163 | for (int i = 0; i < static_cast(inds_.size()); ++i) 164 | { 165 | tsDiffRef_.push_back(ts_[inds_[i]] - ts_[refInd_]); 166 | } 167 | const T tsDiffRef = ts - ts_[refInd_]; 168 | 169 | // decay factor 170 | decay(ts); 171 | 172 | // compute the estimate with the new event 173 | T d = minStep(); 174 | for (iter_ = 0; iter_ < maxIter() && d >= minStep(); ++iter_) 175 | { 176 | // std::cout << "iter: " << iter_ << '\n'; 177 | d = iterate(inds_, nProp, c, tsDiffRef, varse_); 178 | } 179 | // update variables 180 | varse_.update(); 181 | ++nDecay_; 182 | 183 | // add event to grid 184 | grid_[ind].add(curInd_); 185 | c_[curInd_] = c; 186 | ts_[curInd_] = ts; 187 | ij_[curInd_] = ij; 188 | 189 | // circular shift 190 | prevInd_ = curInd_; 191 | curInd_ = fastIncrementCyclic(curInd_, nBuffer_); 192 | 193 | // update stats incrementally 194 | whitening_.updateStats(varse_.vars, c, tsDiffRef, nPoints_); 195 | } 196 | 197 | void 198 | setInc(const T& inc) 199 | { 200 | const int nPointsCur = T(nPointsMax_) / inc; 201 | if (nPoints_ >= nPointsCur) 202 | { 203 | nPoints_ = nPointsCur; 204 | for (int indDiff = nPointsCur_ - nPointsCur; indDiff > 0; --indDiff) 205 | { 206 | refInd_ = fastIncrementCyclic(refInd_, nBuffer_); 207 | // remove old event 208 | grid_[ij2ind(ij_[refInd_])].remove(); 209 | } 210 | } 211 | nPointsCur_ = nPointsCur; 212 | } 213 | 214 | protected: 215 | T 216 | iterate(const StdVector& inds, const T& nProp, const Ref& c, 217 | const T& tsDiffRef, VarsEstimate& varse) const 218 | { 219 | Point cm, cml, cmDiff, cg, cgl, cms, cmsl; 220 | DMatrix dcm, dcml; 221 | PMatrix per, perl; 222 | model_(varse.vars, c, tsDiffRef, cm, dcm, cg, per); 223 | whitening_.processEvent(camParams_, scale_, cm, cms); 224 | 225 | varse.val = T(0.0); 226 | varse.vNum.setZero(); 227 | varse.vDen.setZero(); 228 | 229 | for (int i = 0; i < static_cast(inds.size()); ++i) 230 | { 231 | model_(varse.vars, c_[inds[i]], tsDiffRef_[i], cml, dcml, cgl, perl); 232 | whitening_.processEvent(camParams_, scale_, cml, cmsl); 233 | cmDiff = cms - cmsl; 234 | 235 | dispersionImpl_(cmDiff, dcm - dcml, cm - cml - cg + cgl, per - perl, 236 | varse); 237 | } 238 | 239 | if (T(0.0) < varse.val) 240 | { 241 | varse.vNum *= nProp; // for stability with few points 242 | // least-squares solution 243 | Vars v((varse.vDenCum + varse.vDen) 244 | .template selfadjointView() 245 | .ldlt() // pos/neg semidefinite 246 | //.partialPivLu() // invertible 247 | //.fullPivLu() 248 | .solve(varse.vNumCum + varse.vNum)); 249 | v -= varse.vars; 250 | varse.vars += v; 251 | const T d = v.norm(); 252 | 253 | /*std::cout << "val: " << varse.val << ' ' << nProp << '\n'; 254 | std::cout << "vNum: " << varse.vNum.transpose() << '\n'; 255 | std::cout << "vNumCum: " << (varse.vNumCum + varse.vNum).transpose() 256 | << '\n'; 257 | std::cout << "vDen: " << varse.vDen << '\n'; 258 | std::cout << "vDenCum: " << varse.vDenCum + varse.vDen << '\n'; 259 | std::cout << "vars: " << varse.vars.transpose() << '\n'; 260 | std::cout << "v: " << v.transpose() << ", d: " << d << '\n';*/ 261 | 262 | return d; 263 | } 264 | return T(0.0); 265 | } 266 | 267 | private: 268 | int 269 | ij2ind(const Ref >& ij) const 270 | { 271 | return ij(0) * dim_[1] + ij(1); 272 | } 273 | void 274 | ij2ind(const Ref >& ijMin, 275 | const Ref >& ijMax, const Grid& grid, 276 | StdVector& inds) const 277 | { 278 | inds.clear(); 279 | Vector ij; 280 | for (ij(0) = ijMin(0); ij(0) <= ijMax(0); ++ij(0)) 281 | { 282 | for (ij(1) = ijMin(1); ij(1) <= ijMax(1); ++ij(1)) 283 | { 284 | const int ind = ij2ind(ij); 285 | for (int i = 0; i < grid[ind].n(); ++i) 286 | { 287 | inds.push_back(grid[ind].ind(i)); 288 | } 289 | } 290 | } 291 | } 292 | 293 | void 294 | ijBoundary(const Ref >& ij, Vector& ijMin, 295 | Vector& ijMax) const 296 | { 297 | for (int d = 0; d < 2; ++d) 298 | { 299 | ijMin(d) = std::max(ij(d) - wSize(), static_cast(cMin_[d])); 300 | ijMax(d) = std::min(ij(d) + wSize(), static_cast(cMax_[d])); 301 | } 302 | } 303 | }; 304 | } // namespace incremental 305 | } // namespace EventEMin 306 | 307 | #endif // EVENT_EMIN_INCREMENTAL_DISPERSION_IMPL_H 308 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/incremental_dispersion/potential.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_POTENTIAL_H 2 | #define EVENT_EMIN_INCREMENTAL_POTENTIAL_H 3 | 4 | #include "EventEMin/dispersion/incremental_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | class Potential 12 | { 13 | public: 14 | typedef M Model; 15 | typedef typename Model::T T; 16 | 17 | enum 18 | { 19 | NVars = Model::NVars, 20 | NDims = Model::NDims 21 | }; 22 | 23 | typedef typename Model::Point Point; 24 | typedef typename Model::Vars Vars; 25 | typedef typename Model::CMatrix CMatrix; 26 | typedef typename Model::DMatrix DMatrix; 27 | typedef typename Model::PMatrix PMatrix; 28 | typedef Matrix WMatrix; 29 | 30 | public: 31 | Potential(void) = default; 32 | 33 | void 34 | operator()(const Ref& cmDiff, const Ref& dcmDiff, 35 | const Ref& cmgDiff, const Ref& perDiff, 36 | VarsEstimate& varse) const 37 | { 38 | const T cmDiffExp = computeExp(T(-0.5) * (cmDiff.transpose() * cmDiff)(0)); 39 | varse.val += cmDiffExp; 40 | const WMatrix cWeight(cmDiffExp * dcmDiff.transpose()); 41 | varse.vNum.noalias() -= cWeight * cmgDiff; 42 | varse.vDen.noalias() += cWeight * perDiff; 43 | } 44 | }; 45 | } // namespace incremental 46 | 47 | template 48 | using IncrementalPotential = 49 | incremental::Dispersion, 50 | incremental::Whitening >; 51 | template 52 | using IncrementalPotentialWhiten = 53 | incremental::Dispersion, 54 | incremental::Whitening >; 55 | } // namespace EventEMin 56 | 57 | #endif // EVENT_EMIN_INCREMENTAL_POTENTIAL_H 58 | -------------------------------------------------------------------------------- /include/EventEMin/dispersion/incremental_dispersion/tsallis.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_TSALLIS_H 2 | #define EVENT_EMIN_INCREMENTAL_TSALLIS_H 3 | 4 | #include "EventEMin/dispersion/incremental_dispersion/dispersion.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Tsallis 12 | { 13 | public: 14 | typedef M Model; 15 | typedef typename Model::T T; 16 | 17 | enum 18 | { 19 | NVars = Model::NVars, 20 | NDims = Model::NDims 21 | }; 22 | 23 | typedef typename Model::Point Point; 24 | typedef typename Model::Vars Vars; 25 | typedef typename Model::CMatrix CMatrix; 26 | typedef typename Model::DMatrix DMatrix; 27 | typedef typename Model::PMatrix PMatrix; 28 | typedef Matrix WMatrix; 29 | 30 | private: 31 | const T alpha_; 32 | 33 | public: 34 | Tsallis(const T& alpha = T(2.0)) : alpha_(alpha) {} 35 | T 36 | alpha(void) const 37 | { 38 | return alpha_; 39 | } 40 | 41 | void 42 | operator()(const Ref& cmDiff, const Ref& dcmDiff, 43 | const Ref& cmgDiff, const Ref& perDiff, 44 | VarsEstimate& varse) const 45 | { 46 | const T cmDiffExp = 47 | computeExp(T(-0.5) * alpha() * (cmDiff.transpose() * cmDiff)(0)); 48 | varse.val += cmDiffExp; 49 | const WMatrix cWeight(cmDiffExp * dcmDiff.transpose()); 50 | varse.vNum.noalias() -= cWeight * cmgDiff; 51 | varse.vDen.noalias() += cWeight * perDiff; 52 | } 53 | }; 54 | } // namespace incremental 55 | 56 | template 57 | using IncrementalTsallis = 58 | incremental::Dispersion, 59 | incremental::Whitening >; 60 | template 61 | using IncrementalTsallisWhiten = 62 | incremental::Dispersion, 63 | incremental::Whitening >; 64 | } // namespace EventEMin 65 | 66 | #endif // EVENT_EMIN_INCREMENTAL_TSALLIS_H -------------------------------------------------------------------------------- /include/EventEMin/event.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_EVENT_ALL_H 2 | #define EVENT_EMIN_EVENT_ALL_H 3 | 4 | #include "EventEMin/event/conversion.h" 5 | #include "EventEMin/event/io.h" 6 | #include "EventEMin/event/show.h" 7 | #include "EventEMin/event/transform.h" 8 | #include "EventEMin/event/type.h" 9 | #include "EventEMin/event/undistort.h" 10 | 11 | #endif // EVENT_EMIN_EVENT_ALL_H 12 | -------------------------------------------------------------------------------- /include/EventEMin/event/conversion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_EVENT_CONVERSION_H 2 | #define EVENT_EMIN_EVENT_CONVERSION_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/event/type.h" 8 | #include "EventEMin/types_def.h" 9 | 10 | namespace EventEMin 11 | { 12 | template 13 | void 14 | events2cv(const Events& evs, const int width, const int height, 15 | CvMatrix& img) 16 | { 17 | assert(N >= 2); 18 | const int nEvents = static_cast(evs.size()); 19 | 20 | img.create(height, width, CV_TYPE(T, 1)); 21 | img.setTo(0.0); 22 | for (int k = 0; k < nEvents; ++k) 23 | { 24 | const int x = std::round(evs[k].c(0)); 25 | const int y = std::round(evs[k].c(1)); 26 | if (0 <= x && x < width && 0 <= y && y < height) 27 | { 28 | img.at(y, x) += static_cast(evs[k].polarity); 29 | } 30 | } 31 | } 32 | 33 | template 34 | void 35 | events2cv(const Events& evs, const Ref >& w, 36 | const int width, const int height, CvMatrix& img) 37 | { 38 | assert(N >= 2); 39 | const int nEvents = static_cast(evs.size()); 40 | assert(nEvents == w.size()); 41 | 42 | img.create(height, width, CV_TYPE(T, 1)); 43 | img.setTo(0.0); 44 | for (int k = 0; k < nEvents; ++k) 45 | { 46 | const int x = std::round(evs[k].c(0)); 47 | const int y = std::round(evs[k].c(1)); 48 | if (0 <= x && x < width && 0 <= y && y < height) 49 | { 50 | img.at(y, x) += w(k) * static_cast(evs[k].polarity); 51 | } 52 | } 53 | } 54 | 55 | template 56 | void 57 | event2eigen(const Event& ev, 58 | const DenseBase& c, typename Derived::Scalar& ts) 59 | { 60 | const_cast&>(c) << ev.c; 61 | ts = ev.ts; 62 | } 63 | 64 | template 65 | void 66 | event2eigen(const Event& ev, 67 | const DenseBase& c, typename Derived::Scalar& ts, 68 | int& polarity) 69 | { 70 | event2eigen(ev, c, ts); 71 | polarity = ev.polarity; 72 | } 73 | 74 | template 75 | void 76 | events2eigen(const Events& evs, Matrix& c, Vector& ts) 77 | { 78 | const int nEvents = static_cast(evs.size()); 79 | assert(nEvents > 0); 80 | 81 | c.resize(N, nEvents); 82 | ts.resize(nEvents); 83 | for (int k = 0; k < nEvents; ++k) 84 | { 85 | event2eigen(evs[k], c.col(k), ts(k)); 86 | } 87 | } 88 | 89 | template 90 | void 91 | events2eigen(const Events& evs, Matrix& c, Vector& ts, 92 | Vector& polarity) 93 | { 94 | const int nEvents = static_cast(evs.size()); 95 | assert(nEvents > 0); 96 | 97 | c.resize(N, nEvents); 98 | ts.resize(nEvents); 99 | polarity.resize(nEvents); 100 | for (int k = 0; k < nEvents; ++k) 101 | { 102 | event2eigen(evs[k], c.col(k), ts(k), polarity(k)); 103 | } 104 | } 105 | } // namespace EventEMin 106 | 107 | #endif // EVENT_EMIN_EVENT_CONVERSION_H 108 | -------------------------------------------------------------------------------- /include/EventEMin/event/show.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_EVENT_SHOW_H 2 | #define EVENT_EMIN_EVENT_SHOW_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "EventEMin/event/conversion.h" 9 | #include "EventEMin/event/type.h" 10 | #include "EventEMin/image/conversion.h" 11 | 12 | namespace EventEMin 13 | { 14 | template 15 | void 16 | showGray(const Events& evs, const int width, const int height, 17 | const std::string& imgName = "Image of Events") 18 | { 19 | assert(N >= 2); 20 | CvMatrix img; 21 | events2cv(evs, width, height, img); 22 | cv2gray(img); 23 | cv::imshow(imgName, img); 24 | } 25 | 26 | template 27 | void 28 | showGray(const Events& evs, const DenseBase& w, const int width, 29 | const int height, const std::string& imgName = "Image of Events") 30 | { 31 | assert(N >= 2); 32 | assert(static_cast(evs.size()) == w.size()); 33 | 34 | CvMatrix img; 35 | events2cv(evs, w, width, height, img); 36 | cv2gray(img); 37 | cv::imshow(imgName, img); 38 | } 39 | } // namespace EventEMin 40 | 41 | #endif // EVENT_EMIN_EVENT_SHOW_H 42 | -------------------------------------------------------------------------------- /include/EventEMin/event/transform.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_EVENT_TRANSFORM_H 2 | #define EVENT_EMIN_EVENT_TRANSFORM_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/types_def.h" 8 | 9 | namespace EventEMin 10 | { 11 | // partial specialisation only works for structs not functions 12 | template 13 | struct computeBoundaries 14 | { 15 | void 16 | operator()(const Ref >& c, const Array& cMin, 17 | const Array& cMax, Array& lMin, 18 | Array& lMax) const 19 | { 20 | for (I d = 0; d < N; ++d) 21 | { 22 | lMin[d] = std::max(static_cast(std::floor(c(d).value())), cMin[d]); 23 | lMax[d] = std::min(static_cast(std::ceil(c(d).value())), cMax[d]); 24 | } 25 | } 26 | }; 27 | template 28 | struct computeBoundaries 29 | { 30 | void 31 | operator()(const Ref >& c, const Array& cMin, 32 | const Array& cMax, Array& lMin, 33 | Array& lMax) const 34 | { 35 | for (I d = 0; d < N; ++d) 36 | { 37 | lMin[d] = std::max(static_cast(std::floor(c(d))), cMin[d]); 38 | lMax[d] = std::min(static_cast(std::ceil(c(d))), cMax[d]); 39 | } 40 | } 41 | }; 42 | template 43 | struct computeBoundaries 44 | { 45 | void 46 | operator()(const Ref >& c, const Array& cMin, 47 | const Array& cMax, Array& lMin, 48 | Array& lMax) const 49 | { 50 | for (I d = 0; d < N; ++d) 51 | { 52 | lMin[d] = std::max(static_cast(std::floor(c(d))), cMin[d]); 53 | lMax[d] = std::min(static_cast(std::ceil(c(d))), cMax[d]); 54 | } 55 | } 56 | }; 57 | 58 | template 59 | struct projectEvent; 60 | template 61 | struct projectEvent 62 | { 63 | void 64 | operator()(const Ref >& camParams, 65 | Ref > c) const 66 | { 67 | assert(c.size() >= 2); 68 | 69 | c(0) = camParams(0, 0) * c(0) + camParams(0, 2); 70 | c(1) = camParams(1, 1) * c(1) + camParams(1, 2); 71 | } 72 | void 73 | operator()(const Ref >& camParams, 74 | const Ref >& cIn, Ref > cOut) const 75 | { 76 | assert(cIn.size() >= 2); 77 | assert(cOut.size() >= 2); 78 | 79 | cOut(0) = camParams(0, 0) * cIn(0) + camParams(0, 2); 80 | cOut(1) = camParams(1, 1) * cIn(1) + camParams(1, 2); 81 | } 82 | }; 83 | template 84 | struct projectEvent 85 | { 86 | void 87 | operator()(const Ref >& camParams, 88 | Ref > c) const 89 | { 90 | assert(c.size() >= 3); 91 | 92 | c.template head<3>() = camParams * c.template head<3>(); 93 | c.template head<2>() /= c(2); 94 | } 95 | void 96 | operator()(const Ref >& camParams, 97 | const Ref >& cIn, Ref > cOut) const 98 | { 99 | assert(cIn.size() >= 3); 100 | assert(cOut.size() >= 3); 101 | 102 | cOut.template head<3>().noalias() = camParams * cIn.template head<3>(); 103 | cOut.template head<2>() /= cIn(2); 104 | } 105 | }; 106 | 107 | template 108 | struct projectEvents; 109 | template 110 | struct projectEvents 111 | { 112 | void 113 | operator()(const Ref >& camParams, 114 | Ref > c) const 115 | { 116 | assert(c.rows() >= 2); 117 | 118 | c.row(0) = camParams(0, 0) * c.row(0).array() + camParams(0, 2); 119 | c.row(1) = camParams(1, 1) * c.row(1).array() + camParams(1, 2); 120 | } 121 | void 122 | operator()(const Ref >& camParams, 123 | const Ref >& cIn, Ref > cOut) const 124 | { 125 | assert(cIn.rows() >= 2); 126 | assert(cOut.rows() >= 2); 127 | assert(cIn.cols() == cOut.cols()); 128 | 129 | cOut.row(0) = camParams(0, 0) * cIn.row(0).array() + camParams(0, 2); 130 | cOut.row(1) = camParams(1, 1) * cIn.row(1).array() + camParams(1, 2); 131 | } 132 | }; 133 | template 134 | struct projectEvents 135 | { 136 | void 137 | operator()(const Ref >& camParams, 138 | Ref > c) const 139 | { 140 | assert(c.rows() >= 3); 141 | 142 | c.template topRows<3>() = camParams * c.template topRows<3>(); 143 | c.template topRows<2>().rowwise().array() /= c.row(2).array(); 144 | } 145 | void 146 | operator()(const Ref >& camParams, 147 | const Ref >& cIn, Ref > cOut) const 148 | { 149 | assert(cIn.rows() >= 3); 150 | assert(cOut.rows() >= 3); 151 | assert(cIn.cols() == cOut.cols()); 152 | 153 | cOut.template topRows<3>().noalias() = 154 | camParams * cIn.template topRows<3>(); 155 | cOut.template topRows<2>().array().rowwise() /= cIn.row(2).array(); 156 | } 157 | }; 158 | 159 | template 160 | struct unprojectEvent; 161 | template 162 | struct unprojectEvent 163 | { 164 | void 165 | operator()(const Ref >& camParams, 166 | Ref > c) const 167 | { 168 | assert(c.size() >= 2); 169 | 170 | c(0) = (c(0) - camParams(0, 2)) / camParams(0, 0); 171 | c(1) = (c(1) - camParams(1, 2)) / camParams(1, 1); 172 | } 173 | void 174 | operator()(const Ref >& camParams, 175 | const Ref >& cIn, Ref > cOut) const 176 | { 177 | assert(cIn.size() >= 2); 178 | assert(cOut.size() >= 2); 179 | 180 | cOut(0) = (cIn(0) - camParams(0, 2)) / camParams(0, 0); 181 | cOut(1) = (cIn(1) - camParams(1, 2)) / camParams(1, 1); 182 | } 183 | }; 184 | template 185 | struct unprojectEvent 186 | { 187 | void 188 | operator()(const Ref >& camParams, 189 | Ref > c) const 190 | { 191 | assert(c.size() >= 3); 192 | 193 | c.template head<2>() *= c(2); 194 | c.template head<3>() = camParams.inverse() * c.template head<3>(); 195 | } 196 | void 197 | operator()(const Ref >& camParams, 198 | const Ref >& cIn, Ref > cOut) const 199 | { 200 | assert(cIn.size() >= 3); 201 | assert(cOut.size() >= 3); 202 | 203 | cOut.template head<3>() = cIn.template head<3>(); 204 | this->operator()(camParams, cOut); 205 | } 206 | }; 207 | 208 | template 209 | struct unprojectEvents; 210 | template 211 | struct unprojectEvents 212 | { 213 | void 214 | operator()(const Ref >& camParams, 215 | Ref > c) const 216 | { 217 | assert(c.rows() >= 2); 218 | 219 | c.row(0) = (c.row(0).array() - camParams(0, 2)) / camParams(0, 0); 220 | c.row(1) = (c.row(1).array() - camParams(1, 2)) / camParams(1, 1); 221 | } 222 | void 223 | operator()(const Ref >& camParams, 224 | const Ref >& cIn, Ref > cOut) const 225 | { 226 | assert(cIn.rows() >= 2); 227 | assert(cOut.rows() >= 2); 228 | assert(cIn.cols() == cOut.cols()); 229 | 230 | cOut.row(0) = (cIn.row(0).array() - camParams(0, 2)) / camParams(0, 0); 231 | cOut.row(1) = (cIn.row(1).array() - camParams(1, 2)) / camParams(1, 1); 232 | } 233 | }; 234 | template 235 | struct unprojectEvents 236 | { 237 | void 238 | operator()(const Ref >& camParams, 239 | Ref > c) const 240 | { 241 | assert(c.rows() >= 3); 242 | 243 | c.template topRows<2>().array().rowwise() *= c.row(2).array(); 244 | c.template topRows<3>() = camParams.inverse() * c.template topRows<3>(); 245 | } 246 | void 247 | operator()(const Ref >& camParams, 248 | const Ref >& cIn, Ref > cOut) const 249 | { 250 | assert(cIn.rows() >= 3); 251 | assert(cOut.rows() >= 3); 252 | assert(cIn.cols() == cOut.cols()); 253 | 254 | cOut.template topRows<3>() = cIn.template topRows<3>(); 255 | this->operator()(camParams, cOut); 256 | } 257 | }; 258 | } // namespace EventEMin 259 | 260 | #endif // EVENT_EMIN_EVENT_TRANSFORM_H 261 | -------------------------------------------------------------------------------- /include/EventEMin/event/type.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_EVENT_TYPE_H 2 | #define EVENT_EMIN_EVENT_TYPE_H 3 | 4 | #include 5 | 6 | #include "EventEMin/types_def.h" 7 | 8 | namespace EventEMin 9 | { 10 | template 11 | struct Event 12 | { 13 | Vector c; 14 | T ts; 15 | int polarity; 16 | 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | 19 | Event(void) {} 20 | Event(const Ref >& c, const T& ts, const int polarity) 21 | : c(c), ts(ts), polarity(polarity) 22 | { 23 | } 24 | 25 | friend std::ostream& 26 | operator<<(std::ostream& os, const Event& ev) 27 | { 28 | return os << ev.ts << ' ' << ev.c.transpose() << ' ' << ev.polarity; 29 | } 30 | friend std::istream& 31 | operator>>(std::istream& is, Event& ev) 32 | { 33 | is >> ev.ts; 34 | for (int i = 0; i < ev.c.size(); ++i) 35 | { 36 | is >> ev.c(i); 37 | } 38 | is >> ev.polarity; 39 | if (ev.polarity == 0) 40 | { 41 | ev.polarity = -1; 42 | } 43 | return is; 44 | } 45 | }; 46 | 47 | template 48 | using Events = StdVector >; 49 | } // namespace EventEMin 50 | 51 | #endif // EVENT_EMIN_EVENT_TYPE_H 52 | -------------------------------------------------------------------------------- /include/EventEMin/event/undistort.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_EVENT_UNDISTORT_H 2 | #define EVENT_EMIN_EVENT_UNDISTORT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "EventEMin/event/type.h" 10 | #include "EventEMin/types_def.h" 11 | 12 | namespace EventEMin 13 | { 14 | template 15 | void 16 | undistort(const int xMin, const int xMax, const int yMin, const int yMax, 17 | const CvMatrix& map, const Events& evs, Events& uevs) 18 | { 19 | const int nEvents = static_cast(evs.size()); 20 | assert(N >= 2); 21 | 22 | uevs.clear(); 23 | uevs.reserve(evs.size()); 24 | 25 | Event ev; 26 | for (int k = 0; k < nEvents; ++k) 27 | { 28 | CvVector mapPoint = map.at >(std::round(evs[k].c(1)), 29 | std::round(evs[k].c(0))); 30 | const int x = std::round(mapPoint[0]); 31 | const int y = std::round(mapPoint[1]); 32 | if (xMin <= x && x < xMax && yMin <= y && y < yMax) 33 | { 34 | ev = evs[k]; 35 | ev.c(0) = mapPoint[0]; 36 | ev.c(1) = mapPoint[1]; 37 | uevs.push_back(ev); 38 | } 39 | } 40 | } 41 | } // namespace EventEMin 42 | 43 | #endif // EVENT_EMIN_EVENT_UNDISTORT_H 44 | -------------------------------------------------------------------------------- /include/EventEMin/gauss_kernel.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_GAUSS_KERNEL_H 2 | #define EVENT_EMIN_GAUSS_KERNEL_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/types_def.h" 8 | 9 | namespace EventEMin 10 | { 11 | namespace kernel 12 | { 13 | template 14 | T 15 | gauss(const T& x) 16 | { 17 | const T val = x * x; 18 | return std::exp(-T(0.5) * val); 19 | } 20 | template 21 | T 22 | gauss(const T& x, const T& sigmaInv) 23 | { 24 | const T val = x * sigmaInv * x; 25 | return std::exp(-T(0.5) * val); 26 | } 27 | template 28 | T 29 | gauss(const Ref >& x) 30 | { 31 | const T val = x.transpose() * x; 32 | return std::exp(-T(0.5) * val); 33 | } 34 | template 35 | T 36 | gauss(const Ref >& x, 37 | const Ref >& sigmaInv) 38 | { 39 | const T val = x.transpose() * (sigmaInv.asDiagonal() * x); 40 | return std::exp(-T(0.5) * val); 41 | } 42 | template 43 | T 44 | gauss(const Ref >& x) 45 | { 46 | const T val = x.transpose() * x; 47 | return std::exp(-T(0.5) * val); 48 | } 49 | template 50 | T 51 | gauss(const Ref >& x, const Ref >& sigmaInv) 52 | { 53 | assert(x.size() == sigmaInv.size()); 54 | const T val = x.transpose() * (sigmaInv.asDiagonal() * x); 55 | return std::exp(-T(0.5) * val); 56 | } 57 | 58 | template 59 | void 60 | gaussKernelIterate(const int ksize, const int hksize, const int d, Vector& p, 61 | Array& ind, Tensor& kernel) 62 | { 63 | if (d >= N) 64 | { 65 | kernel(ind) = gauss(p); 66 | return; 67 | } 68 | 69 | for (ind[d] = 0; ind[d] < ksize; ++ind[d]) 70 | { 71 | p(d) = ind[d] - hksize; 72 | gaussKernelIterate(ksize, hksize, d + 1, p, ind, kernel); 73 | } 74 | } 75 | template 76 | void 77 | gaussKernel(const int ksize, Tensor& kernel) 78 | { 79 | Array sizeArray; 80 | sizeArray.fill(ksize); 81 | kernel.resize(sizeArray); 82 | const int hksize = ksize >> 1; 83 | Vector p(N); 84 | Array ind; 85 | gaussKernelIterate(ksize, hksize, 0, p, ind, kernel); 86 | sizeArray.fill(hksize); 87 | kernel = kernel / (kernel(sizeArray) * std::pow(static_cast(2.0 * M_PI), 88 | static_cast(N * 0.5))); 89 | } 90 | template 91 | void 92 | gaussKernel(const int ksize, Vector& kernel) 93 | { 94 | kernel.resize(ksize); 95 | const int hksize = ksize >> 1; 96 | T p; 97 | for (int i = 0; i < ksize; ++i) 98 | { 99 | p = i - hksize; 100 | kernel(i) = gauss(p); 101 | } 102 | kernel /= kernel(hksize, hksize) * std::sqrt(static_cast(2.0 * M_PI)); 103 | } 104 | template 105 | void 106 | gaussKernel(const int ksize, Matrix& kernel) 107 | { 108 | kernel.resize(ksize, ksize); 109 | const int hksize = ksize >> 1; 110 | Vector p; 111 | for (int i = 0; i < ksize; ++i) 112 | { 113 | p(0) = i - hksize; 114 | for (int j = 0; j < ksize; ++j) 115 | { 116 | p(1) = j - hksize; 117 | kernel(i, j) = gauss(p); 118 | } 119 | } 120 | kernel /= kernel(hksize, hksize) * static_cast(2.0 * M_PI); 121 | } 122 | 123 | template 124 | void 125 | gaussKernelIterate(const int ksize, const int hksize, 126 | const Ref >& sigmaInv, const int d, 127 | Vector& p, Array& ind, Tensor& kernel) 128 | { 129 | if (d >= N) 130 | { 131 | kernel(ind) = gauss(p, sigmaInv); 132 | return; 133 | } 134 | 135 | for (ind[d] = 0; ind[d] < ksize; ++ind[d]) 136 | { 137 | p(d) = ind[d] - hksize; 138 | gaussKernelIterate(ksize, hksize, sigmaInv, d + 1, p, ind, kernel); 139 | } 140 | } 141 | template 142 | void 143 | gaussKernel(const int ksize, const Ref >& sigmaInv, 144 | const T& sigmaDet, Tensor& kernel) 145 | { 146 | assert(sigmaInv.size() == N); 147 | 148 | Array sizeArray; 149 | sizeArray.fill(ksize); 150 | kernel.resize(sizeArray); 151 | const int hksize = ksize >> 1; 152 | Vector p(N); 153 | Array ind; 154 | gaussKernelIterate(ksize, hksize, sigmaInv, 0, p, ind, kernel); 155 | sizeArray.fill(hksize); 156 | kernel = 157 | kernel / (kernel(sizeArray) * 158 | std::pow(static_cast(2.0 * M_PI), static_cast(N * 0.5)) * 159 | std::sqrt(sigmaDet)); 160 | } 161 | template 162 | void 163 | gaussKernel(const int ksize, const T& sigmaInv, const T& sigmaDet, 164 | Vector& kernel) 165 | { 166 | kernel.resize(ksize); 167 | const int hksize = ksize >> 1; 168 | T p; 169 | for (int i = 0; i < ksize; ++i) 170 | { 171 | p = i - hksize; 172 | kernel(i) = gauss(p, sigmaInv); 173 | } 174 | kernel /= 175 | kernel(hksize, hksize) * static_cast(2.0 * M_PI) * std::sqrt(sigmaDet); 176 | } 177 | template 178 | void 179 | gaussKernel(const int ksize, const Ref >& sigmaInv, 180 | const T& sigmaDet, Matrix& kernel) 181 | { 182 | assert(sigmaInv.size() == 2); 183 | 184 | kernel.resize(ksize, ksize); 185 | const int hksize = ksize >> 1; 186 | Vector p; 187 | for (int i = 0; i < ksize; ++i) 188 | { 189 | p(0) = i - hksize; 190 | for (int j = 0; j < ksize; ++j) 191 | { 192 | p(1) = j - hksize; 193 | kernel(i, j) = gauss(p, sigmaInv); 194 | } 195 | } 196 | kernel /= 197 | kernel(hksize, hksize) * static_cast(2.0 * M_PI) * std::sqrt(sigmaDet); 198 | } 199 | } // namespace kernel 200 | } // namespace EventEMin 201 | 202 | #endif // EVENT_EMIN_GAUSS_KERNEL_H 203 | -------------------------------------------------------------------------------- /include/EventEMin/image.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_IMAGE_ALL_H 2 | #define EVENT_EMIN_IMAGE_ALL_H 3 | 4 | #include "EventEMin/image/conversion.h" 5 | #include "EventEMin/image/show.h" 6 | #include "EventEMin/image/undistort.h" 7 | 8 | #endif // EVENT_EMIN_IMAGE_ALL_H 9 | -------------------------------------------------------------------------------- /include/EventEMin/image/conversion.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_IMAGE_CONVERSION_H 2 | #define EVENT_EMIN_IMAGE_CONVERSION_H 3 | 4 | #include 5 | 6 | #include "EventEMin/types_def.h" 7 | 8 | namespace EventEMin 9 | { 10 | template 11 | void 12 | eigen2cv(const DenseBase& c, const int width, const int height, 13 | CvMatrix& img, const double inc = -1.0) 14 | { 15 | assert(c.rows() >= 2); 16 | 17 | typedef typename Derived::Scalar T; 18 | 19 | img.create(height, width, CV_TYPE(T, 1)); 20 | img.setTo(0.0); 21 | for (int k = 0; k < c.cols(); ++k) 22 | { 23 | const int x = std::round(c(0, k)); 24 | const int y = std::round(c(1, k)); 25 | if (0 <= x && x < width && 0 <= y && y < height) 26 | { 27 | img.at(y, x) += static_cast(inc); 28 | } 29 | } 30 | } 31 | 32 | template 33 | void 34 | eigen2cv(const DenseBase& c, const DenseBase& w, 35 | const int width, const int height, CvMatrix& img) 36 | { 37 | assert(c.rows() >= 2); 38 | assert(c.cols() == w.size()); 39 | 40 | typedef typename DerivedC::Scalar T; 41 | 42 | img.create(height, width, CV_TYPE(T, 1)); 43 | img.setTo(0.0); 44 | for (int k = 0; k < c.cols(); ++k) 45 | { 46 | const int x = std::round(c(0, k)); 47 | const int y = std::round(c(1, k)); 48 | if (0 <= x && x < width && 0 <= y && y < height) 49 | { 50 | img.at(y, x) += static_cast(w(k)); 51 | } 52 | } 53 | } 54 | 55 | template 56 | void 57 | eigen2cv(const DenseBase& c, const StdVector& colour, 58 | const int width, const int height, CvMatrix& img) 59 | { 60 | assert(2 <= c.rows()); 61 | assert(c.cols() == static_cast(colour.size())); 62 | 63 | img.create(height, width, CV_8UC3); 64 | img.setTo(255); 65 | for (int k = 0; k < c.cols(); ++k) 66 | { 67 | const int x = std::round(c(0, k)); 68 | const int y = std::round(c(1, k)); 69 | if (0 <= x && x < width && 0 <= y && y < height) 70 | { 71 | // img.at(y, x) = colour[k]; 72 | cv::circle(img, cv::Point(x, y), 1, colour[k], 1); 73 | } 74 | } 75 | } 76 | 77 | void 78 | cv2gray(const CvMatrix& img, CvMatrix& imgGray) 79 | { 80 | double min, max; 81 | cv::minMaxLoc(img, &min, &max); 82 | const double a = 255.0 / (max - min + 1.0e-32), b = -a * min; 83 | img.convertTo(imgGray, CV_8UC1, a, b); 84 | } 85 | 86 | void 87 | cv2gray(CvMatrix& img) 88 | { 89 | cv2gray(img, img); 90 | } 91 | 92 | void 93 | flow2colour(const CvMatrix& flowX, const CvMatrix& flowY, CvMatrix& bgr, 94 | const double& thresh = 5.0, const double& norm = 1.0) 95 | { 96 | CvMatrix magnitude, angle, magnNorm; 97 | cv::cartToPolar(flowX, flowY, magnitude, angle, true); 98 | cv::threshold(magnitude, magnitude, thresh, thresh, cv::THRESH_TRUNC); 99 | cv::normalize(magnitude, magnNorm, 0.0, norm, cv::NORM_MINMAX); 100 | angle *= 180.0 / (360.0 * 255.0); 101 | // build hsv image 102 | CvMatrix _hsv[3], hsv, hsv8; 103 | _hsv[0] = angle; 104 | _hsv[1] = CvMatrix::ones(angle.size(), CV_32F); 105 | _hsv[2] = magnNorm; 106 | cv::merge(_hsv, 3, hsv); 107 | hsv.convertTo(hsv8, CV_8U, 255.0); 108 | cv::cvtColor(hsv8, bgr, cv::COLOR_HSV2BGR); 109 | } 110 | 111 | void 112 | flow2colour(const CvMatrix& flow, CvMatrix& bgr, const double& thresh = 5.0, 113 | const double& norm = 1.0) 114 | { 115 | CvMatrix flowParts[2]; 116 | cv::split(flow, flowParts); 117 | flow2colour(flowParts[0], flowParts[1], bgr, thresh, norm); 118 | } 119 | } // namespace EventEMin 120 | 121 | #endif // EVENT_EMIN_IMAGE_CONVERSION_H 122 | -------------------------------------------------------------------------------- /include/EventEMin/image/show.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_IMAGE_SHOW_H 2 | #define EVENT_EMIN_IMAGE_SHOW_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "EventEMin/image/conversion.h" 9 | #include "EventEMin/types_def.h" 10 | 11 | namespace EventEMin 12 | { 13 | template 14 | void 15 | showColour(const DenseBase& c, const StdVector& colour, 16 | const int width, const int height, CvMatrix& img, 17 | const std::string& imgName = "Image of Points") 18 | { 19 | assert(2 <= c.rows()); 20 | assert(c.cols() == static_cast(colour.size())); 21 | 22 | eigen2cv(c, colour, width, height, img); 23 | cv::imshow(imgName, img); 24 | } 25 | 26 | template 27 | void 28 | showColour(const DenseBase& c, const StdVector& colour, 29 | const int width, const int height, 30 | const std::string& imgName = "Image of Points") 31 | { 32 | CvMatrix img; 33 | showColour(c, colour, width, height, img, imgName); 34 | } 35 | 36 | template 37 | void 38 | showGray(const DenseBase& c, const int width, const int height, 39 | const std::string& imgName = "Image of Points") 40 | { 41 | assert(c.rows() >= 2); 42 | 43 | CvMatrix img; 44 | eigen2cv(c, width, height, img); 45 | cv2gray(img); 46 | cv::imshow(imgName, img); 47 | } 48 | 49 | template 50 | void 51 | showGray(const DenseBase& c, const DenseBase& w, 52 | const int width, const int height, 53 | const std::string& imgName = "Image of Points") 54 | { 55 | assert(c.rows() >= 2); 56 | assert(c.cols() == w.size()); 57 | 58 | CvMatrix img; 59 | eigen2cv(c, w, width, height, img); 60 | cv2gray(img); 61 | cv::imshow(imgName, img); 62 | } 63 | 64 | template 65 | void 66 | showGray(const DenseBase& c, 67 | const std::string& imgName = "Image of Points") 68 | { 69 | typedef typename Derived::Scalar T; 70 | 71 | const CvMatrix img(c.cols(), c.rows(), CV_TYPE(T, 1), const_cast(&c(0))); 72 | CvMatrix imgGray; 73 | cv2gray(img, imgGray); 74 | cv::imshow(imgName, imgGray); 75 | } 76 | 77 | void 78 | showOpticalFlowDirection(const int width, const int height, 79 | const std::string& imgName = "Optical Flow Direction") 80 | { 81 | const int w = (width >> 1) - 1, h = (height >> 1) - 1; 82 | 83 | CvMatrix flow[2]; 84 | flow[0].create(height, width, CV_32F); 85 | flow[1].create(height, width, CV_32F); 86 | for (int j = 0; j < height; ++j) 87 | { 88 | for (int i = 0; i < width; ++i) 89 | { 90 | flow[0].at(j, i) = static_cast(w - i); 91 | flow[1].at(j, i) = static_cast(h - j); 92 | } 93 | } 94 | 95 | CvMatrix bgr; 96 | flow2colour(flow[0], flow[1], bgr, 1.0e6); 97 | cv::imshow(imgName, bgr); 98 | } 99 | } // namespace EventEMin 100 | 101 | #endif // EVENT_EMIN_IMAGE_SHOW_H 102 | -------------------------------------------------------------------------------- /include/EventEMin/image/undistort.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_IMAGE_UNDISTORT_H 2 | #define EVENT_EMIN_IMAGE_UNDISTORT_H 3 | 4 | #include "EventEMin/types_def.h" 5 | 6 | namespace EventEMin 7 | { 8 | template 9 | void 10 | initUndistort(const int width, const int height, const CvMatrix& camParams, 11 | const CvMatrix& distCoeffs, CvMatrix& map) 12 | { 13 | const int size = width * height; 14 | 15 | CvMatrix points(size, 1, CV_TYPE(T, 2)); 16 | for (int y = 0; y < height; ++y) 17 | { 18 | const int yy = y * width; 19 | for (int x = 0; x < width; ++x) 20 | { 21 | points.at >(yy + x) = CvVector(x, y); 22 | } 23 | } 24 | 25 | CvMatrix mapPoints(size, 1, CV_TYPE(T, 2)); 26 | cv::undistortPoints(points, mapPoints, camParams, distCoeffs, cv::noArray(), 27 | camParams); 28 | 29 | map.create(height, width, CV_TYPE(T, 2)); 30 | for (int y = 0; y < height; ++y) 31 | { 32 | const int yy = y * width; 33 | for (int x = 0; x < width; ++x) 34 | { 35 | map.at >(y, x) = mapPoints.at >(yy + x); 36 | } 37 | } 38 | } 39 | } // namespace EventEMin 40 | 41 | #endif // EVENT_EMIN_IMAGE_UNDISTORT_H 42 | -------------------------------------------------------------------------------- /include/EventEMin/model.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_MODEL_ALL_H 2 | #define EVENT_EMIN_MODEL_ALL_H 3 | 4 | // in 2D (image) space 5 | #include "EventEMin/model/model/affinity.h" 6 | #include "EventEMin/model/model/homography.h" 7 | #include "EventEMin/model/model/isometry.h" 8 | #include "EventEMin/model/model/rotation.h" 9 | #include "EventEMin/model/model/similarity.h" 10 | #include "EventEMin/model/model/translation.h" 11 | #include "EventEMin/model/model/translation2d.h" 12 | #include "EventEMin/model/model/translation_normal.h" 13 | // in 2D (image) space incremental 14 | #include "EventEMin/model/incremental_model/affinity.h" 15 | #include "EventEMin/model/incremental_model/isometry.h" 16 | #include "EventEMin/model/incremental_model/rotation.h" 17 | #include "EventEMin/model/incremental_model/similarity.h" 18 | #include "EventEMin/model/incremental_model/translation2d.h" 19 | #include "EventEMin/model/incremental_model/translation_normal.h" 20 | 21 | // in 3D space 22 | #include "EventEMin/model/model/six_dof.h" 23 | #include "EventEMin/model/model/translation3d.h" 24 | // in 3D space incremental 25 | #include "EventEMin/model/incremental_model/six_dof.h" 26 | #include "EventEMin/model/incremental_model/translation3d.h" 27 | 28 | #endif // EVENT_EMIN_MODEL_ALL_H 29 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_MODEL_H 2 | #define EVENT_EMIN_INCREMENTAL_MODEL_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/types_def.h" 8 | 9 | namespace EventEMin 10 | { 11 | namespace incremental 12 | { 13 | template 14 | class Model 15 | { 16 | public: 17 | typedef F Func; 18 | typedef typename Func::T T; 19 | typedef typename Func::Point Point; 20 | typedef typename Func::PointHomogeneous PointHomogeneous; 21 | typedef typename Func::Vars Vars; 22 | typedef typename Func::CMatrix CMatrix; 23 | typedef typename Func::DMatrix DMatrix; 24 | typedef typename Func::PMatrix PMatrix; 25 | typedef typename Func::GMatrix GMatrix; 26 | typedef typename Func::TMatrix TMatrix; 27 | 28 | enum 29 | { 30 | NVars = Func::NVars, 31 | NDims = Func::NDims, 32 | NMatrix = Func::NMatrix, 33 | }; 34 | 35 | protected: 36 | Func func_; 37 | 38 | public: 39 | Model(void) = default; 40 | 41 | void 42 | operator()(const Vars& vars, const Ref >& c, 43 | const Ref >& ts, Matrix& cm) const 44 | { 45 | assert(c.rows() == NDims); 46 | assert(cm.rows() == NDims); 47 | assert(c.cols() == cm.cols()); 48 | assert(c.cols() == ts.size()); 49 | 50 | const Vector t(ts.array() - ts(0)); 51 | for (int k = 0; k < c.cols(); ++k) 52 | { 53 | (*this)(vars, c.col(k), t(k), cm.col(k)); 54 | } 55 | } 56 | 57 | void 58 | operator()(const Ref& vars, const Ref& c, const T& t, 59 | Ref cm) const 60 | { 61 | func_(vars, c, t, cm); 62 | } 63 | 64 | void 65 | operator()(const Ref& vars, const Ref& c, const T& t, 66 | Ref cm, Ref dcm, Ref cg, 67 | Ref pMtx) const 68 | { 69 | func_(vars, c, t, cm, dcm, cg, pMtx); 70 | } 71 | 72 | void 73 | operator()(const Ref& vars, const Ref& c, const T& t, 74 | Ref cm, Ref dcm, Ref dc) const 75 | { 76 | func_(vars, c, t, cm, dcm, dc); 77 | } 78 | 79 | void 80 | transformation(const Ref& varst, Ref tMatrix) const 81 | { 82 | func_.transformation(varst, tMatrix); 83 | } 84 | }; 85 | } // namespace incremental 86 | } // namespace EventEMin 87 | 88 | #endif // EVENT_EMIN_INCREMENTAL_MODEL_H 89 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/affinity.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_AFFINITY_H 2 | #define EVENT_EMIN_INCREMENTAL_AFFINITY_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Affinity 12 | { 13 | enum 14 | { 15 | NWt = 1, 16 | NWp = 1, 17 | NL = 2, 18 | NV = 2, 19 | NVars = NWt + NWp + NL + NV, 20 | NDims = 2, 21 | NMatrix = 3 22 | }; 23 | 24 | typedef Scalar T; 25 | typedef Vector Point; 26 | typedef Vector PointHomogeneous; 27 | typedef Vector Vars; 28 | typedef Matrix CMatrix; 29 | typedef Matrix DMatrix; 30 | typedef Matrix PMatrix; 31 | typedef Matrix GMatrix; 32 | typedef Matrix TMatrix; 33 | 34 | Affinity(void) = default; 35 | 36 | void 37 | operator()(const Ref& vars, const Ref& c, const T& t, 38 | Ref cm) const 39 | { 40 | const Vars varst(t * vars); 41 | 42 | GMatrix gMatrix; 43 | generator(varst, gMatrix); 44 | 45 | PointHomogeneous ch; 46 | ch << c, T(1.0); 47 | 48 | // transformation 49 | transformation(gMatrix, ch, cm); 50 | } 51 | 52 | void 53 | operator()(const Ref& vars, const Ref& c, const T& t, 54 | Ref cm, Ref dcm, Ref cg, 55 | Ref pMatrix) const 56 | { 57 | const Vars varst(t * vars); 58 | 59 | GMatrix gMatrix; 60 | generator(varst, gMatrix); 61 | 62 | PointHomogeneous ch; 63 | ch << c, T(1.0); 64 | 65 | // transformation 66 | transformation(gMatrix, ch, cm); 67 | 68 | // generator 69 | generator(gMatrix, ch, cg); 70 | 71 | // derivatives wrt to vars 72 | dtransformationdvars(ch, t, dcm); 73 | 74 | // perturbation 75 | perturbation(c, t, pMatrix); 76 | } 77 | 78 | void 79 | operator()(const Ref& vars, const Ref& c, const T& t, 80 | Ref cm, Ref dcm, Ref dc) const 81 | { 82 | const Vars varst(t * vars); 83 | 84 | GMatrix gMatrix; 85 | generator(varst, gMatrix); 86 | 87 | PointHomogeneous ch; 88 | ch << c, T(1.0); 89 | 90 | // transformation 91 | TMatrix tMatrix; 92 | transformation(gMatrix, ch, cm, tMatrix); 93 | 94 | // derivatives wrt to vars 95 | dtransformationdvars(ch, t, dcm); 96 | 97 | // derivatives wrt to c 98 | dc = tMatrix.template topLeftCorner(); 99 | } 100 | 101 | void 102 | generator(const Ref& gMatrix, 103 | const Ref& ch, Ref cg) const 104 | { 105 | cg.noalias() = gMatrix.template topRows() * ch; 106 | } 107 | 108 | void 109 | generator(const Ref& varst, Ref gMatrix) const 110 | { 111 | gMatrix << varst(1) + varst(2), varst(3) - varst(0), varst(4), 112 | varst(3) + varst(0), varst(1) - varst(2), varst(5), T(0.0), T(0.0), 113 | T(0.0); 114 | } 115 | 116 | void 117 | dgeneratordw(const T& t, Ref dgMatrix) const 118 | { 119 | dgMatrix << T(0.0), -t, T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 120 | } 121 | 122 | void 123 | dgeneratords(const T& t, Ref dgMatrix) const 124 | { 125 | dgMatrix << t, T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0); 126 | } 127 | 128 | void 129 | dgeneratordh(const T& t, Ref dgMatrix) const 130 | { 131 | dgMatrix << t, T(0.0), T(0.0), T(0.0), -t, T(0.0), T(0.0), T(0.0), T(0.0); 132 | } 133 | 134 | void 135 | dgeneratordr(const T& t, Ref dgMatrix) const 136 | { 137 | dgMatrix << T(0.0), t, T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 138 | } 139 | 140 | void 141 | dgeneratordvx(const T& t, Ref dgMatrix) const 142 | { 143 | dgMatrix << T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 144 | T(0.0); 145 | } 146 | 147 | void 148 | dgeneratordvy(const T& t, Ref dgMatrix) const 149 | { 150 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), 151 | T(0.0); 152 | } 153 | 154 | void 155 | perturbation(const Ref& c, const T& t, 156 | Ref pMatrix) const 157 | { 158 | const Point ct(t * c); 159 | pMatrix << -ct(1), ct(0), ct(0), ct(1), t, T(0.0), ct(0), ct(1), -ct(1), 160 | ct(0), T(0.0), t; 161 | } 162 | 163 | void 164 | transformation(const Ref& gMatrix, 165 | const Ref& ch, Ref cm) const 166 | { 167 | TMatrix tMatrix; 168 | transformation(gMatrix, ch, cm, tMatrix); 169 | } 170 | 171 | void 172 | transformation(const Ref& gMatrix, 173 | const Ref& ch, Ref cm, 174 | Ref tMatrix) const 175 | { 176 | transformationG(gMatrix, tMatrix); 177 | cm.noalias() = tMatrix.template topRows() * ch; 178 | } 179 | 180 | void 181 | transformation(const Ref& varst, Ref tMatrix) const 182 | { 183 | GMatrix gMatrix; 184 | generator(varst, gMatrix); 185 | transformationG(gMatrix, tMatrix); 186 | } 187 | 188 | void 189 | transformationG(const Ref& gMatrix, Ref tMatrix) const 190 | { 191 | // 1st order Taylor series expansion 192 | tMatrix.setIdentity(); 193 | tMatrix += gMatrix; 194 | } 195 | 196 | void 197 | dtransformationdvars(const Ref& ch, const T& t, 198 | Ref dcm) const 199 | { 200 | // derivatives wrt to vars 201 | GMatrix dgMatrix; 202 | dgeneratordw(t, dgMatrix); 203 | dtransformationdvars(dgMatrix, ch, dcm.col(0)); 204 | dgeneratords(t, dgMatrix); 205 | dtransformationdvars(dgMatrix, ch, dcm.col(1)); 206 | dgeneratordh(t, dgMatrix); 207 | dtransformationdvars(dgMatrix, ch, dcm.col(2)); 208 | dgeneratordr(t, dgMatrix); 209 | dtransformationdvars(dgMatrix, ch, dcm.col(3)); 210 | dgeneratordvx(t, dgMatrix); 211 | dtransformationdvars(dgMatrix, ch, dcm.col(4)); 212 | dgeneratordvy(t, dgMatrix); 213 | dtransformationdvars(dgMatrix, ch, dcm.col(5)); 214 | } 215 | 216 | void 217 | dtransformationdvars(const Ref& dgMatrix, 218 | const Ref& ch, 219 | Ref dcm) const 220 | { 221 | dcm.noalias() = dgMatrix.template topRows() * ch; 222 | } 223 | }; 224 | } // namespace incremental 225 | 226 | template 227 | using IncrementalAffinity = incremental::Model >; 228 | } // namespace EventEMin 229 | 230 | #endif // EVENT_EMIN_INCREMENTAL_AFFINITY_H 231 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/isometry.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_ISOMETRY_H 2 | #define EVENT_EMIN_INCREMENTAL_ISOMETRY_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Isometry 12 | { 13 | enum 14 | { 15 | NW = 1, 16 | NV = 2, 17 | NVars = NW + NV, 18 | NDims = 2, 19 | NMatrix = 3 20 | }; 21 | 22 | typedef Scalar T; 23 | typedef Vector Point; 24 | typedef Vector PointHomogeneous; 25 | typedef Vector Vars; 26 | typedef Matrix CMatrix; 27 | typedef Matrix DMatrix; 28 | typedef Matrix PMatrix; 29 | typedef Matrix GMatrix; 30 | typedef Matrix TMatrix; 31 | 32 | Isometry(void) = default; 33 | 34 | void 35 | operator()(const Ref& vars, const Ref& c, const T& t, 36 | Ref cm) const 37 | { 38 | const Vars varst(t * vars); 39 | 40 | GMatrix gMatrix; 41 | generator(varst, gMatrix); 42 | 43 | PointHomogeneous ch; 44 | ch << c, T(1.0); 45 | 46 | // transformation 47 | transformation(gMatrix, ch, cm); 48 | } 49 | 50 | void 51 | operator()(const Ref& vars, const Ref& c, const T& t, 52 | Ref cm, Ref dcm, Ref cg, 53 | Ref pMatrix) const 54 | { 55 | const Vars varst(t * vars); 56 | 57 | GMatrix gMatrix; 58 | generator(varst, gMatrix); 59 | 60 | PointHomogeneous ch; 61 | ch << c, T(1.0); 62 | 63 | // transformation 64 | transformation(gMatrix, ch, cm); 65 | 66 | // generator 67 | generator(gMatrix, ch, cg); 68 | 69 | // derivatives wrt to vars 70 | dtransformationdvars(ch, t, dcm); 71 | 72 | // perturbation 73 | perturbation(c, t, pMatrix); 74 | } 75 | 76 | void 77 | operator()(const Ref& vars, const Ref& c, const T& t, 78 | Ref cm, Ref dcm, Ref dc) const 79 | { 80 | const Vars varst(t * vars); 81 | 82 | GMatrix gMatrix; 83 | generator(varst, gMatrix); 84 | 85 | PointHomogeneous ch; 86 | ch << c, T(1.0); 87 | 88 | // transformation 89 | TMatrix tMatrix; 90 | transformation(gMatrix, ch, cm, tMatrix); 91 | 92 | // derivatives wrt to vars 93 | dtransformationdvars(ch, t, dcm); 94 | 95 | // derivatives wrt to c 96 | dc = tMatrix.template topLeftCorner(); 97 | } 98 | 99 | void 100 | generator(const Ref& gMatrix, 101 | const Ref& ch, Ref cg) const 102 | { 103 | cg.noalias() = gMatrix.template topRows() * ch; 104 | } 105 | 106 | void 107 | generator(const Ref& varst, Ref gMatrix) const 108 | { 109 | gMatrix << T(0.0), -varst(0), varst(1), varst(0), T(0.0), varst(2), T(0.0), 110 | T(0.0), T(0.0); 111 | } 112 | 113 | void 114 | dgeneratordw(const T& t, Ref dgMatrix) const 115 | { 116 | dgMatrix << T(0.0), -t, T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 117 | } 118 | 119 | void 120 | dgeneratordvx(const T& t, Ref dgMatrix) const 121 | { 122 | dgMatrix << T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 123 | T(0.0); 124 | } 125 | 126 | void 127 | dgeneratordvy(const T& t, Ref dgMatrix) const 128 | { 129 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), 130 | T(0.0); 131 | } 132 | 133 | void 134 | perturbation(const Ref& c, const T& t, 135 | Ref pMatrix) const 136 | { 137 | const Point ct(t * c); 138 | pMatrix << -ct(1), t, T(0.0), ct(0), T(0.0), t; 139 | } 140 | 141 | void 142 | transformation(const Ref& gMatrix, 143 | const Ref& ch, Ref cm) const 144 | { 145 | TMatrix tMatrix; 146 | return transformation(gMatrix, ch, cm, tMatrix); 147 | } 148 | 149 | void 150 | transformation(const Ref& gMatrix, 151 | const Ref& ch, Ref cm, 152 | Ref tMatrix) const 153 | { 154 | transformationG(gMatrix, tMatrix); 155 | cm.noalias() = tMatrix.template topRows() * ch; 156 | } 157 | 158 | void 159 | transformation(const Ref& varst, Ref tMatrix) const 160 | { 161 | GMatrix gMatrix; 162 | generator(varst, gMatrix); 163 | transformationG(gMatrix, tMatrix); 164 | } 165 | 166 | void 167 | transformationG(const Ref& gMatrix, Ref tMatrix) const 168 | { 169 | // 1st order Taylor series expansion 170 | tMatrix.setIdentity(); 171 | tMatrix += gMatrix; 172 | } 173 | 174 | void 175 | dtransformationdvars(const Ref& ch, const T& t, 176 | Ref dcm) const 177 | { 178 | GMatrix dgMatrix; 179 | dgeneratordw(t, dgMatrix); 180 | dtransformationdvars(dgMatrix, ch, dcm.col(0)); 181 | dgeneratordvx(t, dgMatrix); 182 | dtransformationdvars(dgMatrix, ch, dcm.col(1)); 183 | dgeneratordvy(t, dgMatrix); 184 | dtransformationdvars(dgMatrix, ch, dcm.col(2)); 185 | } 186 | 187 | void 188 | dtransformationdvars(const Ref& dgMatrix, 189 | const Ref& ch, 190 | Ref dcm) const 191 | { 192 | dcm.noalias() = dgMatrix.template topRows() * ch; 193 | } 194 | }; 195 | } // namespace incremental 196 | 197 | template 198 | using IncrementalIsometry = incremental::Model >; 199 | } // namespace EventEMin 200 | 201 | #endif // EVENT_EMIN_INCREMENTAL_ISOMETRY_H 202 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/rotation.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_ROTATION_H 2 | #define EVENT_EMIN_INCREMENTAL_ROTATION_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Rotation 12 | { 13 | enum 14 | { 15 | NW = 3, 16 | NVars = NW, 17 | NDims = 2, 18 | NMatrix = 3 19 | }; 20 | 21 | typedef Scalar T; 22 | typedef Vector Point; 23 | typedef Vector PointHomogeneous; 24 | typedef Vector Vars; 25 | typedef Matrix CMatrix; 26 | typedef Matrix DMatrix; 27 | typedef Matrix PMatrix; 28 | typedef Matrix GMatrix; 29 | typedef Matrix TMatrix; 30 | 31 | Rotation(void) = default; 32 | 33 | void 34 | operator()(const Ref& vars, const Ref& c, const T& t, 35 | Ref cm) const 36 | { 37 | const Vars varst(t * vars); 38 | 39 | GMatrix gMatrix; 40 | generator(varst, gMatrix); 41 | 42 | PointHomogeneous ch; 43 | ch << c, T(1.0); 44 | 45 | // transformation 46 | transformation(gMatrix, ch, cm); 47 | } 48 | 49 | void 50 | operator()(const Ref& vars, const Ref& c, const T& t, 51 | Ref cm, Ref dcm, Ref cg, 52 | Ref pMatrix) const 53 | { 54 | const Vars varst(t * vars); 55 | 56 | GMatrix gMatrix; 57 | generator(varst, gMatrix); 58 | 59 | PointHomogeneous ch; 60 | ch << c, T(1.0); 61 | 62 | // transformation 63 | const T z = transformation(gMatrix, ch, cm); 64 | 65 | // generator 66 | generator(gMatrix, ch, z, cg); 67 | 68 | // derivatives wrt to vars 69 | dtransformationdvars(ch, cm, t, z, dcm); 70 | 71 | // perturbation 72 | perturbation(c, t, z, pMatrix); 73 | } 74 | 75 | void 76 | operator()(const Ref& vars, const Ref& c, const T& t, 77 | Ref cm, Ref dcm, Ref dc) const 78 | { 79 | const Vars varst(t * vars); 80 | 81 | GMatrix gMatrix; 82 | generator(varst, gMatrix); 83 | 84 | PointHomogeneous ch; 85 | ch << c, T(1.0); 86 | 87 | // transformation 88 | TMatrix tMatrix; 89 | const T z = transformation(gMatrix, ch, cm, tMatrix); 90 | 91 | // derivatives wrt to vars 92 | dtransformationdvars(ch, cm, t, z, dcm); 93 | 94 | // derivatives wrt to c 95 | dtransformationdc(tMatrix, z, dc); 96 | } 97 | 98 | void 99 | generator(const Ref& gMatrix, 100 | const Ref& ch, const T& z, 101 | Ref cg) const 102 | { 103 | cg.noalias() = (gMatrix.template topRows() * ch) / z; 104 | } 105 | 106 | void 107 | generator(const Ref& varst, Ref gMatrix) const 108 | { 109 | gMatrix << T(0.0), -varst(2), varst(1), varst(2), T(0.0), -varst(0), 110 | -varst(1), varst(0), T(0.0); 111 | } 112 | 113 | void 114 | dgeneratordwx(const T& t, Ref dgMatrix) const 115 | { 116 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), -t, T(0.0), t, T(0.0); 117 | } 118 | 119 | void 120 | dgeneratordwy(const T& t, Ref dgMatrix) const 121 | { 122 | dgMatrix << T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), -t, T(0.0), T(0.0); 123 | } 124 | 125 | void 126 | dgeneratordwz(const T& t, Ref dgMatrix) const 127 | { 128 | dgMatrix << T(0.0), -t, T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 129 | } 130 | 131 | void 132 | perturbation(const Ref& c, const T& t, const T& z, 133 | Ref pMatrix) const 134 | { 135 | const T tz = t / z; 136 | const Point ctz(tz * c); 137 | pMatrix << T(0.0), tz, -ctz(1), -tz, T(0.0), ctz(0); 138 | } 139 | 140 | T 141 | transformation(const Ref& gMatrix, 142 | const Ref& ch, Ref cm) const 143 | { 144 | TMatrix tMatrix; 145 | return transformation(gMatrix, ch, cm, tMatrix); 146 | } 147 | 148 | T 149 | transformation(const Ref& gMatrix, 150 | const Ref& ch, Ref cm, 151 | Ref tMatrix) const 152 | { 153 | transformationG(gMatrix, tMatrix); 154 | const PointHomogeneous cmh(tMatrix * ch); 155 | cm = cmh.template head() / cmh(2); 156 | return cmh(2); 157 | } 158 | 159 | void 160 | transformation(const Ref& varst, Ref tMatrix) const 161 | { 162 | GMatrix gMatrix; 163 | generator(varst, gMatrix); 164 | transformationG(gMatrix, tMatrix); 165 | } 166 | 167 | void 168 | transformationG(const Ref& gMatrix, Ref tMatrix) const 169 | { 170 | // 1st order Taylor series expansion 171 | tMatrix.setIdentity(); 172 | tMatrix += gMatrix; 173 | } 174 | 175 | void 176 | dtransformationdvars(const Ref& ch, 177 | const Ref& cm, const T& t, const T& z, 178 | Ref dcm) const 179 | { 180 | GMatrix dgMatrix; 181 | dgeneratordwx(t, dgMatrix); 182 | dtransformationdvars(dgMatrix, ch, cm, z, dcm.col(0)); 183 | dgeneratordwy(t, dgMatrix); 184 | dtransformationdvars(dgMatrix, ch, cm, z, dcm.col(1)); 185 | dgeneratordwz(t, dgMatrix); 186 | dtransformationdvars(dgMatrix, ch, cm, z, dcm.col(2)); 187 | } 188 | 189 | void 190 | dtransformationdvars(const Ref& dgMatrix, 191 | const Ref& ch, 192 | const Ref& cm, const T& z, 193 | Ref dcm) const 194 | { 195 | dcm = (dgMatrix.template topRows() * ch - 196 | dgMatrix.template bottomRows<1>() * ch * cm) / 197 | z; 198 | } 199 | 200 | void 201 | dtransformationdc(const Ref& tMatrix, const T& z, 202 | Ref dc) const 203 | { 204 | PointHomogeneous dcc; 205 | dcc << T(1.0), T(0.0), T(0.0); 206 | dtransformationdc(tMatrix, dcc, z, dc.col(0)); 207 | dcc << T(0.0), T(1.0), T(0.0); 208 | dtransformationdc(tMatrix, dcc, z, dc.col(1)); 209 | } 210 | 211 | void 212 | dtransformationdc(const Ref& tMatrix, 213 | const Ref& dcc, const T& z, 214 | Ref dc) const 215 | { 216 | dc.noalias() = 217 | tMatrix.template topRows() * 218 | (dcc.array() - (tMatrix.template bottomRows<1>() * dcc)(0) / z) 219 | .matrix() / 220 | z; 221 | } 222 | }; 223 | } // namespace incremental 224 | 225 | template 226 | using IncrementalRotation = incremental::Model >; 227 | } // namespace EventEMin 228 | 229 | #endif // EVENT_EMIN_INCREMENTAL_ROTATION_H 230 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/similarity.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_SIMILARITY_H 2 | #define EVENT_EMIN_INCREMENTAL_SIMILARITY_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Similarity 12 | { 13 | enum 14 | { 15 | NW = 1, 16 | NS = 1, 17 | NV = 2, 18 | NVars = NW + NS + NV, 19 | NDims = 2, 20 | NMatrix = 3 21 | }; 22 | 23 | typedef Scalar T; 24 | typedef Vector Point; 25 | typedef Vector PointHomogeneous; 26 | typedef Vector Vars; 27 | typedef Matrix CMatrix; 28 | typedef Matrix DMatrix; 29 | typedef Matrix PMatrix; 30 | typedef Matrix GMatrix; 31 | typedef Matrix TMatrix; 32 | 33 | Similarity(void) = default; 34 | 35 | void 36 | operator()(const Ref& vars, const Ref& c, const T& t, 37 | Ref cm) const 38 | { 39 | const Vars varst(t * vars); 40 | 41 | GMatrix gMatrix; 42 | generator(varst, gMatrix); 43 | 44 | PointHomogeneous ch; 45 | ch << c, T(1.0); 46 | 47 | // transformation 48 | transformation(gMatrix, ch, cm); 49 | } 50 | 51 | void 52 | operator()(const Ref& vars, const Ref& c, const T& t, 53 | Ref cm, Ref dcm, Ref cg, 54 | Ref pMatrix) const 55 | { 56 | const Vars varst(t * vars); 57 | 58 | GMatrix gMatrix; 59 | generator(varst, gMatrix); 60 | 61 | PointHomogeneous ch; 62 | ch << c, T(1.0); 63 | 64 | // transformation 65 | transformation(gMatrix, ch, cm); 66 | 67 | // generator 68 | generator(gMatrix, ch, cg); 69 | 70 | // derivatives wrt to vars 71 | dtransformationdvars(ch, t, dcm); 72 | 73 | // perturbation 74 | perturbation(c, t, pMatrix); 75 | } 76 | 77 | void 78 | operator()(const Ref& vars, const Ref& c, const T& t, 79 | Ref cm, Ref dcm, Ref dc) const 80 | { 81 | const Vars varst(t * vars); 82 | 83 | GMatrix gMatrix; 84 | generator(varst, gMatrix); 85 | 86 | PointHomogeneous ch; 87 | ch << c, T(1.0); 88 | 89 | // transformation 90 | TMatrix tMatrix; 91 | transformation(gMatrix, ch, cm, tMatrix); 92 | 93 | // derivatives wrt to vars 94 | dtransformationdvars(ch, t, dcm); 95 | 96 | // derivatives wrt to c 97 | dc = tMatrix.template topLeftCorner(); 98 | } 99 | 100 | void 101 | generator(const Ref& gMatrix, 102 | const Ref& ch, Ref cg) const 103 | { 104 | cg.noalias() = gMatrix.template topRows() * ch; 105 | } 106 | 107 | void 108 | generator(const Ref& varst, Ref gMatrix) const 109 | { 110 | gMatrix << varst(1), -varst(0), varst(2), varst(0), varst(1), varst(3), 111 | T(0.0), T(0.0), T(0.0); 112 | } 113 | 114 | void 115 | dgeneratordw(const T& t, Ref dgMatrix) const 116 | { 117 | dgMatrix << T(0.0), -t, T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 118 | } 119 | 120 | void 121 | dgeneratords(const T& t, Ref dgMatrix) const 122 | { 123 | dgMatrix << t, T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0); 124 | } 125 | 126 | void 127 | dgeneratordvx(const T& t, Ref dgMatrix) const 128 | { 129 | dgMatrix << T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 130 | T(0.0); 131 | } 132 | 133 | void 134 | dgeneratordvy(const T& t, Ref dgMatrix) const 135 | { 136 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), 137 | T(0.0); 138 | } 139 | 140 | void 141 | perturbation(const Ref& c, const T& t, 142 | Ref pMatrix) const 143 | { 144 | const Point ct(t * c); 145 | pMatrix << -ct(1), ct(0), t, T(0.0), ct(0), ct(1), T(0.0), t; 146 | } 147 | 148 | void 149 | transformation(const Ref& gMatrix, 150 | const Ref& ch, Ref cm) const 151 | { 152 | TMatrix tMatrix; 153 | transformation(gMatrix, ch, cm, tMatrix); 154 | } 155 | 156 | void 157 | transformation(const Ref& gMatrix, 158 | const Ref& ch, Ref cm, 159 | Ref tMatrix) const 160 | { 161 | transformationG(gMatrix, tMatrix); 162 | cm.noalias() = tMatrix.template topRows() * ch; 163 | } 164 | 165 | void 166 | transformation(const Ref& varst, Ref tMatrix) const 167 | { 168 | GMatrix gMatrix; 169 | generator(varst, gMatrix); 170 | transformationG(gMatrix, tMatrix); 171 | } 172 | 173 | void 174 | transformationG(const Ref& gMatrix, Ref tMatrix) const 175 | { 176 | // 1st order Taylor series expansion 177 | tMatrix.setIdentity(); 178 | tMatrix += gMatrix; 179 | } 180 | 181 | void 182 | dtransformationdvars(const Ref& ch, const T& t, 183 | Ref dcm) const 184 | { 185 | GMatrix dgMatrix; 186 | dgeneratordw(t, dgMatrix); 187 | dtransformationvars(dgMatrix, ch, dcm.col(0)); 188 | dgeneratords(t, dgMatrix); 189 | dtransformationvars(dgMatrix, ch, dcm.col(1)); 190 | dgeneratordvx(t, dgMatrix); 191 | dtransformationvars(dgMatrix, ch, dcm.col(2)); 192 | dgeneratordvy(t, dgMatrix); 193 | dtransformationvars(dgMatrix, ch, dcm.col(3)); 194 | } 195 | 196 | void 197 | dtransformationvars(const Ref& dgMatrix, 198 | const Ref& ch, 199 | Ref dcm) const 200 | { 201 | dcm.noalias() = dgMatrix.template topRows() * ch; 202 | } 203 | }; 204 | } // namespace incremental 205 | 206 | template 207 | using IncrementalSimilarity = incremental::Model >; 208 | } // namespace EventEMin 209 | 210 | #endif // EVENT_EMIN_INCREMENTAL_SIMILARITY_H 211 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/six_dof.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_SIX_DOF_H 2 | #define EVENT_EMIN_INCREMENTAL_SIX_DOF_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct SixDOF 12 | { 13 | enum 14 | { 15 | NW = 3, 16 | NV = 3, 17 | NVars = NW + NV, 18 | NDims = 3, 19 | NMatrix = 4 20 | }; 21 | 22 | typedef Scalar T; 23 | typedef Vector Point; 24 | typedef Vector PointHomogeneous; 25 | typedef Vector Vars; 26 | typedef Matrix CMatrix; 27 | typedef Matrix DMatrix; 28 | typedef Matrix PMatrix; 29 | typedef Matrix GMatrix; 30 | typedef Matrix TMatrix; 31 | 32 | SixDOF(void) = default; 33 | 34 | void 35 | operator()(const Ref& vars, const Ref& c, const T& t, 36 | Ref cm) const 37 | { 38 | const Vars varst(t * vars); 39 | 40 | GMatrix gMatrix; 41 | generator(varst, gMatrix); 42 | 43 | PointHomogeneous ch; 44 | ch << c, T(1.0); 45 | 46 | // transformation 47 | transformation(gMatrix, ch, cm); 48 | } 49 | 50 | void 51 | operator()(const Ref& vars, const Ref& c, const T& t, 52 | Ref cm, Ref dcm, Ref cg, 53 | Ref pMatrix) const 54 | { 55 | const Vars varst(t * vars); 56 | 57 | GMatrix gMatrix; 58 | generator(varst, gMatrix); 59 | 60 | PointHomogeneous ch; 61 | ch << c, T(1.0); 62 | 63 | // transformation 64 | transformation(gMatrix, ch, cm); 65 | 66 | // generator 67 | generator(gMatrix, ch, cg); 68 | 69 | // derivatives wrt to vars 70 | dtransformationdvars(ch, t, dcm); 71 | 72 | // perturbation 73 | perturbation(c, t, pMatrix); 74 | } 75 | 76 | void 77 | operator()(const Ref& vars, const Ref& c, const T& t, 78 | Ref cm, Ref dcm, Ref dc) const 79 | { 80 | const Vars varst(t * vars); 81 | 82 | GMatrix gMatrix; 83 | generator(varst, gMatrix); 84 | 85 | PointHomogeneous ch; 86 | ch << c, T(1.0); 87 | 88 | // transformation 89 | TMatrix tMatrix; 90 | transformation(gMatrix, ch, cm, tMatrix); 91 | 92 | // derivatives wrt to vars 93 | dtransformationdvars(ch, t, dcm); 94 | 95 | // derivatives wrt to c 96 | dc = tMatrix.template topLeftCorner(); 97 | } 98 | 99 | void 100 | generator(const Ref& gMatrix, 101 | const Ref& ch, Ref cg) const 102 | { 103 | cg.noalias() = gMatrix.template topRows() * ch; 104 | } 105 | 106 | void 107 | generator(const Ref& varst, Ref gMatrix) const 108 | { 109 | gMatrix << T(0.0), -varst(2), varst(1), varst(3), varst(2), T(0.0), 110 | -varst(0), varst(4), -varst(1), varst(0), T(0.0), varst(5), T(0.0), 111 | T(0.0), T(0.0), T(0.0); 112 | } 113 | 114 | void 115 | dgeneratordwx(const T& t, Ref dgMatrix) const 116 | { 117 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), -t, T(0.0), 118 | T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 119 | } 120 | 121 | void 122 | dgeneratordwy(const T& t, Ref dgMatrix) const 123 | { 124 | dgMatrix << T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), -t, 125 | T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 126 | } 127 | 128 | void 129 | dgeneratordwz(const T& t, Ref dgMatrix) const 130 | { 131 | dgMatrix << T(0.0), -t, T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), 132 | T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 133 | } 134 | 135 | void 136 | dgeneratordvx(const T& t, Ref dgMatrix) const 137 | { 138 | dgMatrix << T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), 139 | T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 140 | } 141 | 142 | void 143 | dgeneratordvy(const T& t, Ref dgMatrix) const 144 | { 145 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), t, 146 | T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 147 | } 148 | 149 | void 150 | dgeneratordvz(const T& t, Ref dgMatrix) const 151 | { 152 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 153 | T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0); 154 | } 155 | 156 | void 157 | perturbation(const Ref& c, const T& t, 158 | Ref pMatrix) const 159 | { 160 | const Point ct(t * c); 161 | pMatrix << T(0.0), ct(2), -ct(1), t, T(0.0), T(0.0), -ct(2), T(0.0), ct(0), 162 | T(0.0), t, T(0.0), ct(1), -ct(0), T(0.0), T(0.0), T(0.0), t; 163 | } 164 | 165 | void 166 | transformation(const Ref& gMatrix, 167 | const Ref& ch, Ref cm) const 168 | { 169 | TMatrix tMatrix; 170 | transformation(gMatrix, ch, cm, tMatrix); 171 | } 172 | 173 | void 174 | transformation(const Ref& gMatrix, 175 | const Ref& ch, Ref cm, 176 | Ref tMatrix) const 177 | { 178 | transformationG(gMatrix, tMatrix); 179 | cm.noalias() = tMatrix.template topRows() * ch; 180 | } 181 | 182 | void 183 | transformation(const Ref& varst, Ref tMatrix) const 184 | { 185 | GMatrix gMatrix; 186 | generator(varst, gMatrix); 187 | transformationG(gMatrix, tMatrix); 188 | } 189 | 190 | void 191 | transformationG(const Ref& gMatrix, Ref tMatrix) const 192 | { 193 | // 1st order Taylor series expansion 194 | tMatrix.setIdentity(); 195 | tMatrix += gMatrix; 196 | } 197 | 198 | void 199 | dtransformationdvars(const Ref& ch, const T& t, 200 | Ref dcm) const 201 | { 202 | GMatrix dgMatrix; 203 | dgeneratordwx(t, dgMatrix); 204 | dtransformationdvars(dgMatrix, ch, dcm.col(0)); 205 | dgeneratordwy(t, dgMatrix); 206 | dtransformationdvars(dgMatrix, ch, dcm.col(1)); 207 | dgeneratordwz(t, dgMatrix); 208 | dtransformationdvars(dgMatrix, ch, dcm.col(2)); 209 | dgeneratordvx(t, dgMatrix); 210 | dtransformationdvars(dgMatrix, ch, dcm.col(3)); 211 | dgeneratordvy(t, dgMatrix); 212 | dtransformationdvars(dgMatrix, ch, dcm.col(4)); 213 | dgeneratordvz(t, dgMatrix); 214 | dtransformationdvars(dgMatrix, ch, dcm.col(5)); 215 | } 216 | 217 | void 218 | dtransformationdvars(const Ref& dgMatrix, 219 | const Ref& ch, 220 | Ref dcm) const 221 | { 222 | dcm.noalias() = dgMatrix.template topRows() * ch; 223 | } 224 | }; 225 | } // namespace incremental 226 | 227 | template 228 | using IncrementalSixDOF = incremental::Model >; 229 | } // namespace EventEMin 230 | 231 | #endif // EVENT_EMIN_INCREMENTAL_SIX_DOF_H 232 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/translation2d.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_TRANSLATION_2D_H 2 | #define EVENT_EMIN_INCREMENTAL_TRANSLATION_2D_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Translation2D 12 | { 13 | enum 14 | { 15 | NV = 2, 16 | NVars = NV, 17 | NDims = 2, 18 | NMatrix = 3 19 | }; 20 | 21 | typedef Scalar T; 22 | typedef Vector Point; 23 | typedef Vector PointHomogeneous; 24 | typedef Vector Vars; 25 | typedef Matrix CMatrix; 26 | typedef Matrix DMatrix; 27 | typedef Matrix PMatrix; 28 | typedef Matrix GMatrix; 29 | typedef Matrix TMatrix; 30 | 31 | Translation2D(void) = default; 32 | 33 | void 34 | operator()(const Ref& vars, const Ref& c, const T& t, 35 | Ref cm) const 36 | { 37 | const Vars varst(t * vars); 38 | 39 | // transformation 40 | transformation(varst, c, cm); 41 | } 42 | 43 | void 44 | operator()(const Ref& vars, const Ref& c, const T& t, 45 | Ref cm, Ref dcm, Ref cg, 46 | Ref pMatrix) const 47 | { 48 | const Vars varst(t * vars); 49 | 50 | // transformation 51 | transformation(varst, c, cm); 52 | 53 | // generator 54 | cg = varst; 55 | 56 | // derivatives wrt to vars 57 | dcm << t, T(0.0), T(0.0), t; 58 | 59 | // perturbation 60 | pMatrix << t, T(0.0), T(0.0), t; 61 | } 62 | 63 | void 64 | operator()(const Ref& vars, const Ref& c, const T& t, 65 | Ref cm, Ref dcm, Ref dc) const 66 | { 67 | const Vars varst(t * vars); 68 | 69 | // transformation 70 | transformation(varst, c, cm); 71 | 72 | // derivatives wrt to vars 73 | dcm << t, T(0.0), T(0.0), t; 74 | 75 | // derivatives wrt to c 76 | dc.setIdentity(); 77 | } 78 | 79 | void 80 | generator(const Ref& varst, Ref gMatrix) const 81 | { 82 | gMatrix << T(0.0), T(0.0), varst(0), T(0.0), T(0.0), varst(1), T(0.0), 83 | T(0.0), T(0.0); 84 | } 85 | 86 | void 87 | transformation(const Ref& varst, const Ref& c, 88 | Ref cm) const 89 | { 90 | cm = c + varst; 91 | } 92 | 93 | void 94 | transformation(const Ref& varst, Ref tMatrix) const 95 | { 96 | GMatrix gMatrix; 97 | generator(varst, gMatrix); 98 | transformationG(gMatrix, tMatrix); 99 | } 100 | 101 | void 102 | transformationG(const Ref& gMatrix, Ref tMatrix) const 103 | { 104 | tMatrix.setIdentity(); 105 | tMatrix += gMatrix; 106 | } 107 | }; 108 | } // namespace incremental 109 | 110 | template 111 | using IncrementalTranslation2D = 112 | incremental::Model >; 113 | } // namespace EventEMin 114 | 115 | #endif // EVENT_EMIN_INCREMENTAL_TRANSLATION_2D_H 116 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/translation3d.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_TRANSLATION3D_H 2 | #define EVENT_EMIN_INCREMENTAL_TRANSLATION3D_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct Translation3D 12 | { 13 | enum 14 | { 15 | NV = 3, 16 | NVars = NV, 17 | NDims = 3, 18 | NMatrix = 4 19 | }; 20 | 21 | typedef Scalar T; 22 | typedef Vector Point; 23 | typedef Vector PointHomogeneous; 24 | typedef Vector Vars; 25 | typedef Matrix CMatrix; 26 | typedef Matrix DMatrix; 27 | typedef Matrix PMatrix; 28 | typedef Matrix GMatrix; 29 | typedef Matrix TMatrix; 30 | 31 | Translation3D(void) = default; 32 | 33 | void 34 | operator()(const Ref& vars, const Ref& c, const T& t, 35 | Ref cm) const 36 | { 37 | const Vars varst(t * vars); 38 | 39 | GMatrix gMatrix; 40 | generator(varst, gMatrix); 41 | 42 | PointHomogeneous ch; 43 | ch << c, T(1.0); 44 | 45 | // transformation 46 | transformation(gMatrix, ch, cm); 47 | } 48 | 49 | void 50 | operator()(const Ref& vars, const Ref& c, const T& t, 51 | Ref cm, Ref dcm, Ref cg, 52 | Ref pMatrix) const 53 | { 54 | const Vars varst(t * vars); 55 | 56 | GMatrix gMatrix; 57 | generator(varst, gMatrix); 58 | 59 | PointHomogeneous ch; 60 | ch << c, T(1.0); 61 | 62 | // transformation 63 | transformation(gMatrix, ch, cm); 64 | 65 | // generator 66 | generator(gMatrix, ch, cg); 67 | 68 | // derivatives wrt to vars 69 | dtransformationdvars(ch, t, dcm); 70 | 71 | // perturbation 72 | perturbation(c, t, pMatrix); 73 | } 74 | 75 | void 76 | operator()(const Ref& vars, const Ref& c, const T& t, 77 | Ref cm, Ref dcm, Ref dc) const 78 | { 79 | const Vars varst(t * vars); 80 | 81 | GMatrix gMatrix; 82 | generator(varst, gMatrix); 83 | 84 | PointHomogeneous ch; 85 | ch << c, T(1.0); 86 | 87 | // transformation 88 | TMatrix tMatrix; 89 | transformation(gMatrix, ch, cm, tMatrix); 90 | 91 | // derivatives wrt to vars 92 | dtransformationdvars(ch, t, dcm); 93 | 94 | // derivatives wrt to c 95 | dc = tMatrix.template topLeftCorner(); 96 | } 97 | 98 | void 99 | generator(const Ref& gMatrix, 100 | const Ref& ch, Ref cg) const 101 | { 102 | cg.noalias() = gMatrix.template topRows() * ch; 103 | } 104 | 105 | void 106 | generator(const Ref& varst, Ref gMatrix) const 107 | { 108 | gMatrix << T(0.0), T(0.0), T(0.0), varst(0), T(0.0), T(0.0), T(0.0), 109 | varst(1), T(0.0), T(0.0), T(0.0), varst(2), T(0.0), T(0.0), T(0.0), 110 | T(0.0); 111 | } 112 | 113 | void 114 | dgeneratordvx(const T& t, Ref dgMatrix) const 115 | { 116 | dgMatrix << T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0), 117 | T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 118 | } 119 | 120 | void 121 | dgeneratordvy(const T& t, Ref dgMatrix) const 122 | { 123 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), t, 124 | T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0); 125 | } 126 | 127 | void 128 | dgeneratordvz(const T& t, Ref dgMatrix) const 129 | { 130 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 131 | T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), T(0.0); 132 | } 133 | 134 | void 135 | perturbation(const Ref& c, const T& t, 136 | Ref pMatrix) const 137 | { 138 | const Point ct(t * c); 139 | pMatrix << t, T(0.0), T(0.0), T(0.0), t, T(0.0), T(0.0), T(0.0), t; 140 | } 141 | 142 | void 143 | transformation(const Ref& gMatrix, 144 | const Ref& ch, Ref cm) const 145 | { 146 | TMatrix tMatrix; 147 | transformation(gMatrix, ch, cm, tMatrix); 148 | } 149 | 150 | void 151 | transformation(const Ref& gMatrix, 152 | const Ref& ch, Ref cm, 153 | Ref tMatrix) const 154 | { 155 | transformationG(gMatrix, tMatrix); 156 | cm.noalias() = tMatrix.template topRows() * ch; 157 | } 158 | 159 | void 160 | transformation(const Ref& varst, Ref tMatrix) const 161 | { 162 | GMatrix gMatrix; 163 | generator(varst, gMatrix); 164 | transformationG(gMatrix, tMatrix); 165 | } 166 | 167 | void 168 | transformationG(const Ref& gMatrix, Ref tMatrix) const 169 | { 170 | // 1st order Taylor series expansion 171 | tMatrix.setIdentity(); 172 | tMatrix += gMatrix; 173 | } 174 | 175 | void 176 | dtransformationdvars(const Ref& ch, const T& t, 177 | Ref dcm) const 178 | { 179 | GMatrix dgMatrix; 180 | dgeneratordvx(t, dgMatrix); 181 | dtransformationdvars(dgMatrix, ch, dcm.col(0)); 182 | dgeneratordvy(t, dgMatrix); 183 | dtransformationdvars(dgMatrix, ch, dcm.col(1)); 184 | dgeneratordvz(t, dgMatrix); 185 | dtransformationdvars(dgMatrix, ch, dcm.col(2)); 186 | } 187 | 188 | void 189 | dtransformationdvars(const Ref& dgMatrix, 190 | const Ref& ch, 191 | Ref dcm) const 192 | { 193 | dcm.noalias() = dgMatrix.template topRows() * ch; 194 | } 195 | }; 196 | } // namespace incremental 197 | 198 | template 199 | using IncrementalTranslation3D = 200 | incremental::Model >; 201 | } // namespace EventEMin 202 | 203 | #endif // EVENT_EMIN_INCREMENTAL_TRANSLATION3D_H 204 | -------------------------------------------------------------------------------- /include/EventEMin/model/incremental_model/translation_normal.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_INCREMENTAL_TRANSLATION_NORMAL_H 2 | #define EVENT_EMIN_INCREMENTAL_TRANSLATION_NORMAL_H 3 | 4 | #include "EventEMin/model/incremental_model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace incremental 9 | { 10 | template 11 | struct TranslationNormal 12 | { 13 | enum 14 | { 15 | NV = 3, 16 | NVars = NV, 17 | NDims = 2, 18 | NMatrix = 3 19 | }; 20 | 21 | typedef Scalar T; 22 | typedef Vector Point; 23 | typedef Vector PointHomogeneous; 24 | typedef Vector Vars; 25 | typedef Matrix CMatrix; 26 | typedef Matrix DMatrix; 27 | typedef Matrix PMatrix; 28 | typedef Matrix GMatrix; 29 | typedef Matrix TMatrix; 30 | 31 | TranslationNormal(void) = default; 32 | 33 | void 34 | operator()(const Ref& vars, const Ref& c, const T& t, 35 | Ref cm) const 36 | { 37 | const Vars varst(t * vars); 38 | 39 | // transformation 40 | transformation(varst, c, cm); 41 | } 42 | 43 | void 44 | operator()(const Ref& vars, const Ref& c, const T& t, 45 | Ref cm, Ref dcm, Ref cg, 46 | Ref pMatrix) const 47 | { 48 | const Vars varst(t * vars); 49 | 50 | // transformation 51 | const T z = transformation(varst, c, cm); 52 | 53 | PointHomogeneous ch; 54 | ch << c, T(1.0); 55 | 56 | // generator 57 | generator(varst, ch, z, cg); 58 | 59 | // derivatives wrt to vars 60 | dtransformationdvars(ch, cm, t, z, dcm); 61 | 62 | // perturbation 63 | perturbation(c, t, z, pMatrix); 64 | } 65 | 66 | void 67 | operator()(const Ref& vars, const Ref& c, const T& t, 68 | Ref cm, Ref dcm, Ref dc) const 69 | { 70 | const Vars varst(t * vars); 71 | 72 | // transformation 73 | const T z = transformation(varst, c, cm); 74 | 75 | PointHomogeneous ch; 76 | ch << c, T(1.0); 77 | 78 | // derivatives wrt to vars 79 | dtransformationdvars(ch, cm, t, z, dcm); 80 | 81 | // derivatives wrt to c 82 | const T zInv = T(1.0) / z; 83 | dc << zInv, T(0.0), T(0.0), zInv; 84 | } 85 | 86 | void 87 | generator(const Ref& varst, const Ref& ch, 88 | const T& z, Ref cg) const 89 | { 90 | GMatrix gMatrix; 91 | generator(varst, gMatrix); 92 | cg.noalias() = (gMatrix.template topRows() * ch) / z; 93 | } 94 | 95 | void 96 | generator(const Ref& varst, Ref gMatrix) const 97 | { 98 | gMatrix << varst(2), T(0.0), -varst(0), T(0.0), varst(2), -varst(1), T(0.0), 99 | T(0.0), T(0.0); 100 | } 101 | 102 | void 103 | perturbation(const Ref& c, const T& t, const T& z, 104 | Ref pMatrix) const 105 | { 106 | const T tz = t / z; 107 | const Point ctz(tz * c); 108 | pMatrix << -tz, T(0.0), ctz(0), T(0.0), -tz, ctz(1); 109 | } 110 | 111 | T 112 | transformation(const Ref& varst, const Ref& c, 113 | Ref cm) const 114 | { 115 | const T z = T(1.0) - varst(2); 116 | cm.noalias() = (c - varst.template head<2>()) / z; 117 | return z; 118 | } 119 | 120 | void 121 | dtransformationdvx(const T& t, Ref dgMatrix) const 122 | { 123 | dgMatrix << T(0.0), T(0.0), -t, T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 124 | T(0.0); 125 | } 126 | 127 | void 128 | dtransformationdvy(const T& t, Ref dgMatrix) const 129 | { 130 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), -t, T(0.0), T(0.0), 131 | T(0.0); 132 | } 133 | 134 | void 135 | dtransformationdvz(const T& t, Ref dgMatrix) const 136 | { 137 | dgMatrix << T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), T(0.0), 138 | -t; 139 | } 140 | 141 | void 142 | dtransformationdvars(const Ref& ch, 143 | const Ref& cm, const T& t, const T& z, 144 | Ref dcm) const 145 | { 146 | GMatrix dgMatrix; 147 | dtransformationdvx(t, dgMatrix); 148 | dtransformationdvars(dgMatrix, ch, cm, z, dcm.col(0)); 149 | dtransformationdvy(t, dgMatrix); 150 | dtransformationdvars(dgMatrix, ch, cm, z, dcm.col(1)); 151 | dtransformationdvz(t, dgMatrix); 152 | dtransformationdvars(dgMatrix, ch, cm, z, dcm.col(2)); 153 | } 154 | 155 | void 156 | dtransformationdvars(const Ref& dgMatrix, 157 | const Ref& ch, 158 | const Ref& cm, const T& z, 159 | Ref dcm) const 160 | { 161 | dcm = (dgMatrix.template topRows() * ch - 162 | dgMatrix.template bottomRows<1>() * ch * cm) / 163 | z; 164 | } 165 | }; 166 | } // namespace incremental 167 | 168 | template 169 | using IncrementalTranslationNormal = 170 | incremental::Model >; 171 | } // namespace EventEMin 172 | 173 | #endif // EVENT_EMIN_INCREMENTAL_TRANSLATION_NORMAL_H 174 | -------------------------------------------------------------------------------- /include/EventEMin/model/model.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_MODEL_H 2 | #define EVENT_EMIN_MODEL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "EventEMin/types_def.h" 9 | 10 | namespace EventEMin 11 | { 12 | namespace batch 13 | { 14 | template 15 | class Model 16 | { 17 | public: 18 | typedef F Func; 19 | typedef typename Func::T T; 20 | 21 | enum 22 | { 23 | NVars = Func::NVars, 24 | NDims = Func::NDims, 25 | NMatrix = Func::NMatrix, 26 | InputsAtCompileTime = NVars, 27 | ValuesAtCompileTime = NDims 28 | }; 29 | 30 | typedef Vector InputType; 31 | typedef Vector ValueType; 32 | 33 | protected: 34 | Func func_; 35 | 36 | public: 37 | Model(void) = default; 38 | 39 | template 40 | void 41 | operator()(const U* const vars, const T& t, 42 | Matrix& m) const 43 | { 44 | func_(vars, t, m); 45 | } 46 | 47 | template 48 | void 49 | operator()(const Vector& vars, const T& t, 50 | Matrix& m) const 51 | { 52 | (*this)(vars.data(), t, m); 53 | } 54 | 55 | template 56 | void 57 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 58 | { 59 | func_(vars, c, t, cm); 60 | } 61 | 62 | template 63 | void 64 | operator()(const Vector& vars, Vector* cm, 65 | const Ref >& c, const T& t) const 66 | { 67 | assert(c.size() == NDims); 68 | 69 | (*this)(vars.data(), c.data(), t, cm->data()); 70 | } 71 | 72 | template 73 | void 74 | operator()(const Vector& vars, const Ref >& c, 75 | const Ref >& ts, const T& tsRef, 76 | Matrix& cm) const 77 | { 78 | assert(c.rows() == NDims); 79 | assert(cm.rows() == NDims); 80 | assert(c.cols() == cm.cols()); 81 | assert(c.cols() == ts.size()); 82 | 83 | const Vector t(ts.array() - tsRef); 84 | Vector vcm; 85 | for (int k = 0; k < c.cols(); ++k) 86 | { 87 | (*this)(vars, &vcm, c.col(k), t(k)); 88 | cm.col(k) = vcm; 89 | } 90 | } 91 | 92 | template 93 | void 94 | operator()(const Vector& vars, const Ref >& c, 95 | const T& t, Vector& cm) const 96 | { 97 | (*this)(vars, &cm, c, t); 98 | } 99 | 100 | template 101 | void 102 | operator()(const Vector& vars, const Ref >& c, 103 | const Ref >& ts, Matrix& cm) const 104 | { 105 | (*this)(vars, c, ts, ts(0), cm); 106 | } 107 | }; 108 | } // namespace batch 109 | } // namespace EventEMin 110 | 111 | #endif // EVENT_EMIN_MODEL_H 112 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/affinity.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_AFFINITY_H 2 | #define EVENT_EMIN_AFFINITY_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Affinity 12 | { 13 | enum 14 | { 15 | NWt = 1, 16 | NWp = 1, 17 | NL = 2, 18 | NV = 2, 19 | NVars = NWt + NWp + NL + NV, 20 | NDims = 2, 21 | NMatrix = 3, 22 | InputsAtCompileTime = NVars, 23 | ValuesAtCompileTime = NDims 24 | }; 25 | 26 | typedef Scalar T; 27 | typedef Vector InputType; 28 | typedef Vector ValueType; 29 | 30 | Affinity(void) = default; 31 | 32 | template 33 | void 34 | operator()(const U* const vars, const T& t, 35 | Matrix& m) const 36 | { 37 | const Map > varsMap(vars); 38 | const Vector varst(t * varsMap); 39 | const Map > wtt(varst.template segment(0).data()); 40 | const Map > wpt( 41 | varst.template segment(NWt).data()); 42 | const Map > lt( 43 | varst.template segment(NWt + NWp).data()); 44 | const Map > vt( 45 | varst.template segment(NWt + NWp + NL).data()); 46 | 47 | const U cosTheta = cos(wtt(0)), sinTheta = sin(wtt(0)); 48 | const U cosPhi = cos(wpt(0)), sinPhi = sin(wpt(0)); 49 | 50 | Matrix rThetaMatrix; 51 | rThetaMatrix << cosTheta, -sinTheta, sinTheta, cosTheta; 52 | Matrix rPhiMatrix; 53 | rPhiMatrix << cosPhi, -sinPhi, sinPhi, cosPhi; 54 | Matrix dMatrix; 55 | dMatrix << lt.maxCoeff() + T(1.0), T(0.0), T(0.0), lt.minCoeff() + T(1.0); 56 | 57 | const Matrix aMatrix(rThetaMatrix * rPhiMatrix.transpose() * 58 | dMatrix * rPhiMatrix); 59 | m << aMatrix(0, 0), aMatrix(0, 1), vt(0), aMatrix(1, 0), aMatrix(1, 1), 60 | vt(1), T(0.0), T(0.0), T(1.0); 61 | } 62 | 63 | template 64 | void 65 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 66 | { 67 | Matrix tMatrix; 68 | (*this)(vars, t, tMatrix); 69 | 70 | const Map > cMap(c); 71 | Map > cmMap(cm); 72 | Vector ch; 73 | Vector chm; 74 | ch << cMap, T(1.0); 75 | chm.noalias() = tMatrix.inverse() * ch; 76 | cmMap = chm.template head() / chm(NDims); 77 | } 78 | }; 79 | } // namespace batch 80 | 81 | template 82 | using Affinity = batch::Model >; 83 | } // namespace EventEMin 84 | 85 | #endif // EVENT_EMIN_AFFINITY_H 86 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/homography.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_HOMOGRAPHY_H 2 | #define EVENT_EMIN_HOMOGRAPHY_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Homography 12 | { 13 | enum 14 | { 15 | NW = 3, 16 | NV = 3, 17 | NNSph = 2, 18 | NN = 3, 19 | NVars = NW + NV + NNSph, 20 | NDims = 2, 21 | NMatrix = 3, 22 | InputsAtCompileTime = NVars, 23 | ValuesAtCompileTime = NDims 24 | }; 25 | 26 | typedef Scalar T; 27 | typedef Vector InputType; 28 | typedef Vector ValueType; 29 | 30 | Homography(void) = default; 31 | 32 | template 33 | void 34 | operator()(const U* const vars, const T& t, 35 | Matrix& m) const 36 | { 37 | const Map > varsMap(vars); 38 | const Map > w(varsMap.template segment(0).data()); 39 | const Map > v(varsMap.template segment(NW).data()); 40 | const Map > nsph( 41 | varsMap.template segment(NW + NV).data()); 42 | 43 | const U cosTheta = cos(nsph(0)), sinTheta = sin(nsph(0)); 44 | const U cosPhi = cos(nsph(1)), sinPhi = sin(nsph(1)); 45 | Vector n; 46 | n << cosTheta * sinPhi, sinTheta * sinPhi, cosPhi; 47 | 48 | Matrix rMatrix( 49 | Matrix::Identity()); 50 | const U wNorm = w.norm(); 51 | if (U(0.0) < wNorm) 52 | { 53 | const U theta = t * wNorm; 54 | Matrix skewMatrix; 55 | skewMatrix << T(0.0), -w(2), w(1), w(2), T(0.0), -w(0), -w(1), w(0), 56 | T(0.0); 57 | skewMatrix /= wNorm; 58 | rMatrix += sin(theta) * skewMatrix + 59 | (T(1.0) - cos(theta)) * skewMatrix * skewMatrix; 60 | } 61 | m = rMatrix - t * v * n.transpose(); 62 | } 63 | 64 | template 65 | void 66 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 67 | { 68 | Matrix tMatrix; 69 | (*this)(vars, t, tMatrix); 70 | 71 | const Map > cMap(c); 72 | Map > cmMap(cm); 73 | Vector ch; 74 | Vector chm; 75 | ch << cMap, T(1.0); 76 | chm.noalias() = tMatrix.inverse() * ch; 77 | cmMap = chm.template head() / chm(NDims); 78 | } 79 | }; 80 | } // namespace batch 81 | 82 | template 83 | using Homography = batch::Model >; 84 | } // namespace EventEMin 85 | 86 | #endif // EVENT_EMIN_HOMOGRAPHY_H 87 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/isometry.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_ISOMETRY_H 2 | #define EVENT_EMIN_ISOMETRY_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Isometry 12 | { 13 | enum 14 | { 15 | NW = 1, 16 | NV = 2, 17 | NVars = NW + NV, 18 | NDims = 2, 19 | NMatrix = 3, 20 | InputsAtCompileTime = NVars, 21 | ValuesAtCompileTime = NDims 22 | }; 23 | 24 | typedef Scalar T; 25 | typedef Vector InputType; 26 | typedef Vector ValueType; 27 | 28 | Isometry(void) = default; 29 | 30 | template 31 | void 32 | operator()(const U* const vars, const T& t, 33 | Matrix& m) const 34 | { 35 | const Map > varsMap(vars); 36 | const Vector varst(t * varsMap); 37 | const Map > wt(varst.template segment(0).data()); 38 | const Map > vt(varst.template segment(NW).data()); 39 | 40 | const U cosTheta = cos(wt(0)), sinTheta = sin(wt(0)); 41 | 42 | Matrix rThetaMatrix; 43 | rThetaMatrix << cosTheta, -sinTheta, sinTheta, cosTheta; 44 | 45 | m << rThetaMatrix(0, 0), rThetaMatrix(0, 1), vt(0), rThetaMatrix(1, 0), 46 | rThetaMatrix(1, 1), vt(1), T(0.0), T(0.0), T(1.0); 47 | } 48 | 49 | template 50 | void 51 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 52 | { 53 | Matrix tMatrix; 54 | (*this)(vars, t, tMatrix); 55 | 56 | const Map > cMap(c); 57 | Map > cmMap(cm); 58 | Vector ch; 59 | Vector chm; 60 | ch << cMap, T(1.0); 61 | chm.noalias() = tMatrix.inverse() * ch; 62 | cmMap = chm.template head() / chm(NDims); 63 | } 64 | }; 65 | } // namespace batch 66 | 67 | template 68 | using Isometry = batch::Model >; 69 | } // namespace EventEMin 70 | 71 | #endif // EVENT_EMIN_ISOMETRY_H 72 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/rotation.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_ROTATION_H 2 | #define EVENT_EMIN_ROTATION_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Rotation 12 | { 13 | enum 14 | { 15 | NW = 3, 16 | NVars = NW, 17 | NDims = 2, 18 | NMatrix = 3, 19 | InputsAtCompileTime = NVars, 20 | ValuesAtCompileTime = NDims 21 | }; 22 | 23 | typedef Scalar T; 24 | typedef Vector InputType; 25 | typedef Vector ValueType; 26 | 27 | Rotation(void) = default; 28 | 29 | template 30 | void 31 | operator()(const U* const vars, const T& t, 32 | Matrix& m) const 33 | { 34 | const Map > w(vars); 35 | 36 | m.setIdentity(); 37 | const U wNorm = w.norm(); 38 | if (U(0.0) < wNorm) 39 | { 40 | const U theta = t * wNorm; 41 | Matrix skewMatrix; 42 | skewMatrix << T(0.0), -w(2), w(1), w(2), T(0.0), -w(0), -w(1), w(0), 43 | T(0.0); 44 | skewMatrix /= wNorm; 45 | m += sin(theta) * skewMatrix + 46 | (T(1.0) - cos(theta)) * skewMatrix * skewMatrix; 47 | } 48 | } 49 | 50 | template 51 | void 52 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 53 | { 54 | Matrix tMatrix; 55 | (*this)(vars, t, tMatrix); 56 | 57 | const Map > cMap(c); 58 | Map > cmMap(cm); 59 | Vector ch; 60 | Vector chm; 61 | ch << cMap, T(1.0); 62 | chm.noalias() = tMatrix.transpose() * ch; 63 | cmMap = chm.template head() / chm(NDims); 64 | } 65 | }; 66 | } // namespace batch 67 | 68 | template 69 | using Rotation = batch::Model >; 70 | } // namespace EventEMin 71 | 72 | #endif // EVENT_EMIN_ROTATION_H 73 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/similarity.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_SIMILARITY_H 2 | #define EVENT_EMIN_SIMILARITY_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Similarity 12 | { 13 | enum 14 | { 15 | NW = 1, 16 | NS = 1, 17 | NV = 2, 18 | NVars = NW + NS + NV, 19 | NDims = 2, 20 | NMatrix = 3, 21 | InputsAtCompileTime = NVars, 22 | ValuesAtCompileTime = NDims 23 | }; 24 | 25 | typedef Scalar T; 26 | typedef Vector InputType; 27 | typedef Vector ValueType; 28 | 29 | Similarity(void) = default; 30 | 31 | template 32 | void 33 | operator()(const U* const vars, const T& t, 34 | Matrix& m) const 35 | { 36 | const Map > varsMap(vars); 37 | const Vector varst(t * varsMap); 38 | const Map > wt(varst.template segment(0).data()); 39 | const Map > st(varst.template segment(NW).data()); 40 | const Map > vt( 41 | varst.template segment(NW + NS).data()); 42 | 43 | const U cosTheta = cos(wt(0)), sinTheta = sin(wt(0)); 44 | 45 | Matrix rThetaMatrix; 46 | rThetaMatrix << cosTheta, -sinTheta, sinTheta, cosTheta; 47 | 48 | const Matrix sMatrix((st(0) + T(1.0)) * rThetaMatrix); 49 | m << sMatrix(0, 0), sMatrix(0, 1), vt(0), sMatrix(1, 0), sMatrix(1, 1), 50 | vt(1), T(0.0), T(0.0), T(1.0); 51 | } 52 | 53 | template 54 | void 55 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 56 | { 57 | Matrix tMatrix; 58 | (*this)(vars, t, tMatrix); 59 | 60 | const Map > cMap(c); 61 | Map > cmMap(cm); 62 | Vector ch; 63 | Vector chm; 64 | ch << cMap, T(1.0); 65 | chm.noalias() = tMatrix.inverse() * ch; 66 | cmMap = chm.template head() / chm(NDims); 67 | } 68 | }; 69 | } // namespace batch 70 | 71 | template 72 | using Similarity = batch::Model >; 73 | } // namespace EventEMin 74 | 75 | #endif // EVENT_EMIN_SIMILARITY_H 76 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/six_dof.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_SIX_DOF_H 2 | #define EVENT_EMIN_SIX_DOF_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct SixDOF 12 | { 13 | enum 14 | { 15 | NW = 3, 16 | NV = 3, 17 | NVars = NW + NV, 18 | NDims = 3, 19 | NMatrix = 4, 20 | InputsAtCompileTime = NVars, 21 | ValuesAtCompileTime = NDims 22 | }; 23 | 24 | typedef Scalar T; 25 | typedef Vector InputType; 26 | typedef Vector ValueType; 27 | 28 | SixDOF(void) = default; 29 | 30 | template 31 | void 32 | operator()(const U* const vars, const T& t, 33 | Matrix& m) const 34 | { 35 | const Map > varsMap(vars); 36 | const Map > w(varsMap.template segment(0).data()); 37 | const Map > v(varsMap.template segment(NW).data()); 38 | 39 | // First-order Taylor expansion 40 | Matrix skewMtx; 41 | skewMtx << T(0.0), -w(2), w(1), v(0), w(2), T(0.0), -w(0), v(1), -w(1), 42 | w(0), T(0.0), v(2), T(0.0), T(0.0), T(0.0), T(0.0); 43 | m = t * skewMtx; 44 | m.diagonal().array() += T(1.0); 45 | } 46 | 47 | template 48 | void 49 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 50 | { 51 | Matrix tMtx; 52 | (*this)(vars, t, tMtx); 53 | 54 | const Map > cMap(c); 55 | Map > cmMap(cm); 56 | Vector ch; 57 | Vector chm; 58 | ch << cMap, T(1.0); 59 | chm.noalias() = tMtx.inverse() * ch; 60 | cmMap = chm.template head() / chm(NDims); 61 | } 62 | }; 63 | } // namespace batch 64 | 65 | template 66 | using SixDOF = batch::Model >; 67 | } // namespace EventEMin 68 | 69 | #endif // EVENT_EMIN_SIX_DOF_H 70 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/translation.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TRANSLATION_H 2 | #define EVENT_EMIN_TRANSLATION_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Translation 12 | { 13 | enum 14 | { 15 | NV = 3, 16 | NNSph = 2, 17 | NN = 3, 18 | NVars = NV + NNSph, 19 | NDims = 2, 20 | NMatrix = 3, 21 | InputsAtCompileTime = NVars, 22 | ValuesAtCompileTime = NDims 23 | }; 24 | 25 | typedef Scalar T; 26 | typedef Vector InputType; 27 | typedef Vector ValueType; 28 | 29 | Translation(void) = default; 30 | 31 | template 32 | void 33 | operator()(const U* const vars, const T& t, 34 | Matrix& m) const 35 | { 36 | const Map > varsMap(vars); 37 | const Vector varst(t * varsMap.template head()); 38 | const Map > vt(varst.template segment(0).data()); 39 | const Map > nsph( 40 | varsMap.template segment(NV).data()); 41 | 42 | const U cosTheta = cos(nsph(0)), sinTheta = sin(nsph(0)); 43 | const U cosphi = cos(nsph(1)), sinphi = sin(nsph(1)); 44 | Vector n; 45 | n << cosTheta * sinphi, sinTheta * sinphi, cosphi; 46 | 47 | m.noalias() = -vt * n.transpose(); 48 | m.diagonal().array() += T(1.0); 49 | } 50 | 51 | template 52 | void 53 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 54 | { 55 | Matrix tMtx; 56 | (*this)(vars, t, tMtx); 57 | 58 | const Map > cMap(c); 59 | Map > cmMap(cm); 60 | Vector ch; 61 | Vector chm; 62 | ch << cMap, T(1.0); 63 | chm.noalias() = tMtx.inverse() * ch; 64 | cmMap = chm.template head() / chm(NDims); 65 | } 66 | }; 67 | } // namespace batch 68 | 69 | template 70 | using Translation = batch::Model >; 71 | } // namespace EventEMin 72 | 73 | #endif // EVENT_EMIN_TRANSLATION_H 74 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/translation2d.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TRANSLATION_2D_H 2 | #define EVENT_EMIN_TRANSLATION_2D_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Translation2D 12 | { 13 | enum 14 | { 15 | NV = 2, 16 | NVars = NV, 17 | NDims = 2, 18 | NMatrix = 3, 19 | InputsAtCompileTime = NVars, 20 | ValuesAtCompileTime = NDims 21 | }; 22 | 23 | typedef Scalar T; 24 | typedef Vector InputType; 25 | typedef Vector ValueType; 26 | 27 | Translation2D(void) = default; 28 | 29 | template 30 | void 31 | operator()(const U* const vars, const T& t, 32 | Matrix& m) const 33 | { 34 | const Map > v(vars); 35 | 36 | m.setIdentity(); 37 | m.template topRightCorner() = t * v; 38 | } 39 | 40 | template 41 | void 42 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 43 | { 44 | Matrix tMtx; 45 | (*this)(vars, t, tMtx); 46 | 47 | const Map > cMap(c); 48 | Map > cmMap(cm); 49 | Vector ch; 50 | Vector chm; 51 | ch << cMap, T(1.0); 52 | chm.noalias() = tMtx.inverse() * ch; 53 | cmMap = chm.template head() / chm(NDims); 54 | } 55 | }; 56 | } // namespace batch 57 | 58 | template 59 | using Translation2D = batch::Model >; 60 | } // namespace EventEMin 61 | 62 | #endif // EVENT_EMIN_TRANSLATION_2D_H 63 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/translation3d.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TRANSLATION_3D_H 2 | #define EVENT_EMIN_TRANSLATION_3D_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct Translation3D 12 | { 13 | enum 14 | { 15 | NV = 3, 16 | NVars = NV, 17 | NDims = 3, 18 | NMatrix = 4, 19 | InputsAtCompileTime = NVars, 20 | ValuesAtCompileTime = NDims 21 | }; 22 | 23 | typedef Scalar T; 24 | typedef Vector InputType; 25 | typedef Vector ValueType; 26 | 27 | Translation3D(void) = default; 28 | 29 | template 30 | void 31 | operator()(const U* const vars, const T& t, 32 | Matrix& m) const 33 | { 34 | const Map > varsMap(vars); 35 | const Map > v(varsMap.template segment(0).data()); 36 | 37 | Matrix skewMtx(Matrix::Zero()); 38 | skewMtx.template rightCols<1>().template head() = v; 39 | m = t * skewMtx; 40 | m.diagonal().array() += T(1.0); 41 | } 42 | 43 | template 44 | void 45 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 46 | { 47 | Matrix tMtx; 48 | (*this)(vars, t, tMtx); 49 | 50 | const Map > cMap(c); 51 | Map > cmMap(cm); 52 | Vector ch; 53 | Vector chm; 54 | ch << cMap, T(1.0); 55 | chm.noalias() = tMtx.inverse() * ch; 56 | cmMap = chm.template head() / chm(NDims); 57 | } 58 | }; 59 | } // namespace batch 60 | 61 | template 62 | using Translation3D = batch::Model >; 63 | } // namespace EventEMin 64 | 65 | #endif // EVENT_EMIN_TRANSLATION_3D_H 66 | -------------------------------------------------------------------------------- /include/EventEMin/model/model/translation_normal.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TRANSLATION_NORMAL_H 2 | #define EVENT_EMIN_TRANSLATION_NORMAL_H 3 | 4 | #include "EventEMin/model/model.h" 5 | 6 | namespace EventEMin 7 | { 8 | namespace batch 9 | { 10 | template 11 | struct TranslationNormal 12 | { 13 | enum 14 | { 15 | NV = 3, 16 | NVars = NV, 17 | NDims = 2, 18 | NMatrix = 3, 19 | InputsAtCompileTime = NVars, 20 | ValuesAtCompileTime = NDims 21 | }; 22 | 23 | typedef Scalar T; 24 | typedef Vector InputType; 25 | typedef Vector ValueType; 26 | 27 | TranslationNormal(void) = default; 28 | 29 | template 30 | void 31 | operator()(const U* const vars, const T& t, 32 | Matrix& m) const 33 | { 34 | const Map > v(vars); 35 | const Vector vt(t * v); 36 | 37 | m.setIdentity(); 38 | m.template rightCols<1>() -= vt; 39 | } 40 | 41 | template 42 | void 43 | operator()(const U* const vars, const V* const c, const T& t, U* cm) const 44 | { 45 | Matrix tMtx; 46 | (*this)(vars, t, tMtx); 47 | 48 | const Map > cMap(c); 49 | Map > cmMap(cm); 50 | Vector ch; 51 | Vector chm; 52 | ch << cMap, T(1.0); 53 | chm.noalias() = tMtx.inverse() * ch; 54 | cmMap = chm.template head() / chm(NDims); 55 | } 56 | }; 57 | } // namespace batch 58 | 59 | template 60 | using TranslationNormal = batch::Model >; 61 | } // namespace EventEMin 62 | 63 | #endif // EVENT_EMIN_TRANSLATION_NORMAL_H 64 | -------------------------------------------------------------------------------- /include/EventEMin/optimiser.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_OPTIMISER_ALL_H 2 | #define EVENT_EMIN_OPTIMISER_ALL_H 3 | 4 | #include "EventEMin/optimiser/gsl_fdf_optimiser.h" 5 | 6 | #endif // EVENT_EMIN_OPTIMISER_ALL_H 7 | -------------------------------------------------------------------------------- /include/EventEMin/optimiser/gsl_fdf_optimiser.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_GSL_FDF_OPTIMIZER_H 2 | #define EVENT_EMIN_GSL_FDF_OPTIMIZER_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "EventEMin/types_def.h" 10 | 11 | namespace EventEMin 12 | { 13 | template 14 | double 15 | gslfdffFunc(const gsl_vector* gvars, void* params); 16 | 17 | template 18 | void 19 | gslfdfdFunc(const gsl_vector* gvars, void* params, gsl_vector* gdf); 20 | 21 | template 22 | void 23 | gslfdffdFunc(const gsl_vector* gvars, void* params, double* gf, 24 | gsl_vector* gdf); 25 | 26 | struct GSLfdfOptimiserParams 27 | { 28 | const gsl_multimin_fdfminimizer_type* type; 29 | const double iniStep, tol; 30 | const int maxIter, maxrIter, verbose; 31 | 32 | GSLfdfOptimiserParams(const gsl_multimin_fdfminimizer_type* type = 33 | gsl_multimin_fdfminimizer_conjugate_fr, 34 | const double iniStep = 1.0e-1, 35 | const double tol = 1.0e-6, const int maxIter = 100, 36 | const int maxrIter = 1, const int verbose = 0) 37 | : type(type), 38 | iniStep(iniStep), 39 | tol(tol), 40 | maxIter(maxIter), 41 | maxrIter(maxrIter), 42 | verbose(verbose) 43 | { 44 | } 45 | }; 46 | 47 | template 48 | class GSLfdfOptimiser 49 | { 50 | public: 51 | typedef F Func; 52 | typedef typename Func::T T; 53 | typedef GSLfdfOptimiserParams OptimiserParams; 54 | 55 | static constexpr int NVars = Func::NVars; 56 | 57 | private: 58 | const OptimiserParams params_; 59 | int iter_; 60 | 61 | protected: 62 | const Eigen::AutoDiffJacobian func_; 63 | 64 | gsl_vector* gvars_; 65 | gsl_multimin_fdfminimizer* s_; 66 | gsl_multimin_function_fdf gslFunc_; 67 | 68 | Map > gvarsMap_; 69 | Vector vars_; 70 | 71 | public: 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | 74 | GSLfdfOptimiser(const Func& func, 75 | const OptimiserParams& params = OptimiserParams()) 76 | : params_(params), 77 | iter_(0), 78 | func_(func), 79 | gvars_(gsl_vector_alloc(NVars)), 80 | s_(gsl_multimin_fdfminimizer_alloc(params_.type, NVars)), 81 | gvarsMap_(gvars_->data, NVars) 82 | { 83 | gslFunc_.n = NVars; 84 | gslFunc_.f = &gslfdffFunc; 85 | gslFunc_.df = &gslfdfdFunc; 86 | gslFunc_.fdf = &gslfdffdFunc; 87 | gslFunc_.params = (void*)this; 88 | } 89 | GSLfdfOptimiser(const GSLfdfOptimiser& optimiser) 90 | : params_(optimiser.params_), 91 | iter_(optimiser.iter_), 92 | func_(optimiser.func_), 93 | gvars_(gsl_vector_alloc(NVars)), 94 | s_(gsl_multimin_fdfminimizer_alloc(params_.type, NVars)), 95 | gslFunc_(optimiser.gslFunc_), 96 | gvarsMap_(gvars_->data, NVars) 97 | { 98 | gslFunc_.params = (void*)this; 99 | } 100 | ~GSLfdfOptimiser(void) 101 | { 102 | if (gvars_ != nullptr) 103 | { 104 | gsl_vector_free(gvars_); 105 | } 106 | if (s_ != nullptr) 107 | { 108 | gsl_multimin_fdfminimizer_free(s_); 109 | } 110 | } 111 | 112 | const Vector& 113 | vars(void) const 114 | { 115 | return vars_; 116 | } 117 | double 118 | iniStep(void) const 119 | { 120 | return params_.iniStep; 121 | } 122 | double 123 | tol(void) const 124 | { 125 | return params_.tol; 126 | } 127 | int 128 | maxIter(void) const 129 | { 130 | return params_.maxIter; 131 | } 132 | int 133 | maxrIter(void) const 134 | { 135 | return params_.maxrIter; 136 | } 137 | int 138 | verbose(void) const 139 | { 140 | return params_.verbose; 141 | } 142 | int 143 | iter(void) const 144 | { 145 | return iter_; 146 | } 147 | 148 | T 149 | run(const Ref >& iniVars) 150 | { 151 | assert(iniVars.size() == NVars); 152 | 153 | gvarsMap_ = iniVars.template cast(); 154 | 155 | iter_ = 0; 156 | int riter = 0; 157 | int status; 158 | 159 | gsl_multimin_fdfminimizer_set(s_, &gslFunc_, gvars_, iniStep(), 1.0e-1); 160 | const Map > gvarsMap(s_->x->data, NVars); 161 | const Map > gradientMap(s_->gradient->data, NVars); 162 | 163 | do 164 | { 165 | ++iter_; 166 | status = gsl_multimin_fdfminimizer_iterate(s_); 167 | if (status) 168 | { 169 | gsl_multimin_fdfminimizer_set(s_, &gslFunc_, s_->x, iniStep(), 1.0e-2); 170 | ++riter; 171 | } 172 | else 173 | { 174 | riter = 0; 175 | } 176 | if (verbose()) 177 | { 178 | std::cout << iter_ << ", " << riter << ": " << s_->f << ' ' 179 | << gradientMap.norm() << '\n'; 180 | std::cout << gradientMap.transpose() << '\n'; 181 | } 182 | status = gsl_multimin_test_gradient(s_->gradient, tol()); 183 | if (verbose()) 184 | { 185 | if (status == GSL_SUCCESS) 186 | { 187 | std::cout << "minimum found at:\n"; 188 | } 189 | std::cout << gvarsMap.transpose() << '\n'; 190 | } 191 | } while (status == GSL_CONTINUE && iter_ < maxIter() && riter < maxrIter()); 192 | 193 | vars_ = gvarsMap.cast(); 194 | return static_cast(s_->f); 195 | } 196 | 197 | double 198 | operator()(const gsl_vector* gvars) const 199 | { 200 | const Map > varsMap(gvars->data, NVars); 201 | const Vector vars(varsMap.cast()); 202 | Vector f; 203 | func_(vars, &f, nullptr); 204 | return static_cast(f(0)); 205 | } 206 | 207 | void 208 | operator()(const gsl_vector* gvars, gsl_vector* gdf) const 209 | { 210 | const Map > varsMap(gvars->data, NVars); 211 | Map > dfMap(gdf->data, NVars); 212 | const Vector vars(varsMap.cast()); 213 | Vector f; 214 | Matrix df; 215 | func_(vars, &f, &df); 216 | dfMap = df.template cast(); 217 | } 218 | 219 | void 220 | operator()(const gsl_vector* gvars, double* gf, gsl_vector* gdf) const 221 | { 222 | const Map > varsMap(gvars->data, NVars); 223 | Map > dfMap(gdf->data, NVars); 224 | const Vector vars(varsMap.cast()); 225 | Vector f; 226 | Matrix df; 227 | func_(vars, &f, &df); 228 | *gf = static_cast(f(0)); 229 | dfMap = df.template cast(); 230 | } 231 | 232 | protected: 233 | private: 234 | }; 235 | 236 | template 237 | double 238 | gslfdffFunc(const gsl_vector* gvars, void* params) 239 | { 240 | return (*static_cast*>(params))(gvars); 241 | } 242 | 243 | template 244 | void 245 | gslfdfdFunc(const gsl_vector* gvars, void* params, gsl_vector* gdf) 246 | { 247 | (*static_cast*>(params))(gvars, gdf); 248 | } 249 | 250 | template 251 | void 252 | gslfdffdFunc(const gsl_vector* gvars, void* params, double* gf, gsl_vector* gdf) 253 | { 254 | (*static_cast*>(params))(gvars, gf, gdf); 255 | } 256 | } // namespace EventEMin 257 | 258 | #endif // GSL_OPTIMIZER_H 259 | -------------------------------------------------------------------------------- /include/EventEMin/test.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TEST_H 2 | #define EVENT_EMIN_TEST_H 3 | 4 | #include 5 | #include 6 | 7 | #include "EventEMin/event.h" 8 | #include "EventEMin/image.h" 9 | #include "EventEMin/optimiser.h" 10 | #include "EventEMin/types_def.h" 11 | 12 | namespace EventEMin 13 | { 14 | struct TestBatchParams 15 | { 16 | // dataset folder 17 | std::string fdir; 18 | // initial step size of the optimisation 19 | double iniStep; 20 | // tolerance that indicates a minimum has been reached 21 | double tol; 22 | // maximum iterations 23 | int maxIter, maxrIter; 24 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 25 | int verbosity; 26 | }; 27 | 28 | template 29 | int 30 | testBatchExample(const TestBatchParams& testBatchParams) 31 | { 32 | typedef typename Dispersion::T T; 33 | typedef typename Dispersion::Model Model; 34 | 35 | constexpr int NDims = Model::NDims, NVars = Model::NVars; 36 | 37 | // optimiser 38 | typedef GSLfdfOptimiser Optimiser; 39 | 40 | IO_STATUS ioStatus; 41 | // read events from file 42 | Events evs; 43 | const std::string fevents(testBatchParams.fdir + "/events.txt"); 44 | ioStatus = load(fevents, evs); 45 | if (ioStatus != IO_SUCCESS) 46 | { 47 | ioStatusMessage(ioStatus, fevents); 48 | return -1; 49 | } 50 | const int nEvents = evs.size(); 51 | std::cout << "number of events: " << nEvents << '\n'; 52 | 53 | int width, height; 54 | Matrix camParams; 55 | const std::string fcalib(testBatchParams.fdir + "/calib.txt"); 56 | ioStatus = loadCamParams(fcalib, width, height, camParams); 57 | if (ioStatus != IO_SUCCESS) 58 | { 59 | ioStatusMessage(ioStatus, fcalib); 60 | return -1; 61 | } 62 | 63 | Matrix c; 64 | Vector ts; 65 | Vector polarity; 66 | events2eigen(evs, c, ts, polarity); 67 | 68 | // show original events 69 | showGray(c.template topRows<2>(), polarity, width, height, "Original Events"); 70 | std::cout << "press any key to continue...\n"; 71 | cv::waitKey(0); 72 | 73 | Matrix ct(NDims, nEvents); 74 | unprojectEvents()(camParams, c, ct); 75 | 76 | Dispersion dispersion(width); 77 | dispersion.assignPoints(ct, ts, polarity); 78 | 79 | // initial parameters 80 | Vector vars; 81 | vars.setConstant(1.0e-6); 82 | 83 | // optimise 84 | Optimiser optimiser( 85 | dispersion, 86 | typename Optimiser::OptimiserParams( 87 | gsl_multimin_fdfminimizer_conjugate_fr, testBatchParams.iniStep, 88 | testBatchParams.tol, testBatchParams.maxIter, 89 | testBatchParams.maxrIter, testBatchParams.verbosity)); 90 | 91 | std::cout << "score: " << optimiser.run(vars) 92 | << ", v: " << optimiser.vars().transpose() << '\n'; 93 | 94 | // show transformed events according to the estimated parameters 95 | const Model model; 96 | Matrix cm(NDims, nEvents), ctm(NDims, nEvents); 97 | model(optimiser.vars(), ct, ts, ctm); 98 | projectEvents()(camParams, ctm, cm); 99 | showGray(cm.template topRows<2>(), polarity, width, height, 100 | "Transformed Events"); 101 | std::cout << "press any key to exit...\n"; 102 | cv::waitKey(0); 103 | 104 | return 0; 105 | } 106 | 107 | struct TestIncrementalParams 108 | { 109 | // dataset folder 110 | std::string fdir; 111 | // tolerance that indicates a minimum has been reached 112 | double minStep; 113 | // maximum iterations 114 | int maxIter; 115 | // neighbouring radius 116 | int wSize; 117 | // number of events to maintain 118 | int nEvents; 119 | // depth scaling factor 120 | // only used when the number of dimensions is higher than 2 121 | float depthScale; 122 | }; 123 | 124 | template 125 | int 126 | testIncrementalExample(const TestIncrementalParams& testIncrementalParams) 127 | { 128 | typedef typename Dispersion::T T; 129 | typedef typename Dispersion::Model Model; 130 | 131 | constexpr int NDims = Model::NDims; 132 | 133 | IO_STATUS ioStatus; 134 | // read events from file 135 | Events evs; 136 | const std::string fevents(testIncrementalParams.fdir + "/events.txt"); 137 | ioStatus = load(fevents, evs); 138 | if (ioStatus != IO_SUCCESS) 139 | { 140 | ioStatusMessage(ioStatus, fevents); 141 | return -1; 142 | } 143 | const int nEvents = evs.size(); 144 | std::cout << "number of events: " << nEvents << '\n'; 145 | 146 | int width, height; 147 | Matrix camParams; 148 | const std::string fcalib(testIncrementalParams.fdir + "/calib.txt"); 149 | ioStatus = loadCamParams(fcalib, width, height, camParams); 150 | if (ioStatus != IO_SUCCESS) 151 | { 152 | ioStatusMessage(ioStatus, fcalib); 153 | return -1; 154 | } 155 | 156 | const std::string imgTransformedEvents("Transformed Events"), 157 | imgOriginalEvents("Original Events"); 158 | cv::namedWindow(imgTransformedEvents); 159 | cv::moveWindow(imgTransformedEvents, 440, 0); 160 | cv::namedWindow(imgOriginalEvents); 161 | 162 | Matrix c; 163 | Vector ts; 164 | Vector polarity; 165 | events2eigen(evs, c, ts, polarity); 166 | 167 | Matrix ct(NDims, nEvents); 168 | unprojectEvents()(camParams, c, ct); 169 | 170 | Vector scale(Vector::Ones()); 171 | if constexpr (NDims == 3) 172 | { 173 | scale(2) = testIncrementalParams.depthScale; 174 | } 175 | 176 | Dispersion dispersion( 177 | camParams, scale, 178 | typename Dispersion::Params(testIncrementalParams.minStep, 179 | testIncrementalParams.maxIter, 180 | testIncrementalParams.wSize), 181 | testIncrementalParams.nEvents, {width, height}); 182 | 183 | for (int k = 0; k < nEvents; ++k) 184 | { 185 | const int x = std::round(c(0, k)); 186 | const int y = std::round(c(1, k)); 187 | if (0 <= x && x < width && 0 <= y && y < height) 188 | { 189 | dispersion.run(ct.col(k), ts(k)); 190 | } 191 | 192 | if ((k + 1) % testIncrementalParams.nEvents == 0) 193 | { 194 | std::cout << "ts: " << ts(k) << ", v: " << -dispersion.vars().transpose() 195 | << '\n'; 196 | 197 | const int kk = k + 1 - testIncrementalParams.nEvents; 198 | const Model model; 199 | Matrix cm(NDims, testIncrementalParams.nEvents), 200 | ctm(NDims, testIncrementalParams.nEvents); 201 | model(dispersion.vars(), ct.middleCols(kk, testIncrementalParams.nEvents), 202 | ts.segment(kk, testIncrementalParams.nEvents), ctm); 203 | projectEvents()(camParams, ctm, cm); 204 | 205 | showGray(cm.template topRows<2>(), 206 | polarity.segment(kk, testIncrementalParams.nEvents), width, 207 | height, imgTransformedEvents); 208 | showGray( 209 | c.middleCols(kk, testIncrementalParams.nEvents).template topRows<2>(), 210 | polarity.segment(kk, testIncrementalParams.nEvents), width, height, 211 | imgOriginalEvents); 212 | cv::waitKey(30); 213 | } 214 | } 215 | 216 | std::cout << "v: " << -dispersion.vars().transpose() << '\n'; 217 | std::cout << "press any key to exit...\n"; 218 | cv::waitKey(0); 219 | 220 | return 0; 221 | } 222 | } // namespace EventEMin 223 | 224 | #endif // EVENT_EMIN_TEST_H 225 | -------------------------------------------------------------------------------- /include/EventEMin/types_def.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_TYPES_DEF_H 2 | #define EVENT_EMIN_TYPES_DEF_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace EventEMin 12 | { 13 | // std definitions 14 | template 15 | using StdVector = typename std::vector >; 16 | 17 | // OpenCV definitions 18 | typedef cv::Mat CvMatrix; 19 | template 20 | using CvVector = cv::Vec; 21 | 22 | #define CV_TYPE(T, N) CV_MAKETYPE(cv::DataDepth::value, N) 23 | 24 | // Eigen definitions 25 | constexpr int Dynamic = Eigen::Dynamic; 26 | 27 | typedef Eigen::Index Index; 28 | 29 | template 30 | using Array = typename Eigen::array; 31 | template 32 | using Matrix = typename Eigen::Matrix; 33 | template 34 | using RowVector = typename Eigen::Matrix; 35 | template 36 | using Tensor = typename Eigen::Tensor; 37 | template 38 | using Vector = typename Eigen::Matrix; 39 | 40 | template 41 | using DenseBase = typename Eigen::DenseBase; 42 | template 43 | using MatrixBase = typename Eigen::MatrixBase; 44 | 45 | template 46 | using Map = typename Eigen::Map; 47 | 48 | template 49 | using Ref = Eigen::Ref; 50 | template 51 | using TensorRef = typename Eigen::TensorRef; 52 | } // namespace EventEMin 53 | 54 | #endif // EVENT_EMIN_TYPES_DEF_H 55 | -------------------------------------------------------------------------------- /include/EventEMin/utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_EMIN_UTILITIES_H 2 | #define EVENT_EMIN_UTILITIES_H 3 | 4 | #include 5 | 6 | namespace EventEMin 7 | { 8 | inline double 9 | computeExp(const double x) 10 | { 11 | #ifdef EventEMin_FAST_EXP 12 | // fast computation of exp(x) 13 | if (x < -100) 14 | { 15 | return 0; 16 | } 17 | union 18 | { 19 | double d; 20 | long l; 21 | } u; 22 | u.l = long(double((long(1) << 52) / 0.69314718056) * x + 23 | double((long(1) << 52) * 1023)); 24 | return u.d; 25 | #else 26 | return std::exp(x); 27 | #endif 28 | } 29 | 30 | // add modulus (cyclic) 31 | inline int 32 | addCyclic(const int x, const int a, const int n) 33 | { 34 | return (x + a + n) % n; 35 | } 36 | // decrement modulus (cyclic) 37 | inline int 38 | decrementCyclic(const int x, const int n) 39 | { 40 | return addCyclic(x, -1, n); 41 | } 42 | // increment modulus (cyclic) 43 | inline int 44 | incrementCyclic(const int x, const int n) 45 | { 46 | return addCyclic(x, 1, n); 47 | } 48 | 49 | // fast cyclic operations assume n = 2^m-1, where m > 0 is some integer 50 | // fast add modulus (cyclic) 51 | inline int 52 | fastAddCyclic(const int x, const int a, const int n) 53 | { 54 | assert(0 < n && ((n + 1) & n) == 0); 55 | return (x + a) & n; 56 | } 57 | // fast decrement modulus (cyclic) 58 | inline int 59 | fastDecrementCyclic(const int x, const int n) 60 | { 61 | return fastAddCyclic(x, -1, n); 62 | } 63 | 64 | // fast increment modulus (cyclic) 65 | inline int 66 | fastIncrementCyclic(const int x, const int n) 67 | { 68 | return fastAddCyclic(x, 1, n); 69 | } 70 | 71 | // computes the next power of 2 higher than n 72 | inline int 73 | nextPower2(int n) 74 | { 75 | int i = 0; 76 | for (--n; n > 0; n >>= 1, ++i) 77 | ; 78 | return 1 << i; 79 | } 80 | } // namespace EventEMin 81 | 82 | #endif // EVENT_EMIN_UTILITIES_H 83 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ImperialCollegeLondon/EventEMin/0e84ac2b58c712e4b2922bb9c64300ca732c8b0b/src/CMakeLists.txt -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Function to add new executables 2 | function(add_new_executable ARG) 3 | add_executable(example_${ARG} main_${ARG}.cpp) 4 | target_link_libraries(example_${ARG} ${LIB_NAME}) 5 | endfunction() 6 | 7 | if(${LIB_NAME}_BATCH_MODE) 8 | add_new_executable(6dof) 9 | add_new_executable(homography) 10 | add_new_executable(rotation) 11 | add_new_executable(translation2d) 12 | 13 | add_new_executable(test_sequence) 14 | add_new_executable(test_sequence_3d) 15 | endif() 16 | 17 | if(${LIB_NAME}_INCREMENTAL_MODE) 18 | add_new_executable(incremental_6dof) 19 | add_new_executable(incremental_rotation) 20 | add_new_executable(incremental_translation2d) 21 | 22 | add_new_executable(incremental_test_sequence) 23 | add_new_executable(incremental_test_sequence_3d) 24 | endif() 25 | -------------------------------------------------------------------------------- /test/main_6dof.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef SixDOF Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // exact measures 17 | // typedef Potential Dispersion; 18 | // typedef Renyi Dispersion; 19 | // typedef Shannon Dispersion; 20 | // typedef SharmaMittal Dispersion; 21 | // typedef Tsallis Dispersion; 22 | // approximate measures 23 | // typedef ApproximatePotential Dispersion; 24 | // typedef ApproximateRenyi Dispersion; 25 | // typedef ApproximateShannon Dispersion; 26 | // typedef ApproximateSharmaMittal Dispersion; 27 | typedef ApproximateTsallis Dispersion; 28 | 29 | // test parameters for batch mode - default parameters 30 | TestBatchParams testBatchParams; 31 | // dataset folder 32 | testBatchParams.fdir = "../dataset/indoor_flying1"; 33 | // initial step size of the optimisation 34 | testBatchParams.iniStep = 1.0; 35 | // tolerance that indicates a minimum has been reached 36 | testBatchParams.tol = 1.0e-16; 37 | // maximum iterations 38 | testBatchParams.maxIter = 100, testBatchParams.maxrIter = 1; 39 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 40 | testBatchParams.verbosity = 1; 41 | 42 | return testBatchExample(testBatchParams); 43 | } 44 | -------------------------------------------------------------------------------- /test/main_homography.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef Homography Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // exact measures 17 | // typedef Potential Dispersion; 18 | // typedef Renyi Dispersion; 19 | // typedef Shannon Dispersion; 20 | // typedef SharmaMittal Dispersion; 21 | // typedef Tsallis Dispersion; 22 | // approximate measures 23 | // typedef ApproximatePotential Dispersion; 24 | // typedef ApproximateRenyi Dispersion; 25 | // typedef ApproximateShannon Dispersion; 26 | // typedef ApproximateSharmaMittal Dispersion; 27 | typedef ApproximateTsallis Dispersion; 28 | 29 | // test parameters for batch mode - default parameters 30 | TestBatchParams testBatchParams; 31 | // dataset folder 32 | testBatchParams.fdir = "../dataset/poster_6dof"; 33 | // initial step size of the optimisation 34 | testBatchParams.iniStep = 1.0; 35 | // tolerance that indicates a minimum has been reached 36 | testBatchParams.tol = 1.0e-16; 37 | // maximum iterations 38 | testBatchParams.maxIter = 100, testBatchParams.maxrIter = 1; 39 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 40 | testBatchParams.verbosity = 1; 41 | 42 | return testBatchExample(testBatchParams); 43 | } 44 | -------------------------------------------------------------------------------- /test/main_incremental_6dof.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef IncrementalSixDOF Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // incremental measures 17 | typedef IncrementalPotential Dispersion; 18 | // typedef IncrementalTsallis Dispersion; 19 | 20 | // test parameters for incremental mode - default parameters 21 | TestIncrementalParams testIncrementalParams; 22 | // dataset folder 23 | testIncrementalParams.fdir = "../dataset/indoor_flying1"; 24 | // tolerance that indicates a minimum has been reached 25 | testIncrementalParams.minStep = T(1.0e-6); 26 | // maximum iterations 27 | testIncrementalParams.maxIter = 10; 28 | // neighbouring radius 29 | testIncrementalParams.wSize = 4; 30 | // number of events to maintain 31 | testIncrementalParams.nEvents = 15000; 32 | // depth scaling factor 33 | testIncrementalParams.depthScale = T(100.0); 34 | 35 | return testIncrementalExample(testIncrementalParams); 36 | } 37 | -------------------------------------------------------------------------------- /test/main_incremental_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef IncrementalRotation Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // incremental measures 17 | typedef IncrementalPotential Dispersion; 18 | // typedef IncrementalTsallis Dispersion; 19 | 20 | // test parameters for incremental mode - default parameters 21 | TestIncrementalParams testIncrementalParams; 22 | // dataset folder 23 | testIncrementalParams.fdir = "../dataset/poster_rotation"; 24 | // tolerance that indicates a minimum has been reached 25 | testIncrementalParams.minStep = T(1.0e-6); 26 | // maximum iterations 27 | testIncrementalParams.maxIter = 10; 28 | // neighbouring radius 29 | testIncrementalParams.wSize = 4; 30 | // number of events to maintain 31 | testIncrementalParams.nEvents = 10000; 32 | 33 | return testIncrementalExample(testIncrementalParams); 34 | } 35 | -------------------------------------------------------------------------------- /test/main_incremental_test_sequence.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "EventEMin.h" 6 | 7 | using namespace EventEMin; 8 | 9 | int 10 | main(int argc, char* argv[]) 11 | { 12 | if (argc < 5) 13 | { 14 | std::cout << "usage: " << argv[0] 15 | << " [events dir] [number of events] " 16 | "[saving dir] [file name]\n"; 17 | return -1; 18 | } 19 | 20 | typedef float T; 21 | 22 | /* you can modify the model used by uncommenting the corresponding line */ 23 | 24 | // model 25 | // typedef IncrementalAffinity Model; 26 | // typedef IncrementalIsometry Model; 27 | typedef IncrementalRotation Model; 28 | // typedef IncrementalSimilarity Model; 29 | // typedef IncrementalTranslation2D Model; 30 | // typedef IncrementalTranslationNormal Model; 31 | 32 | constexpr int NDims = Model::NDims; 33 | 34 | /* you can modify the dispersion measure used by uncommenting the 35 | corresponding line */ 36 | 37 | // incremental measures 38 | typedef IncrementalPotential Dispersion; 39 | // typedef IncrementalTsallis Dispersion; 40 | // typedef IncrementalPotentialWhiten Dispersion; 41 | // typedef IncrementalTsallisWhiten Dispersion; 42 | 43 | // read distorted events from file 44 | const std::string fevents(std::string(argv[1]) + "/events.txt"); 45 | std::ifstream fin(fevents.c_str()); 46 | if (!fin.is_open()) 47 | { 48 | std::cerr << "error reading events from file " << fevents << '\n'; 49 | return -1; 50 | } 51 | 52 | // write estimates to file 53 | const std::string festimates(std::string(argv[3]) + "/" + 54 | std::string(argv[4]) + "_estimates.txt"); 55 | std::ofstream fout(festimates.c_str()); 56 | if (!fout.is_open()) 57 | { 58 | std::cerr << "error writing estimates to file " << festimates << '\n'; 59 | return -1; 60 | } 61 | 62 | int width, height; 63 | cv::Mat camParamsCV, distCoeffs; 64 | const std::string fcalib(std::string(argv[1]) + "/calib.txt"); 65 | const IO_STATUS ioStatus = 66 | loadCamParams(fcalib, width, height, camParamsCV, distCoeffs); 67 | if (ioStatus != IO_SUCCESS) 68 | { 69 | ioStatusMessage(ioStatus, fcalib); 70 | return -1; 71 | } 72 | 73 | // initialise grid-based undistortion map 74 | Matrix camParams; 75 | cv::cv2eigen(camParamsCV, camParams); 76 | cv::Mat undistortionMap; 77 | initUndistort(width, height, camParamsCV, distCoeffs, undistortionMap); 78 | 79 | const Vector scale(Vector::Ones()); 80 | 81 | // tolerance that indicates a minimum has been reached 82 | const T minStep = T(1.0e-6); 83 | // maximum iterations 84 | const int maxIter = 10; 85 | // neighbouring radius 86 | const int wSize = 4; 87 | // number of events to maintain 88 | const int nEvents = std::atoi(argv[2]); 89 | Dispersion dispersion(camParams, scale, 90 | Dispersion::Params(minStep, maxIter, wSize), nEvents, 91 | {width, height}); 92 | 93 | Vector c, ct; 94 | T ts; 95 | int polarity; 96 | 97 | for (int k = 0; undistort(0, width, 0, height, undistortionMap, fin, 98 | c, ts, polarity) == IO_SUCCESS; 99 | ++k) 100 | { 101 | unprojectEvent()(camParams, c, ct); 102 | 103 | dispersion.run(ct, ts); 104 | 105 | if ((k + 1) % nEvents == 0) 106 | { 107 | std::cout << "ts: " << ts << ", vars: " << dispersion.vars().transpose() 108 | << '\n'; 109 | fout << ts << ' ' << dispersion.vars().transpose() << std::endl; 110 | } 111 | } 112 | 113 | return 0; 114 | } 115 | -------------------------------------------------------------------------------- /test/main_incremental_test_sequence_3d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "EventEMin.h" 6 | 7 | using namespace EventEMin; 8 | 9 | int 10 | main(int argc, char* argv[]) 11 | { 12 | if (argc < 7) 13 | { 14 | std::cout << "usage: " << argv[0] 15 | << " [events dir] [number of events] " 16 | "[min depth] [depth scale] [saving dir] [file " 17 | "name]\n"; 18 | return -1; 19 | } 20 | 21 | typedef float T; 22 | 23 | /* you can modify the model used by uncommenting the corresponding line */ 24 | 25 | // model 26 | typedef IncrementalSixDOF Model; 27 | // typedef IncrementalTranslation3D Model; 28 | 29 | constexpr int NDims = Model::NDims; 30 | 31 | /* you can modify the dispersion measure used by uncommenting the 32 | corresponding line */ 33 | 34 | // incremental measures 35 | typedef IncrementalPotential Dispersion; 36 | // typedef IncrementalTsallis Dispersion; 37 | 38 | // read undistorted depth-augmented events from file 39 | const std::string fevents(std::string(argv[1]) + "/events.txt"); 40 | std::ifstream fin(fevents.c_str()); 41 | if (!fin.is_open()) 42 | { 43 | std::cerr << "error reading events from file " << fevents << '\n'; 44 | return -1; 45 | } 46 | 47 | // write estimates to file 48 | const std::string festimates(std::string(argv[5]) + "/" + 49 | std::string(argv[6]) + "_estimates.txt"); 50 | std::ofstream fout(festimates.c_str()); 51 | if (!fout.is_open()) 52 | { 53 | std::cerr << "error writing estimates to file " << festimates << '\n'; 54 | return -1; 55 | } 56 | 57 | int width, height; 58 | Matrix camParams; 59 | const std::string fcalib(std::string(argv[1]) + "/calib.txt"); 60 | const IO_STATUS ioStatus = loadCamParams(fcalib, width, height, camParams); 61 | if (ioStatus != IO_SUCCESS) 62 | { 63 | ioStatusMessage(ioStatus, fcalib); 64 | return -1; 65 | } 66 | 67 | Vector scale; 68 | scale << T(1.0), T(1.0), std::atof(argv[4]); 69 | 70 | // tolerance that indicates a minimum has been reached 71 | const T minStep = T(1.0e-6); 72 | // maximum iterations 73 | const int maxIter = 10; 74 | // neighbouring radius 75 | const int wSize = 4; 76 | // number of events to maintain 77 | const int nEvents = std::atoi(argv[2]); 78 | Dispersion dispersion(camParams, scale, 79 | Dispersion::Params(minStep, maxIter, wSize), nEvents, 80 | {width, height}); 81 | 82 | const T depthThresh = std::atof(argv[3]); 83 | Vector c, ct; 84 | T ts; 85 | int polarity; 86 | 87 | for (int k = 0; load(fin, c, ts, polarity) == IO_SUCCESS; ++k) 88 | { 89 | const int x = std::round(c(0)); 90 | const int y = std::round(c(1)); 91 | 92 | if (!(0 <= x && x < width && 0 <= y && y < height) || c(2) < depthThresh) 93 | { 94 | --k; 95 | continue; 96 | } 97 | unprojectEvent()(camParams, c, ct); 98 | 99 | dispersion.run(ct, ts); 100 | 101 | if ((k + 1) % nEvents == 0) 102 | { 103 | std::cout << "ts: " << ts << ", vars: " << dispersion.vars().transpose() 104 | << '\n'; 105 | fout << ts << ' ' << dispersion.vars().transpose() << std::endl; 106 | } 107 | } 108 | 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /test/main_incremental_translation2d.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef IncrementalTranslation2D Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // incremental measures 17 | typedef IncrementalPotential Dispersion; 18 | // typedef IncrementalTsallis Dispersion; 19 | 20 | // test parameters for incremental mode - default parameters 21 | TestIncrementalParams testIncrementalParams; 22 | // dataset folder 23 | testIncrementalParams.fdir = "../dataset/poster_translation"; 24 | // tolerance that indicates a minimum has been reached 25 | testIncrementalParams.minStep = T(1.0e-6); 26 | // maximum iterations 27 | testIncrementalParams.maxIter = 10; 28 | // neighbouring radius 29 | testIncrementalParams.wSize = 4; 30 | // number of events to maintain 31 | testIncrementalParams.nEvents = 10000; 32 | 33 | return testIncrementalExample(testIncrementalParams); 34 | } 35 | -------------------------------------------------------------------------------- /test/main_rotation.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef Rotation Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // exact measures 17 | // typedef Potential Dispersion; 18 | // typedef Renyi Dispersion; 19 | // typedef Shannon Dispersion; 20 | // typedef SharmaMittal Dispersion; 21 | // typedef Tsallis Dispersion; 22 | // approximate measures 23 | // typedef ApproximatePotential Dispersion; 24 | // typedef ApproximateRenyi Dispersion; 25 | // typedef ApproximateShannon Dispersion; 26 | // typedef ApproximateSharmaMittal Dispersion; 27 | typedef ApproximateTsallis Dispersion; 28 | 29 | // test parameters for batch mode - default parameters 30 | TestBatchParams testBatchParams; 31 | // dataset folder 32 | testBatchParams.fdir = "../dataset/poster_rotation"; 33 | // initial step size of the optimisation 34 | testBatchParams.iniStep = 1.0; 35 | // tolerance that indicates a minimum has been reached 36 | testBatchParams.tol = 1.0e-16; 37 | // maximum iterations 38 | testBatchParams.maxIter = 100, testBatchParams.maxrIter = 1; 39 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 40 | testBatchParams.verbosity = 1; 41 | 42 | return testBatchExample(testBatchParams); 43 | } 44 | -------------------------------------------------------------------------------- /test/main_test_sequence.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "EventEMin.h" 6 | 7 | using namespace EventEMin; 8 | 9 | int 10 | main(int argc, char* argv[]) 11 | { 12 | if (argc < 5) 13 | { 14 | std::cout << "usage: " << argv[0] 15 | << " [events dir] [number of events] " 16 | "[saving dir] [file name]\n"; 17 | return -1; 18 | } 19 | 20 | typedef float T; 21 | 22 | /* you can modify the model used by uncommenting the corresponding line */ 23 | 24 | // model 25 | // typedef Affinity Model; 26 | // typedef Homography Model; 27 | // typedef Isometry Model; 28 | typedef Rotation Model; 29 | // typedef Similarity Model; 30 | // typedef Translation Model; 31 | // typedef Translation2D Model; 32 | // typedef TranslationNormal Model; 33 | 34 | constexpr int NDims = Model::NDims, NVars = Model::NVars; 35 | 36 | /* you can modify the dispersion measure used by uncommenting the 37 | corresponding line */ 38 | 39 | // exact measures 40 | // typedef Potential Dispersion; 41 | // typedef Renyi Dispersion; 42 | // typedef Shannon Dispersion; 43 | // typedef SharmaMittal Dispersion; 44 | // typedef Tsallis Dispersion; 45 | // approximate measures 46 | // typedef ApproximatePotential Dispersion; 47 | // typedef ApproximateRenyi Dispersion; 48 | // typedef ApproximateShannon Dispersion; 49 | // typedef ApproximateSharmaMittal Dispersion; 50 | typedef ApproximateTsallis Dispersion; 51 | 52 | // optimiser 53 | typedef GSLfdfOptimiser Optimiser; 54 | 55 | // read distorted events from file 56 | const std::string fevents(std::string(argv[1]) + "/events.txt"); 57 | std::ifstream fin(fevents.c_str()); 58 | if (!fin.is_open()) 59 | { 60 | std::cerr << "error reading events from file " << fevents << '\n'; 61 | return -1; 62 | } 63 | 64 | // write estimates to file 65 | const std::string festimates(std::string(argv[3]) + "/" + 66 | std::string(argv[4]) + "_estimates.txt"); 67 | std::ofstream fout(festimates.c_str()); 68 | if (!fout.is_open()) 69 | { 70 | std::cerr << "error writing estimates to file " << festimates << '\n'; 71 | return -1; 72 | } 73 | 74 | int width, height; 75 | cv::Mat camParamsCV, distCoeffs; 76 | const std::string fcalib(std::string(argv[1]) + "/calib.txt"); 77 | const IO_STATUS ioStatus = 78 | loadCamParams(fcalib, width, height, camParamsCV, distCoeffs); 79 | if (ioStatus != IO_SUCCESS) 80 | { 81 | ioStatusMessage(ioStatus, fcalib); 82 | return -1; 83 | } 84 | 85 | // initialise grid-based undistortion map 86 | Matrix camParams; 87 | cv::cv2eigen(camParamsCV, camParams); 88 | cv::Mat undistortionMap; 89 | initUndistort(width, height, camParamsCV, distCoeffs, undistortionMap); 90 | 91 | Dispersion dispersion(width); 92 | 93 | // initial parameters 94 | Vector vars; 95 | vars.setConstant(1.0e-6); 96 | 97 | // initial step size of the optimisation 98 | const double iniStep = 1.0; 99 | // tolerance that indicates a minimum has been reached 100 | const double tol = 1.0e-16; 101 | // maximum iterations 102 | const int maxIter = 100, maxrIter = 1; 103 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 104 | const int verbosity = 1; 105 | // apply whitening pre-processing step 106 | const bool whiten = false; 107 | 108 | const int nEvents = std::atoi(argv[2]); 109 | Matrix c; 110 | Vector ts; 111 | Vector polarity; 112 | while (undistort(0, width, 0, height, undistortionMap, nEvents, fin, 113 | c, ts, polarity) == IO_SUCCESS) 114 | { 115 | Matrix ct(NDims, c.cols()); 116 | unprojectEvents()(camParams, c, ct); 117 | dispersion.assignPoints(ct, ts, polarity, whiten); 118 | 119 | // optimise 120 | Optimiser optimiser( 121 | dispersion, 122 | Optimiser::OptimiserParams(gsl_multimin_fdfminimizer_conjugate_fr, 123 | iniStep, tol, maxIter, maxrIter, verbosity)); 124 | 125 | optimiser.run(vars); 126 | vars = optimiser.vars(); 127 | std::cout << "ts: " << dispersion.tsEnd() << ", vars: " << vars.transpose() 128 | << '\n'; 129 | fout << dispersion.tsEnd() << ' ' << vars.transpose() << std::endl; 130 | } 131 | 132 | return 0; 133 | } 134 | -------------------------------------------------------------------------------- /test/main_test_sequence_3d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "EventEMin.h" 6 | 7 | using namespace EventEMin; 8 | 9 | int 10 | main(int argc, char* argv[]) 11 | { 12 | if (argc < 7) 13 | { 14 | std::cout << "usage: " << argv[0] 15 | << " [events dir] [number of events] " 16 | "[min depth] [max depth] [saving dir] " 17 | "[file name]\n"; 18 | return -1; 19 | } 20 | 21 | typedef float T; 22 | 23 | /* you can modify the model used by uncommenting the corresponding line */ 24 | 25 | // model 26 | typedef SixDOF Model; 27 | // typedef Translation3D Model; 28 | 29 | constexpr int NDims = Model::NDims, NVars = Model::NVars; 30 | 31 | // exact measures 32 | // typedef Potential Dispersion; 33 | // typedef Renyi Dispersion; 34 | // typedef Shannon Dispersion; 35 | // typedef SharmaMittal Dispersion; 36 | // typedef Tsallis Dispersion; 37 | // approximate measures 38 | // typedef ApproximatePotential Dispersion; 39 | // typedef ApproximateRenyi Dispersion; 40 | // typedef ApproximateShannon Dispersion; 41 | // typedef ApproximateSharmaMittal Dispersion; 42 | typedef ApproximateTsallis Dispersion; 43 | 44 | // optimiser 45 | typedef GSLfdfOptimiser Optimiser; 46 | 47 | // read undistorted depth-augmented events from file 48 | const std::string fevents(std::string(argv[1]) + "/events.txt"); 49 | std::ifstream fin(fevents.c_str()); 50 | if (!fin.is_open()) 51 | { 52 | std::cerr << "error reading events from file " << fevents << '\n'; 53 | return -1; 54 | } 55 | 56 | // write estimates to file 57 | const std::string festimates(std::string(argv[5]) + "/" + 58 | std::string(argv[6]) + "_estimates.txt"); 59 | std::ofstream fout(festimates.c_str()); 60 | if (!fout.is_open()) 61 | { 62 | std::cerr << "error writing estimates to file " << festimates << '\n'; 63 | return -1; 64 | } 65 | 66 | int width, height; 67 | Matrix camParams; 68 | const std::string fcalib(std::string(argv[1]) + "/calib.txt"); 69 | const IO_STATUS ioStatus = loadCamParams(fcalib, width, height, camParams); 70 | if (ioStatus != IO_SUCCESS) 71 | { 72 | ioStatusMessage(ioStatus, fcalib); 73 | return -1; 74 | } 75 | 76 | Dispersion dispersion(width); 77 | 78 | // initial parameters 79 | Vector vars; 80 | vars.setConstant(1.0e-6); 81 | 82 | // initial step size of the optimisation 83 | const double iniStep = 1.0; 84 | // tolerance that indicates a minimum has been reached 85 | const double tol = 1.0e-16; 86 | // maximum iterations 87 | const int maxIter = 100, maxrIter = 1; 88 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 89 | const int verbosity = 1; 90 | // apply whitening pre-processing step 91 | const bool whiten = false; 92 | 93 | const int nEvents = std::atoi(argv[2]); 94 | Matrix c; 95 | Vector ts; 96 | Vector polarity; 97 | while (loadDepthThresh(nEvents, std::atof(argv[3]), std::atof(argv[4]), 98 | fin, c, ts, polarity) == IO_SUCCESS) 99 | { 100 | Matrix ct(NDims, c.cols()); 101 | unprojectEvents()(camParams, c, ct); 102 | dispersion.assignPoints(ct, ts, polarity, whiten); 103 | 104 | Optimiser optimiser( 105 | dispersion, 106 | Optimiser::OptimiserParams(gsl_multimin_fdfminimizer_conjugate_fr, 107 | iniStep, tol, maxIter, maxrIter, verbosity)); 108 | 109 | optimiser.run(vars); 110 | vars = optimiser.vars(); 111 | std::cout << "ts: " << dispersion.tsEnd() << ", vars: " << vars.transpose() 112 | << '\n'; 113 | fout << dispersion.tsEnd() << ' ' << vars.transpose() << std::endl; 114 | } 115 | 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /test/main_translation2d.cpp: -------------------------------------------------------------------------------- 1 | #include "EventEMin.h" 2 | 3 | using namespace EventEMin; 4 | 5 | int 6 | main(void) 7 | { 8 | typedef float T; 9 | 10 | // model 11 | typedef Translation2D Model; 12 | 13 | /* you can modify the dispersion measure used by uncommenting the 14 | corresponding line */ 15 | 16 | // exact measures 17 | // typedef Potential Dispersion; 18 | // typedef Renyi Dispersion; 19 | // typedef Shannon Dispersion; 20 | // typedef SharmaMittal Dispersion; 21 | // typedef Tsallis Dispersion; 22 | // approximate measures 23 | // typedef ApproximatePotential Dispersion; 24 | // typedef ApproximateRenyi Dispersion; 25 | // typedef ApproximateShannon Dispersion; 26 | // typedef ApproximateSharmaMittal Dispersion; 27 | typedef ApproximateTsallis Dispersion; 28 | 29 | // test parameters for batch mode - default parameters 30 | TestBatchParams testBatchParams; 31 | // dataset folder 32 | testBatchParams.fdir = "../dataset/poster_translation"; 33 | // initial step size of the optimisation 34 | testBatchParams.iniStep = 1.0; 35 | // tolerance that indicates a minimum has been reached 36 | testBatchParams.tol = 1.0e-16; 37 | // maximum iterations 38 | testBatchParams.maxIter = 100, testBatchParams.maxrIter = 1; 39 | // optimiser status feedback: 0 - no feedback, 1 - feedback at each iteration 40 | testBatchParams.verbosity = 1; 41 | 42 | return testBatchExample(testBatchParams); 43 | } 44 | --------------------------------------------------------------------------------