├── .gitignore ├── .vscode ├── c_cpp_properties.json └── settings.json ├── CMakeLists.txt ├── EXTERNALS ├── CMakeLists.txt └── ais3dTools │ ├── CMakeLists.txt │ └── basics │ ├── CMakeLists.txt │ ├── ais3dTools_stuff_api.h │ ├── filesys_tools.cpp │ ├── filesys_tools.h │ ├── getopt.c │ ├── getopt.h │ ├── hashMap3D │ ├── hash_map_3d.h │ ├── hash_map_3d_search.h │ └── map_3d.h │ ├── macros.h │ ├── message_queue_t.h │ ├── message_queue_t.hpp │ ├── misc.h │ ├── os_specific.c │ ├── os_specific.h │ ├── parameters_manager.cpp │ ├── parameters_manager.h │ ├── parameters_manager.hpp │ ├── pclDownwardsCompatibility.h │ ├── piecewise_linear_function.h │ ├── piecewise_linear_function.hpp │ ├── pngIO.cpp │ ├── pngIO.h │ ├── runtime_error.cpp │ ├── runtime_error.h │ ├── sparsePointCloud.h │ ├── string_tools.cpp │ ├── string_tools.h │ ├── timeutil.cpp │ ├── timeutil.h │ ├── transformationRepresentation.h │ ├── vector_average.h │ └── vector_average.hpp ├── README.txt ├── remissionCalibrationMapCells.dat └── src ├── BackwardCpp ├── .clang-format ├── .gitignore ├── .travis.yml ├── BackwardConfig.cmake ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── backward.cpp ├── backward.hpp ├── builds.sh ├── conanfile.py ├── doc │ ├── nice.png │ ├── pretty.png │ └── rude.png ├── test │ ├── _test_main.cpp │ ├── rectrace.cpp │ ├── select_signals.cpp │ ├── stacktrace.cpp │ ├── suicide.cpp │ ├── test.cpp │ └── test.hpp └── test_package │ ├── CMakeLists.txt │ ├── conanfile.py │ └── main.cpp ├── g2oGraphElements.cpp ├── g2oGraphElements.h ├── g2oGraphElements.hpp ├── mapCellsExtractor.cpp ├── pointXYZIVPL.h ├── remissionCalibration.cpp ├── remissionCalibrationHelper.cpp ├── remissionCalibrationHelper.h ├── remissionCalibrationHelper.hpp ├── remissionCalibrationResult.cpp ├── remissionCalibrationResult.h ├── remissionCalibrationResult.hpp └── statisticsGenerator.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "compileCommands": "${workspaceFolder}/build/compile_commands.json", 6 | "includePath": [ 7 | "${workspaceFolder}/**" 8 | ], 9 | "defines": [], 10 | "compilerPath": "/usr/bin/gcc", 11 | "cStandard": "gnu17", 12 | "cppStandard": "gnu++14" 13 | } 14 | ], 15 | "version": 4 16 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "cctype": "cpp", 4 | "clocale": "cpp", 5 | "cmath": "cpp", 6 | "csignal": "cpp", 7 | "cstdarg": "cpp", 8 | "cstddef": "cpp", 9 | "cstdio": "cpp", 10 | "cstdlib": "cpp", 11 | "cstring": "cpp", 12 | "ctime": "cpp", 13 | "cwchar": "cpp", 14 | "cwctype": "cpp", 15 | "any": "cpp", 16 | "array": "cpp", 17 | "atomic": "cpp", 18 | "strstream": "cpp", 19 | "*.tcc": "cpp", 20 | "bitset": "cpp", 21 | "chrono": "cpp", 22 | "cinttypes": "cpp", 23 | "complex": "cpp", 24 | "condition_variable": "cpp", 25 | "cstdint": "cpp", 26 | "deque": "cpp", 27 | "list": "cpp", 28 | "unordered_map": "cpp", 29 | "unordered_set": "cpp", 30 | "vector": "cpp", 31 | "exception": "cpp", 32 | "algorithm": "cpp", 33 | "functional": "cpp", 34 | "iterator": "cpp", 35 | "map": "cpp", 36 | "memory": "cpp", 37 | "memory_resource": "cpp", 38 | "numeric": "cpp", 39 | "optional": "cpp", 40 | "random": "cpp", 41 | "ratio": "cpp", 42 | "set": "cpp", 43 | "string": "cpp", 44 | "string_view": "cpp", 45 | "system_error": "cpp", 46 | "tuple": "cpp", 47 | "type_traits": "cpp", 48 | "utility": "cpp", 49 | "fstream": "cpp", 50 | "future": "cpp", 51 | "initializer_list": "cpp", 52 | "iomanip": "cpp", 53 | "iosfwd": "cpp", 54 | "iostream": "cpp", 55 | "istream": "cpp", 56 | "limits": "cpp", 57 | "mutex": "cpp", 58 | "new": "cpp", 59 | "ostream": "cpp", 60 | "shared_mutex": "cpp", 61 | "sstream": "cpp", 62 | "stdexcept": "cpp", 63 | "streambuf": "cpp", 64 | "thread": "cpp", 65 | "cfenv": "cpp", 66 | "typeindex": "cpp", 67 | "typeinfo": "cpp", 68 | "variant": "cpp", 69 | "bit": "cpp" 70 | } 71 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 2 | 3 | project(remissionCalibration) 4 | 5 | # The library prefix 6 | SET(LIB_PREFIX ${PROJECT_NAME}_) 7 | 8 | SET(${PROJECT_NAME}_C_FLAGS) 9 | SET(${PROJECT_NAME}_CXX_FLAGS) 10 | 11 | # -Wno-error="deprecated-declarations deprecated-copy" 12 | # disable warnings 13 | set(CMAKE_CXX_FLAGS 14 | "${CMAK_CXX_FLAGS} -Wfatal-errors -Wno-deprecated-declarations -Wno-deprecated-copy") 15 | 16 | # default built type 17 | IF(NOT CMAKE_BUILD_TYPE) 18 | SET(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING 19 | "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." 20 | FORCE) 21 | ENDIF(NOT CMAKE_BUILD_TYPE) 22 | 23 | SET(${PROJECT_NAME}_LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib CACHE PATH "Output path for libraries") 24 | SET(${PROJECT_NAME}_EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin CACHE PATH "Output path for executables") 25 | 26 | # Set the output directory for the build executables and libraries 27 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${${PROJECT_NAME}_LIBRARY_OUTPUT_PATH}) 28 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${${PROJECT_NAME}_EXECUTABLE_OUTPUT_PATH}) 29 | IF(WIN32) 30 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_EXECUTABLE_OUTPUT_PATH}) 31 | ELSE() 32 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_LIBRARY_OUTPUT_PATH}) 33 | ENDIF() 34 | 35 | #Build type 36 | if (UNIX) 37 | SET(BUILD_SHARED_LIBS ON) 38 | ELSEIF (WIN32) 39 | SET(BUILD_SHARED_LIBS OFF) 40 | ENDIF() 41 | 42 | 43 | # PCL 44 | find_package(PCL 1.5 REQUIRED) 45 | include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) 46 | LINK_DIRECTORIES(${PCL_LIBRARY_DIRS}) 47 | add_definitions(${PCL_DEFINITIONS}) 48 | 49 | ADD_SUBDIRECTORY(EXTERNALS) 50 | add_subdirectory(src/BackwardCpp) 51 | 52 | INCLUDE_DIRECTORIES(${G2O_INCLUDE_DIRECTORY}) 53 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/EXTERNALS) 54 | LINK_DIRECTORIES(${${PROJECT_NAME}_LIBRARY_OUTPUT_PATH}) 55 | 56 | ADD_LIBRARY(remissionCalibrationLib 57 | src/g2oGraphElements.h src/g2oGraphElements.hpp src/g2oGraphElements.cpp 58 | src/pointXYZIVPL.h 59 | src/remissionCalibrationHelper.h src/remissionCalibrationHelper.hpp src/remissionCalibrationHelper.cpp 60 | src/remissionCalibrationResult.h src/remissionCalibrationResult.hpp src/remissionCalibrationResult.cpp 61 | ) 62 | TARGET_LINK_LIBRARIES(remissionCalibrationLib 63 | ${PCL_LIBRARIES} 64 | g2o_core g2o_solver_cholmod g2o_solver_csparse g2o_solver_dense 65 | #-lg2o_csparse_extension -lg2o_ext_freeglut_minimal -lg2o_opengl_helper -lg2o_solver_csparse -lg2o_solver_dense -lg2o_solver_pcg -lg2o_solver_slam2d_linear -lg2o_solver_structure_only -lg2o_stuff -lg2o_types_data -lg2o_types_icp -lg2o_types_sba -lg2o_types_sclam2d -lg2o_types_sim3 -lg2o_types_slam2d -lg2o_types_slam3d 66 | ais3dTools_basics 67 | ) 68 | # ADD_DEPENDENCIES(remissionCalibrationLib g2o ais3dTools) 69 | 70 | ADD_EXECUTABLE(mapCellsExtractor 71 | src/mapCellsExtractor.cpp 72 | ${BACKWARD_ENABLE}) 73 | TARGET_LINK_LIBRARIES(mapCellsExtractor 74 | remissionCalibrationLib 75 | dw) 76 | 77 | ADD_EXECUTABLE(remissionCalibration 78 | src/remissionCalibration.cpp 79 | ${BACKWARD_ENABLE}) 80 | TARGET_LINK_LIBRARIES(remissionCalibration 81 | remissionCalibrationLib 82 | dw) 83 | 84 | ADD_EXECUTABLE(statisticsGenerator src/statisticsGenerator.cpp) 85 | TARGET_LINK_LIBRARIES(statisticsGenerator remissionCalibrationLib) 86 | -------------------------------------------------------------------------------- /EXTERNALS/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(ExternalProject) 2 | 3 | ADD_SUBDIRECTORY(ais3dTools) 4 | 5 | # SET(G2O_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/g2o") 6 | # ExternalProject_Add( 7 | # g2o 8 | # DOWNLOAD_COMMAND "" 9 | # SOURCE_DIR ${G2O_SOURCE_DIR} 10 | # INSTALL_DIR ${CMAKE_BINARY_DIR} 11 | # #INSTALL_COMMAND "" 12 | # CMAKE_ARGS -Dg2o_RUNTIME_OUTPUT_DIRECTORY:PATH=${CMAKE_BINARY_DIR}/tmp/g2o-bin 13 | # -Dg2o_LIBRARY_OUTPUT_DIRECTORY:PATH=${CMAKE_BINARY_DIR}/tmp/g2o-lib 14 | # -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_BINARY_DIR} 15 | # ) 16 | # SET(G2O_INCLUDE_DIRECTORY "${CMAKE_BINARY_DIR}/include" CACHE STRING "G2O include dir") 17 | 18 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(ais3dTools) 2 | 3 | # The library prefix 4 | SET(LIB_PREFIX ${PROJECT_NAME}_) 5 | 6 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) 7 | ADD_SUBDIRECTORY(basics) 8 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR} ${QT_QTCORE_INCLUDE_DIR} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR} ${QT_QTCORE_INCLUDE_DIR}/..) 2 | 3 | ADD_LIBRARY(basics 4 | macros.h 5 | misc.h 6 | os_specific.h os_specific.c 7 | runtime_error.h runtime_error.cpp 8 | string_tools.h string_tools.cpp 9 | filesys_tools.h filesys_tools.cpp 10 | timeutil.h timeutil.cpp 11 | ais3dTools_stuff_api.h 12 | getopt.h getopt.c 13 | parameters_manager.h parameters_manager.cpp 14 | pngIO.h pngIO.cpp 15 | ) 16 | IF (UNIX) 17 | TARGET_LINK_LIBRARIES(basics rt) 18 | ENDIF(UNIX) 19 | 20 | SET_TARGET_PROPERTIES(basics PROPERTIES OUTPUT_NAME ${LIB_PREFIX}basics) 21 | 22 | TARGET_LINK_LIBRARIES(basics png) 23 | 24 | INSTALL(TARGETS basics 25 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 26 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 27 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 28 | ) 29 | 30 | #FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") 31 | 32 | #INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/ssa/alu_glwidget) 33 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/ais3dTools_stuff_api.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Description: import/export macros for creating DLLS with Microsoft 3 | * compiler. Any exported function needs to be declared with the 4 | * appropriate G2O_XXXX_API macro. Also, there must be separate macros 5 | * for each DLL (arrrrrgh!!!) 6 | * 7 | * 17 Jan 2012 8 | * Email: pupilli@cs.bris.ac.uk 9 | ****************************************************************************/ 10 | #ifndef AIS3DTOOLS_STUFF_API_H 11 | #define AIS3DTOOLS_STUFF_API_H 12 | 13 | #ifdef _MSC_VER 14 | // We are using a Microsoft compiler: 15 | 16 | #ifdef AIS3DTOOLS_SHARED_LIBS 17 | #ifdef basics_EXPORTS 18 | #define AIS3DTOOLS_STUFF_API __declspec(dllexport) 19 | #else 20 | #define AIS3DTOOLS_STUFF_API __declspec(dllimport) 21 | #endif 22 | #else 23 | #define AIS3DTOOLS_STUFF_API 24 | #endif 25 | 26 | #else 27 | // Not Microsoft compiler so set empty definition: 28 | #define AIS3DTOOLS_STUFF_API 29 | #endif 30 | 31 | #endif // AIS3DTOOLS_STUFF_API_H 32 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/filesys_tools.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * filesysTools.cpp 3 | * 4 | * Fr 02 Mär 2007 23:14:08 CET 5 | * Copyright 2007 Rainer Kümmerle 6 | * Email rk@raikue.net 7 | ****************************************************************************/ 8 | #include "filesys_tools.h" 9 | #include "macros.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #ifdef UNIX 19 | #include 20 | #include 21 | #endif //UNIX 22 | 23 | #ifdef WINDOWS 24 | #include 25 | #include 26 | typedef unsigned int uint; 27 | #endif //windows 28 | 29 | namespace Ais3dTools { 30 | 31 | std::string getFileExtension(const std::string& filename) 32 | { 33 | std::string::size_type lastDot = filename.find_last_of('.'); 34 | if (lastDot != std::string::npos) 35 | return filename.substr(lastDot + 1); 36 | else 37 | return ""; 38 | } 39 | 40 | std::string getPureFilename(const std::string& filename) 41 | { 42 | std::string::size_type lastDot = filename.find_last_of('.'); 43 | if (lastDot != std::string::npos) 44 | return filename.substr(0, lastDot); 45 | else 46 | return filename; 47 | } 48 | 49 | std::string getBasename(const std::string& filename) 50 | { 51 | char dirDivider = '/'; 52 | #if WINDOWS 53 | dirDivider = '\\'; 54 | #endif 55 | std::string::size_type lastSlash = filename.find_last_of(dirDivider); 56 | if (lastSlash != std::string::npos) 57 | return filename.substr(lastSlash + 1); 58 | else 59 | return filename; 60 | } 61 | 62 | std::string getDirname(const std::string& filename) 63 | { 64 | char dirDivider = '/'; 65 | #if WINDOWS 66 | dirDivider = '\\'; 67 | #endif 68 | std::string::size_type lastSlash = filename.find_last_of(dirDivider); 69 | if (lastSlash != std::string::npos) 70 | return filename.substr(0, lastSlash); 71 | else 72 | return ""; 73 | } 74 | 75 | std::string changeFileExtension(const std::string& filename, const std::string& newExt, bool stripDot) 76 | { 77 | std::string::size_type lastDot = filename.find_last_of('.'); 78 | if (lastDot != std::string::npos) { 79 | if (stripDot) 80 | return filename.substr(0, lastDot) + newExt; 81 | else 82 | return filename.substr(0, lastDot + 1) + newExt; 83 | } else 84 | return filename; 85 | } 86 | 87 | bool fileExists(const char* filename) 88 | { 89 | struct stat statInfo; 90 | return (stat(filename, &statInfo) == 0); 91 | } 92 | 93 | bool isRegularFile(const char* filename) 94 | { 95 | #ifdef WINDOWS 96 | std::cerr << __PRETTY_FUNCTION__ << ": not implemented on windows" << std::endl; 97 | return false; 98 | #else 99 | struct stat statInfo; 100 | return (stat(filename, &statInfo) == 0 && S_ISREG(statInfo.st_mode)); 101 | #endif 102 | } 103 | 104 | bool isDirectory(const char* filename) 105 | { 106 | #ifdef WINDOWS 107 | std::cerr << __PRETTY_FUNCTION__ << ": not implemented on windows" << std::endl; 108 | return false; 109 | #else 110 | struct stat statInfo; 111 | return (stat(filename, &statInfo) == 0 && S_ISDIR(statInfo.st_mode)); 112 | #endif 113 | } 114 | 115 | bool isSymbolicLink(const char* filename) 116 | { 117 | #ifdef WINDOWS 118 | return false; 119 | #else 120 | struct stat statInfo; 121 | return (lstat(filename, &statInfo) == 0 && S_ISLNK(statInfo.st_mode)); 122 | #endif 123 | } 124 | 125 | std::string getCurrentDateAsFilename() 126 | { 127 | #ifdef WINDOWS 128 | std::cerr << __PRETTY_FUNCTION__ << ": not implemented on windows" << std::endl; 129 | return std::string(); 130 | #else 131 | time_t t = time(NULL); 132 | const size_t dateStrSize = 1024; 133 | char dateStr[dateStrSize]; 134 | if (strftime(dateStr, dateStrSize, "%Y%m%d_%H%M%S", localtime(&t)) == 0) 135 | fprintf(stderr, "Error (%s: %s) Date: %s\n", __FILE__, __func__, dateStr); 136 | return std::string(dateStr); 137 | #endif 138 | } 139 | 140 | time_t getLastModificationDate(const char* filename) 141 | { 142 | struct stat statInfo; 143 | if (stat(filename, &statInfo) == 0) { 144 | return statInfo.st_mtime; 145 | } else { 146 | return 0; 147 | } 148 | } 149 | 150 | time_t getLastAccessDate(const char* filename) 151 | { 152 | struct stat statInfo; 153 | if (stat(filename, &statInfo) == 0) { 154 | return statInfo.st_atime; 155 | } else { 156 | return 0; 157 | } 158 | } 159 | 160 | time_t getLastStatusChangeDate(const char* filename) 161 | { 162 | struct stat statInfo; 163 | if (stat(filename, &statInfo) == 0) { 164 | return statInfo.st_ctime; 165 | } else { 166 | return 0; 167 | } 168 | } 169 | 170 | #ifdef UNIX 171 | bool createDirectory(const char* dirName, bool pub) 172 | { 173 | bool status = true; 174 | status &= (mkdir(dirName, 0) == 0); 175 | if (pub) 176 | status &= (0 == chmod(dirName, // set directory to rwxrwxrwx 177 | S_IRUSR | S_IWUSR | S_IXUSR | 178 | S_IRGRP | S_IWGRP | S_IXGRP | 179 | S_IROTH | S_IWOTH | S_IXOTH )); 180 | else 181 | status &= (0 == chmod(dirName, // set directory to rwxr-xr-x 182 | S_IRUSR | S_IWUSR | S_IXUSR | 183 | S_IRGRP | S_IXGRP | 184 | S_IROTH | S_IXOTH )); 185 | return status; 186 | } 187 | #endif 188 | 189 | #ifdef WINDOWS 190 | bool createDirectory(const char* dirName, bool pub) 191 | { 192 | std::cerr << __PRETTY_FUNCTION__ << ": not implemented on windows" << std::endl; 193 | bool status = true; 194 | //status &= (mkdir(dirName) == 0); 195 | return status; 196 | } 197 | #endif 198 | 199 | off_t getFileSize(const char* filename) 200 | { 201 | struct stat statInfo; 202 | if (stat(filename, &statInfo) == 0) { 203 | return statInfo.st_size; 204 | } else { 205 | return -1; 206 | } 207 | } 208 | 209 | std::vector getDirectoryElements(const char* dir, bool onlyFiles) 210 | { 211 | std::vector dirEntries; 212 | #ifdef UNIX 213 | DIR* curDir = opendir(dir); 214 | if(curDir == NULL) { 215 | return dirEntries; 216 | } 217 | 218 | struct dirent* dirEnt = readdir(curDir); 219 | while(dirEnt != NULL) { 220 | std::string name = dirEnt->d_name; 221 | 222 | if (name != "." && name != ".." && (!onlyFiles || dirEnt->d_type == DT_REG)) 223 | dirEntries.push_back(name); 224 | 225 | //#ifdef WINDOWS 226 | // if (name != "." && name != ".." && (!onlyFiles)) 227 | // dirEntries.push_back(name); 228 | //#endif //windows 229 | dirEnt = readdir(curDir); 230 | } 231 | closedir(curDir); 232 | #endif //linux 233 | #ifdef WINDOWS 234 | std::cerr << __PRETTY_FUNCTION__ << ": not implemented on windows" << std::endl; 235 | #endif 236 | return dirEntries; 237 | } 238 | 239 | std::vector getFilesByPattern(const char* pattern) 240 | { 241 | #ifdef UNIX 242 | std::vector result; 243 | wordexp_t p; 244 | wordexp(pattern, &p, 0); 245 | result.reserve(p.we_wordc); 246 | for (size_t i = 0; i < p.we_wordc; ++i) 247 | result.push_back(p.we_wordv[i]); 248 | wordfree(&p); 249 | return result; 250 | #endif //UNIX 251 | 252 | 253 | #ifdef WINDOWS 254 | std::cerr << "WARNING: " << __PRETTY_FUNCTION__ << " not implemented on windows target" << std::endl; 255 | return std::vector(); 256 | #endif// WINDOWS 257 | } 258 | 259 | std::string expandFilename(const std::string& filename) { 260 | std::vector filenames = getFilesByPattern(filename.c_str()); 261 | if (filenames.empty()) 262 | return ""; 263 | else 264 | return filenames.back(); 265 | } 266 | 267 | bool copyFile(const char* source, const char* dest) 268 | { 269 | FILE* in = fopen(source, "r"); 270 | FILE* out = fopen(dest, "w"); 271 | if (in == NULL || out == NULL) { 272 | if (in) 273 | fclose(in); 274 | if (out) 275 | fclose(out); 276 | return false; 277 | } 278 | 279 | // copy over the data 280 | int ch = EOF; 281 | while((ch = fgetc(in)) != EOF) { 282 | int wrote = fputc(ch, out); 283 | if (wrote == EOF) { 284 | fclose(in); 285 | fclose(out); 286 | return false; 287 | } 288 | } 289 | 290 | // clean up 291 | fclose(in); 292 | fclose(out); 293 | return true; 294 | } 295 | 296 | } 297 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/filesys_tools.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * filesysTools.h 3 | * 4 | * Fr 02 Mär 2007 23:14:21 CET 5 | * Copyright 2007 Rainer Kümmerle 6 | * Email rk@raikue.net 7 | ****************************************************************************/ 8 | 9 | #ifndef FILESYS_TOOLS_H 10 | #define FILESYS_TOOLS_H 11 | 12 | 13 | /** @addtogroup utils **/ 14 | // @{ 15 | 16 | /** \file filesysTools.h 17 | * \brief utility functions for handling files, directory on Linux/Unix 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace Ais3dTools { 25 | 26 | /** 27 | * get filename extension (the part after the last .), e.g. 28 | * the extension of file.txt is txt 29 | */ 30 | std::string getFileExtension(const std::string& filename); 31 | 32 | /** 33 | * get the filename without the extension. 34 | * file.txt -> file 35 | */ 36 | std::string getPureFilename(const std::string& filename); 37 | 38 | /** 39 | * change the fileextension of a given filename. 40 | * Only if filename contains an extension, otherwise filename is returned. 41 | */ 42 | std::string changeFileExtension(const std::string& filename, const std::string& newExt, bool stripDot = false); 43 | 44 | /** 45 | * return the basename of the given filename 46 | * /etc/fstab -> fstab 47 | */ 48 | std::string getBasename(const std::string& filename); 49 | 50 | /** 51 | * return the directory of a given filename 52 | * /etc/fstab -> /etc 53 | */ 54 | std::string getDirname(const std::string& filename); 55 | 56 | /** 57 | * check if file exists (note a directory is also a file) 58 | */ 59 | bool fileExists(const char* filename); 60 | 61 | /** 62 | * checks if file exists and is a file 63 | */ 64 | bool isRegularFile(const char* filename); 65 | 66 | /** 67 | * is the given filename a valid direcory, e.g. exists 68 | */ 69 | bool isDirectory(const char* filename); 70 | 71 | /** 72 | * is the given file a symbolic link 73 | */ 74 | bool isSymbolicLink(const char* filename); 75 | 76 | /** 77 | * return the size of the file in bytes. 78 | * @return the size of the file (type is off_t -> if large file support, then 64 bit). 79 | * -1 on error. 80 | */ 81 | off_t getFileSize(const char* filename); 82 | 83 | /** 84 | * return the current date as a filename of the form YYYYMMDD_hhmmss 85 | */ 86 | std::string getCurrentDateAsFilename(); 87 | 88 | /** return the modification date of a file */ 89 | time_t getLastModificationDate(const char* filename); 90 | 91 | /** return date of last access to a file */ 92 | time_t getLastAccessDate(const char* filename); 93 | 94 | /** return date of last status change of a file */ 95 | time_t getLastStatusChangeDate(const char* filename); 96 | 97 | /** 98 | * create a directory. if pub is true, then the rights of the dir 99 | * will be rwxrwxrwx (readable and writable for everyone) 100 | */ 101 | bool createDirectory(const char* dirName, bool pub = false); 102 | 103 | /** 104 | * return all filenames in the given directory, may also be a sub directory 105 | * if, onlyFiles is true, only files are returned 106 | */ 107 | std::vector getDirectoryElements(const char* dir, bool onlyFiles = false); 108 | 109 | /** 110 | * expand the given filename 111 | */ 112 | std::string expandFilename(const std::string& filename); 113 | 114 | /** 115 | * return all files that match a given pattern, e.g., ~/blaa*.txt, /tmp/a* 116 | */ 117 | std::vector getFilesByPattern(const char* pattern); 118 | 119 | /** 120 | * copy a file from a to b 121 | * @return true, if copy was sucessful 122 | */ 123 | bool copyFile(const char* source, const char* dest); 124 | 125 | } // end namespace 126 | // @} 127 | #endif 128 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/getopt.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * MODULE NAME : GETOPT.C 4 | * 5 | * COPYRIGHTS: 6 | * This module contains code made available by IBM 7 | * Corporation on an AS IS basis. Any one receiving the 8 | * module is considered to be licensed under IBM copyrights 9 | * to use the IBM-provided source code in any way he or she 10 | * deems fit, including copying it, compiling it, modifying 11 | * it, and redistributing it, with or without 12 | * modifications. No license under any IBM patents or 13 | * patent applications is to be implied from this copyright 14 | * license. 15 | * 16 | * A user of the module should understand that IBM cannot 17 | * provide technical support for the module and will not be 18 | * responsible for any consequences of use of the program. 19 | * 20 | * Any notices, including this one, are not to be removed 21 | * from the module without the prior written consent of 22 | * IBM. 23 | * 24 | * AUTHOR: Original author: 25 | * G. R. Blair (BOBBLAIR at AUSVM1) 26 | * Internet: bobblair@bobblair.austin.ibm.com 27 | * 28 | * Extensively revised by: 29 | * John Q. Walker II, Ph.D. (JOHHQ at RALVM6) 30 | * Internet: johnq@ralvm6.vnet.ibm.com 31 | * 32 | *****************************************************************************/ 33 | 34 | /****************************************************************************** 35 | * getopt() 36 | * 37 | * The getopt() function is a command line parser. It returns the next 38 | * option character in argv that matches an option character in opstring. 39 | * 40 | * The argv argument points to an array of argc+1 elements containing argc 41 | * pointers to character strings followed by a null pointer. 42 | * 43 | * The opstring argument points to a string of option characters; if an 44 | * option character is followed by a colon, the option is expected to have 45 | * an argument that may or may not be separated from it by white space. 46 | * The external variable optarg is set to point to the start of the option 47 | * argument on return from getopt(). 48 | * 49 | * The getopt() function places in optind the argv index of the next argument 50 | * to be processed. The system initializes the external variable optind to 51 | * 1 before the first call to getopt(). 52 | * 53 | * When all options have been processed (that is, up to the first nonoption 54 | * argument), getopt() returns EOF. The special option "--" may be used to 55 | * delimit the end of the options; EOF will be returned, and "--" will be 56 | * skipped. 57 | * 58 | * The getopt() function returns a question mark (?) when it encounters an 59 | * option character not included in opstring. This error message can be 60 | * disabled by setting opterr to zero. Otherwise, it returns the option 61 | * character that was detected. 62 | * 63 | * If the special option "--" is detected, or all options have been 64 | * processed, EOF is returned. 65 | * 66 | * Options are marked by either a minus sign (-) or a slash (/). 67 | * 68 | * No errors are defined. 69 | *****************************************************************************/ 70 | 71 | #if WINDOWS 72 | 73 | #include "getopt.h" 74 | #include /* for EOF */ 75 | #include /* for strchr() */ 76 | 77 | 78 | /* static (global) variables that are specified as exported by getopt() */ 79 | char *optarg = NULL; /* pointer to the start of the option argument */ 80 | int optind = 1; /* number of the next argv[] to be evaluated */ 81 | int opterr = 1; /* non-zero if a question mark should be returned 82 | when a non-valid option character is detected */ 83 | 84 | /* handle possible future character set concerns by putting this in a macro */ 85 | #define _next_char(string) (char)(*(string+1)) 86 | 87 | int getopt(int argc, char **argv, char *opstring) 88 | { 89 | static char *pIndexPosition = NULL; /* place inside current argv string */ 90 | char *pArgString = NULL; /* where to start from next */ 91 | char *pOptString; /* the string in our program */ 92 | 93 | 94 | if (pIndexPosition != NULL) { 95 | /* we last left off inside an argv string */ 96 | if (*(++pIndexPosition)) { 97 | /* there is more to come in the most recent argv */ 98 | pArgString = pIndexPosition; 99 | } 100 | } 101 | 102 | if (pArgString == NULL) { 103 | /* we didn't leave off in the middle of an argv string */ 104 | if (optind >= argc) { 105 | /* more command-line arguments than the argument count */ 106 | pIndexPosition = NULL; /* not in the middle of anything */ 107 | return EOF; /* used up all command-line arguments */ 108 | } 109 | 110 | /*--------------------------------------------------------------------- 111 | * If the next argv[] is not an option, there can be no more options. 112 | *-------------------------------------------------------------------*/ 113 | pArgString = argv[optind++]; /* set this to the next argument ptr */ 114 | 115 | if (('/' != *pArgString) && /* doesn't start with a slash or a dash? */ 116 | ('-' != *pArgString)) { 117 | --optind; /* point to current arg once we're done */ 118 | optarg = NULL; /* no argument follows the option */ 119 | pIndexPosition = NULL; /* not in the middle of anything */ 120 | return EOF; /* used up all the command-line flags */ 121 | } 122 | 123 | /* check for special end-of-flags markers */ 124 | if ((strcmp(pArgString, "-") == 0) || 125 | (strcmp(pArgString, "--") == 0)) { 126 | optarg = NULL; /* no argument follows the option */ 127 | pIndexPosition = NULL; /* not in the middle of anything */ 128 | return EOF; /* encountered the special flag */ 129 | } 130 | 131 | pArgString++; /* look past the / or - */ 132 | } 133 | 134 | if (':' == *pArgString) { /* is it a colon? */ 135 | /*--------------------------------------------------------------------- 136 | * Rare case: if opterr is non-zero, return a question mark; 137 | * otherwise, just return the colon we're on. 138 | *-------------------------------------------------------------------*/ 139 | return (opterr ? (int)'?' : (int)':'); 140 | } 141 | else if ((pOptString = strchr(opstring, *pArgString)) == 0) { 142 | /*--------------------------------------------------------------------- 143 | * The letter on the command-line wasn't any good. 144 | *-------------------------------------------------------------------*/ 145 | optarg = NULL; /* no argument follows the option */ 146 | pIndexPosition = NULL; /* not in the middle of anything */ 147 | return (opterr ? (int)'?' : (int)*pArgString); 148 | } 149 | else { 150 | /*--------------------------------------------------------------------- 151 | * The letter on the command-line matches one we expect to see 152 | *-------------------------------------------------------------------*/ 153 | if (':' == _next_char(pOptString)) { /* is the next letter a colon? */ 154 | /* It is a colon. Look for an argument string. */ 155 | if ('\0' != _next_char(pArgString)) { /* argument in this argv? */ 156 | optarg = &pArgString[1]; /* Yes, it is */ 157 | } 158 | else { 159 | /*------------------------------------------------------------- 160 | * The argument string must be in the next argv. 161 | * But, what if there is none (bad input from the user)? 162 | * In that case, return the letter, and optarg as NULL. 163 | *-----------------------------------------------------------*/ 164 | if (optind < argc) 165 | optarg = argv[optind++]; 166 | else { 167 | optarg = NULL; 168 | return (opterr ? (int)'?' : (int)*pArgString); 169 | } 170 | } 171 | pIndexPosition = NULL; /* not in the middle of anything */ 172 | } 173 | else { 174 | /* it's not a colon, so just return the letter */ 175 | optarg = NULL; /* no argument follows the option */ 176 | pIndexPosition = pArgString; /* point to the letter we're on */ 177 | } 178 | return (int)*pArgString; /* return the letter that matched */ 179 | } 180 | } 181 | 182 | #endif //WINDOWS 183 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/getopt.h: -------------------------------------------------------------------------------- 1 | /* 2 | POSIX getopt for Windows 3 | 4 | AT&T Public License 5 | 6 | Code given out at the 1985 UNIFORUM conference in Dallas. 7 | */ 8 | 9 | #ifdef __GNUC__ 10 | #include 11 | #endif 12 | #ifndef __GNUC__ 13 | 14 | #ifndef _WINGETOPT_H_ 15 | #define _WINGETOPT_H_ 16 | 17 | #ifdef __cplusplus 18 | extern "C" { 19 | #endif 20 | 21 | extern int opterr; 22 | extern int optind; 23 | extern int optopt; 24 | extern char *optarg; 25 | extern int getopt(int argc, char **argv, char *opts); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif /* _GETOPT_H_ */ 32 | #endif /* __GNUC__ */ 33 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/hashMap3D/hash_map_3d.h: -------------------------------------------------------------------------------- 1 | #ifndef HASH_MAP_3D_H 2 | #define HASH_MAP_3D_H 3 | 4 | #include 5 | #include 6 | 7 | namespace Ais3dTools{ 8 | 9 | template 10 | //struct HashMap3D : public std::tr1::unordered_map > > 11 | struct HashMap3D : public std::tr1::unordered_map > > 12 | { 13 | // ===== TYPEDEFS ===== 14 | typedef std::tr1::unordered_map MapZ; 15 | typedef std::tr1::unordered_map MapY; 16 | typedef std::tr1::unordered_map MapX; 17 | typedef typename MapZ::iterator IteratorZ; 18 | typedef typename MapZ::const_iterator ConstIteratorZ; 19 | typedef typename MapY::iterator IteratorY; 20 | typedef typename MapY::const_iterator ConstIteratorY; 21 | typedef typename MapX::iterator IteratorX; 22 | typedef typename MapX::const_iterator ConstIteratorX; 23 | 24 | // ===== CONSTRUCTOR ===== 25 | HashMap3D(float gridSize): _gridSize(gridSize), _worldToGrid(1.0f/_gridSize) {} 26 | 27 | // ===== PUBLIC MEMBER FUNCTIONS ===== 28 | inline float getGridSize() const { return _gridSize; } 29 | 30 | inline Eigen::Vector3f gridToWorld(const Eigen::Vector3i& index) const { 31 | Eigen::Vector3f point; 32 | point = _gridSize * index.cast(); 33 | return point; 34 | } 35 | 36 | inline Eigen::Vector3i worldToGrid(const Eigen::Vector3f& point) const { 37 | Eigen::Vector3i index; 38 | index(0) = floor(_worldToGrid * point(0)); 39 | index(1) = floor(_worldToGrid * point(1)); 40 | index(2) = floor(_worldToGrid * point(2)); 41 | return index; 42 | }; 43 | 44 | inline CellType& cell(const Eigen::Vector3i& index) { 45 | return (*this)[index(0)][index(1)][index(2)]; 46 | } 47 | 48 | inline CellType& cellFromPoint(const Eigen::Vector3f& point) { 49 | Eigen::Vector3i index = worldToGrid(point); 50 | return (*this)[index(0)][index(1)][index(2)]; 51 | } 52 | 53 | inline bool exists(const Eigen::Vector3i& index) const { 54 | MapZ& mapZ = (*this)[index(0)][index(1)]; 55 | ConstIteratorZ it = mapZ.find(index(2)); 56 | return it!=mapZ.end(); 57 | } 58 | 59 | inline CellType& findOrCreate(const Eigen::Vector3i& index, const CellType& defaultValue) { 60 | MapZ& mapZ = (*this)[index(0)][index(1)]; 61 | IteratorZ it = mapZ.find(index(2)); 62 | if (it == mapZ.end()) { // Not existing yet? 63 | CellType& ret = mapZ[index(2)]; 64 | ret = defaultValue; 65 | return ret; 66 | } 67 | else { 68 | return it->second; 69 | } 70 | } 71 | 72 | 73 | protected: 74 | // ===== PROTECTED MEMBER VARIABLES ===== 75 | float _gridSize; 76 | float _worldToGrid; 77 | 78 | }; 79 | 80 | } //end namespace 81 | #endif 82 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/hashMap3D/hash_map_3d_search.h: -------------------------------------------------------------------------------- 1 | #ifndef HASH_MAP_3D_SEARCH_H 2 | #define HASH_MAP_3D_SEARCH_H 3 | 4 | #include 5 | #include "hash_map_3d.h" 6 | #include "pcl/point_types.h" 7 | #include "pcl/point_cloud.h" 8 | 9 | 10 | namespace Ais3dTools{ 11 | 12 | struct HashMap3DSearchIndexComparator { 13 | public: 14 | 15 | HashMap3DSearchIndexComparator(std::vector* distances) : distanceArray(distances) { ; } 16 | 17 | bool operator() (int i, int j) { return (i < j);} 18 | 19 | private: 20 | std::vector* distanceArray; 21 | }; 22 | 23 | 24 | template 25 | struct HashMap3DSearchT : public HashMap3D > 26 | { 27 | 28 | HashMap3DSearchT(float gridSize): HashMap3D >(gridSize){ 29 | 30 | }; 31 | 32 | 33 | void setInput(typename pcl::PointCloud< PointType >::Ptr cloud) 34 | { 35 | pointcloud = cloud; 36 | for(size_t i = 0; i < cloud->points.size(); ++i){ 37 | Eigen::Vector3i index = worldToGrid(cloud->points[i].getVector3fMap()); 38 | cell(index).push_back(i); 39 | } 40 | } 41 | 42 | void radiusSearch(PointType point, float sqrDistance, std::vector& indices, std::vector& distances){ 43 | indices.clear(); 44 | distances.clear(); 45 | Eigen::Vector3f p = point.getVector3fMap(); 46 | Eigen::Vector3i index = worldToGrid(p); 47 | int searchRange = lrint(sqrt(sqrDistance) / _gridSize) + 1; 48 | for(int x = -searchRange; x <= searchRange; ++x) 49 | for(int y = -searchRange; y <= searchRange; ++y) 50 | for(int z = -searchRange; z <= searchRange; ++z){ 51 | Eigen::Vector3i cellIndex(x,y,z); 52 | cellIndex += index; 53 | 54 | for(size_t i = 0; i < cell(cellIndex).size(); ++i){ 55 | float sqrDist = (p - (*pointcloud)[cell(cellIndex)[i]].getVector3fMap()).squaredNorm(); 56 | if(sqrDist < sqrDistance){ 57 | indices.push_back(cell(cellIndex)[i]); 58 | distances.push_back(sqrDist); 59 | } 60 | } 61 | } 62 | 63 | 64 | ///sort results 65 | std::vector sorted_indices(indices.size()); 66 | std::vector sorted_distances(indices.size()); 67 | 68 | std::vector indexWrap(indices.size()); 69 | for(size_t i = 0; i < indexWrap.size(); ++i) 70 | indexWrap[i] = i; 71 | std::sort(indexWrap.begin(), indexWrap.end(), HashMap3DSearchIndexComparator(&distances)); 72 | 73 | for(size_t i = 0; i < indexWrap.size(); ++i){ 74 | sorted_indices[indexWrap[i]] = indices[i]; 75 | sorted_distances[indexWrap[i]] = distances[i]; 76 | } 77 | 78 | std::swap(sorted_indices, indices); 79 | std::swap(sorted_distances, distances); 80 | } 81 | 82 | void nearestNeighborSearch(PointType& point, float& sqrDistance, std::vector& indices, std::vector& distances){ 83 | indices.clear(); 84 | distances.clear(); 85 | Eigen::Vector3f p = point.getVector3fMap(); 86 | Eigen::Vector3i index = worldToGrid(p); 87 | int searchRange = lrint(sqrt(sqrDistance) / _gridSize); 88 | float minDist = sqrDistance + 1e-5; 89 | int minId = -1; 90 | for(int x = -searchRange; x <= searchRange; ++x) 91 | for(int y = -searchRange; y <= searchRange; ++y) 92 | for(int z = -searchRange; z <= searchRange; ++z){ 93 | Eigen::Vector3i cellIndex(x,y,z); 94 | cellIndex += index; 95 | std::vector& cellIndices = cell(cellIndex); 96 | Eigen::Vector3f cp; 97 | ///check if min distance to bin is larger that already found best candidate distance 98 | Eigen::Vector3f cellCenter = gridToWorld(cellIndex); 99 | float centerdist = std::max(0.0f, (cellCenter - p).norm() - (1.5f * _gridSize)); 100 | centerdist *= centerdist; 101 | 102 | if(centerdist < minDist) 103 | for(size_t i = 0; i < cellIndices.size(); ++i){ 104 | cp = pointcloud->points[cellIndices[i]].getVector3fMap(); 105 | float sqrDist = (p(0) - cp(0)) * (p(0) - cp(0)) + (p(1) - cp(1)) * (p(1) - cp(1)) + (p(2) - cp(2)) * (p(2) - cp(2)); 106 | //(p - pointcloud->points[cellIndices[i]].getVector3fMap()).squaredNorm(); 107 | if(sqrDist < minDist){ 108 | minDist = sqrDist; 109 | minId = cellIndices[i]; 110 | } 111 | } 112 | } 113 | if(minId > 0){ 114 | indices.push_back(minId); 115 | distances.push_back(minDist); 116 | } 117 | } 118 | 119 | 120 | typename pcl::PointCloud< PointType >::Ptr pointcloud; 121 | std::tr1::unordered_map > > valid; 122 | }; 123 | 124 | } //end namespace 125 | #endif -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/hashMap3D/map_3d.h: -------------------------------------------------------------------------------- 1 | #ifndef HASH_MAP_3D_H 2 | #define HASH_MAP_3D_H 3 | 4 | #include 5 | #include 6 | 7 | namespace Ais3dTools{ 8 | 9 | template 10 | //struct HashMap3D : public std::tr1::unordered_map > > 11 | struct HashMap3D : public std::vector< std::tr1::unordered_map > 12 | { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | HashMap3D(float gridSize, Eigen::Vector3i& size): _gridSize(gridSize), _origin(Eigen::Vector3f(0.0f, 0.0f, 0.0f)), _size(size), _center(size/2){ 16 | allocate(); 17 | 18 | }; 19 | 20 | HashMap3D(float gridSize, Eigen::Vector3f& origin, Eigen::Vector3i& size): _gridSize(gridSize), _origin(origin), _size(size), _center(size/2){ 21 | allocate(); 22 | }; 23 | 24 | void allocate(){ 25 | this->resize(_size(0)*_size(1)); 26 | } 27 | 28 | /** has to be called bevore adding first point */ 29 | void setOrigin(Eigen::Vector3f& origin){ _origin = origin;} 30 | 31 | Eigen::Vector3f gridToWorld(Eigen::Vector3i index){ 32 | index -= _center; 33 | Eigen::Vector3f point; 34 | point = _gridSize * index.cast(); 35 | point += _origin; 36 | return point; 37 | }; 38 | 39 | Eigen::Vector3i worldToGrid(Eigen::Vector3f point){ 40 | point = point - _origin; 41 | Eigen::Vector3i index; 42 | index(0) = floor(point(0) / _gridSize); 43 | index(1) = floor(point(1) / _gridSize); 44 | index(2) = floor(point(2) / _gridSize); 45 | index += _center; 46 | return index; 47 | }; 48 | 49 | bool isInside(Eigen::Vector3i& index){ 50 | if(index(0) > 0 && index(0) < _size(0) && index(1) > 0 && index(1) < _size(1)) 51 | return true; 52 | return false; 53 | } 54 | 55 | CellType& cell(Eigen::Vector3i& index){ 56 | return (*this)[index(0) + (_size(0) * index(1))][index(2)]; 57 | } 58 | 59 | protected: 60 | float _gridSize; 61 | Eigen::Vector3f _origin; 62 | Eigen::Vector3i _size; 63 | Eigen::Vector3i _center; 64 | 65 | }; 66 | 67 | } //end namespace 68 | #endif -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/macros.h: -------------------------------------------------------------------------------- 1 | #ifndef MACROS_H 2 | #define MACROS_H 3 | 4 | #include 5 | 6 | #ifndef DEG2RAD 7 | #define DEG2RAD(x) ((x) * 0.01745329251994329575) 8 | #endif 9 | 10 | #ifndef RAD2DEG 11 | #define RAD2DEG(x) ((x) * 57.29577951308232087721) 12 | #endif 13 | 14 | //usleep 15 | #ifdef WIN32 16 | #include 17 | #define ais_sleep(x) Sleep(1000*(x)) 18 | #define ais_ms_sleep(x) Sleep(x) 19 | #else 20 | #define ais_sleep(x) sleep(x) 21 | #define ais_ms_sleep(x) usleep(1000*(x)) 22 | #endif 23 | 24 | #include 25 | #include 26 | 27 | // MSVC on Windows 28 | #ifdef _MSC_VER 29 | #define __PRETTY_FUNCTION__ __FUNCTION__ 30 | 31 | template 32 | __inline bool isnan(real x) { return bool(_isnan(x)!=0); } 33 | template 34 | __inline bool isfinite(real x) { return bool(_finite(x)!=0); } 35 | template 36 | __inline bool isinf(real x) { return !isfinite(x) && !isnan(x); } 37 | 38 | #define INFINITY std::numeric_limits::infinity() 39 | #define NAN std::numeric_limits::quiet_NaN() 40 | 41 | __inline double round(double number) { return (number < 0.0 ? ceil(number - 0.5) : floor(number + 0.5)); } 42 | __inline float round(float number) { return (number < 0.0f ? ceil(number - 0.5f) : floor(number + 0.5f));} 43 | 44 | template 45 | __inline long int lrint(real x) { return static_cast(round(x)); } 46 | __inline long int lrintf(float x) { return lrint(x); } 47 | 48 | namespace std { 49 | using ::isnan; 50 | using ::isfinite; 51 | using ::isinf; 52 | using ::round; 53 | using ::abs; 54 | using ::lrint; 55 | using ::lrintf; 56 | using ::log; 57 | } 58 | #else 59 | using std::isnan; 60 | using std::isfinite; 61 | using std::isinf; 62 | #endif 63 | 64 | // some macros that are only useful for c++ 65 | #ifdef __cplusplus 66 | 67 | #define READ_FILE(fvar,fname) \ 68 | std::ifstream fvar(fname);\ 69 | if (!fvar) throw std::runtime_error("Could not open file for reading: "#fname); 70 | #define WRITE_FILE(fvar,fname) \ 71 | std::ofstream fvar(fname);\ 72 | if (!fvar) throw std::runtime_error("Could not open file for writing "#fname); 73 | #define WRITE_VRML_FILE(fvar,fname) \ 74 | std::ofstream fvar(fname);\ 75 | if (!fvar) throw std::runtime_error("Could not open file for writing "#fname);\ 76 | fvar << "#VRML V2.0 utf8\n"; 77 | #define FSKIP_LINE(f) \ 78 | {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);} 79 | #define FSKIP_WHITESPACES(f) \ 80 | {char c; do f.get(c); while(isspace(c) && f.good() && !(f).eof()); f.unget();} 81 | #define FCOPY_LINE(f, g) \ 82 | {char _c=' ';while(_c != '\n' && !(f).eof()){ (f).get(_c); (g).put(_c);}} 83 | #define FGET_LINE(f,l) \ 84 | {l = ""; char c=' '; while(c!='\n' && !f.eof()) l+=c=f.get();} 85 | #define DEL_FEXT(fname) \ 86 | {std::string::size_type d = fname.find_last_of('.'); \ 87 | if (d!=std::string::npos) fname = fname.substr(0,d);} 88 | #define GET_FEXT(fname,fext) \ 89 | {std::string::size_type d = fname.find_last_of('.'); \ 90 | fext = (d!=std::string::npos) ? fname.substr(d+1) : "";} 91 | #define TO_UPPER(s) \ 92 | std::transform(s.begin(), s.end(), s.begin(), (int(*)(int)) std::toupper) 93 | #define TO_LOWER(s) \ 94 | std::transform(s.begin(), s.end(), s.begin(), (int(*)(int)) std::tolower) 95 | 96 | #ifndef PVAR 97 | #define PVAR(s) \ 98 | #s << " = " << (s) << std::flush 99 | #endif 100 | #ifndef PVARN 101 | #define PVARN(s) \ 102 | #s << " = " << (s) << "\n" 103 | #endif 104 | #ifndef PVARC 105 | #define PVARC(s) \ 106 | #s << " = " << (s) << ", " << std::flush 107 | #endif 108 | #ifndef PVARS 109 | #define PVARS(s) \ 110 | #s << " = " << (s) << " " << std::flush 111 | #endif 112 | #ifndef PVARA 113 | #define PVARA(s) \ 114 | #s << " = " << RAD2DEG(s) << "deg" << std::flush 115 | #endif 116 | #ifndef PVARAN 117 | #define PVARAN(s) \ 118 | #s << " = " << RAD2DEG(s) << "deg\n" 119 | #endif 120 | #ifndef PVARAC 121 | #define PVARAC(s) \ 122 | #s << " = " << RAD2DEG(s) << "deg, " << std::flush 123 | #endif 124 | #ifndef PVARAS 125 | #define PVARAS(s) \ 126 | #s << " = " << RAD2DEG(s) << "deg " << std::flush 127 | #endif 128 | 129 | #define FIXED(s) \ 130 | std::fixed << s << std::resetiosflags(std::ios_base::fixed) 131 | 132 | #endif // __cplusplus 133 | 134 | #ifndef GET_COLOR 135 | #define GET_COLOR(i, size, r, g, b) \ 136 | ((floor(i*6./(double)size) == 0) || (floor(i*6./(double)size) == 6) ? \ 137 | (r=1,g=(i*6./size-floor(i*6./size)),b=0) : \ 138 | ((floor(i*6./(double)size) == 1) ? (r=1.-(i*6./size-floor(i*6./size)),g=1,b=0) : \ 139 | ((floor(i*6./(double)size) == 2) ? (r=0,g=1,b=(i*6./size-floor(i*6./size))) : \ 140 | ((floor(i*6./(double)size) == 3) ? (r=0,g=1-(i*6./size-floor(i*6./size)), b=1) : \ 141 | ((floor(i*6./(double)size) == 4) ? (r=(i*6./size-floor(i*6./size)),g=0, b=1) : \ 142 | (r=1,g=0, b=1-(i*6./size-floor(i*6./size)))))))) 143 | #endif 144 | 145 | #define PREC 1e-12 146 | #define SQRT2 1.414213562373095145474621858738828450441 147 | #define SQRT3 1.732050807568877293527446341505872366943 148 | 149 | #ifndef MIN 150 | #define MIN(a,b) ((a)<(b)?(a):(b)) 151 | #endif 152 | 153 | #ifndef MIN3 154 | #define MIN3(a,b,c) MIN((a),MIN((b),(c))) 155 | #endif 156 | 157 | #ifndef MIN4 158 | #define MIN4(a,b,c,d) MIN((d),MIN3((a),(b),(c))) 159 | #endif 160 | 161 | #ifndef MAX 162 | #define MAX(a,b) ((a)>(b)?(a):(b)) 163 | #endif 164 | 165 | #ifndef MAX3 166 | #define MAX3(a,b,c) MAX((a),MAX((b),(c))) 167 | #endif 168 | 169 | #ifndef MAX4 170 | #define MAX4(a,b,c,d) MAX((d),MAX3((a),(b),(c))) 171 | #endif 172 | 173 | #ifndef SQR 174 | #define SQR(x) ((x)*(x)) 175 | #endif 176 | 177 | #ifndef CUB 178 | #define CUB(x) ((x)*(x)*(x)) 179 | #endif 180 | 181 | #ifndef HYPOT 182 | #define HYPOT(x,y) (sqrt(SQR(x) + SQR(y))) 183 | #endif 184 | 185 | #ifndef HYPOT_SQUARED 186 | #define HYPOT_SQUARED(x,y) (SQR(x) + SQR(y)) 187 | #endif 188 | 189 | #ifndef ROUND 190 | #define ROUND(x) ((x) < 0 ? (int)((x) - .5): (int) ((x) + .5)) 191 | #endif 192 | 193 | #ifndef IS_INT 194 | #define IS_INT(a) (fabs(ROUND(a)-(a))0?1:((a)<0?-1:0)) 199 | #endif 200 | 201 | #ifndef IS_ZERO 202 | #define IS_ZERO(f) (fabs(f) < 1e-12) 203 | #endif 204 | 205 | #ifndef IS_EQUAL 206 | #define IS_EQUAL(f, g) (fabs(f - g) < 1e-12) 207 | #endif 208 | 209 | #ifndef ERASE_STRUCT 210 | #define ERASE_STRUCT(var) memset(&var, 0, sizeof(var)) 211 | #endif 212 | 213 | #ifndef ERASE_ARRAY 214 | #define ERASE_ARRAY(var, size) memset(var, 0, size*sizeof(*var)) 215 | #endif 216 | 217 | #ifndef SET_ARRAY 218 | #define SET_ARRAY(var, value, size) {for (int i=0; i<(int)size; ++i) var[i]=value;} 219 | #endif 220 | 221 | // Helper macros for builtin compiler support. 222 | // If your compiler has builtin support for any of the following 223 | // traits concepts, then redefine the appropriate macros to pick 224 | // up on the compiler support: 225 | // 226 | // (these should largely ignore cv-qualifiers) 227 | // AIS_IS_POD(T) should evaluate to true if T is a POD type 228 | 229 | // gcc >= 4.3 230 | #if defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ >= 3) && !defined(__GCCXML__))) 231 | #define AISNAV_IS_POD(T) __is_pod(T) 232 | #endif 233 | 234 | // default compiler 235 | #ifndef AISNAV_IS_POD 236 | #define AISNAV_IS_POD(T) (0) 237 | #endif 238 | 239 | #endif 240 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/message_queue_t.h: -------------------------------------------------------------------------------- 1 | #ifndef MESSAGE_QUEUE_T_H 2 | #define MESSAGE_QUEUE_T_H 3 | 4 | #include 5 | 6 | namespace Ais3dTools{ 7 | 8 | template 9 | class MessageQueueT 10 | { 11 | public: 12 | typedef std::map Buffer; 13 | 14 | public: 15 | MessageQueueT(); 16 | ~MessageQueueT(); 17 | 18 | void add(double timestamp, ElementType& rd); 19 | 20 | ElementType findClosestData(double timestamp) const; 21 | 22 | ElementType before(double timestamp) const; 23 | ElementType after(double timestamp) const; 24 | 25 | ElementType at(double timestamp); 26 | 27 | const Buffer& buffer() const {return _buffer;} 28 | 29 | protected: 30 | Buffer _buffer; 31 | }; 32 | 33 | #include "message_queue_t.hpp" 34 | 35 | } //end namespace 36 | 37 | #endif 38 | 39 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/message_queue_t.hpp: -------------------------------------------------------------------------------- 1 | 2 | #include "message_queue_t.h" 3 | #include 4 | 5 | template 6 | MessageQueueT::MessageQueueT() 7 | { 8 | } 9 | 10 | template 11 | MessageQueueT::~MessageQueueT() 12 | { 13 | } 14 | 15 | template 16 | ElementType MessageQueueT::findClosestData(double timestamp) const 17 | { 18 | if (_buffer.rbegin()->first < timestamp) 19 | return _buffer.rbegin()->second; 20 | if (_buffer.begin()->first > timestamp) 21 | return _buffer.begin()->second; 22 | 23 | typename Buffer::const_iterator ub = _buffer.upper_bound(timestamp); 24 | typename Buffer::const_iterator lb = ub; 25 | --lb; 26 | if (fabs(lb->first - timestamp) < fabs(ub->first - timestamp)) 27 | return lb->second; 28 | else 29 | return ub->second; 30 | } 31 | 32 | template 33 | ElementType MessageQueueT::before(double timestamp) const 34 | { 35 | if (_buffer.size() == 0 || _buffer.begin()->first > timestamp) 36 | return 0; 37 | typename Buffer::const_iterator lb = _buffer.upper_bound(timestamp); 38 | --lb; // now it's the lower bound 39 | return lb->second; 40 | } 41 | 42 | template 43 | ElementType MessageQueueT::after(double timestamp) const 44 | { 45 | if (_buffer.size() == 0 || _buffer.rbegin()->first < timestamp) 46 | return 0; 47 | typename Buffer::const_iterator ub = _buffer.upper_bound(timestamp); 48 | if (ub == _buffer.end()) 49 | return 0; 50 | return ub->second; 51 | } 52 | 53 | template 54 | ElementType MessageQueueT::at(double timestamp) 55 | { 56 | return _buffer[timestamp]; 57 | } 58 | 59 | template 60 | void MessageQueueT::add(double timestamp, ElementType& rd) 61 | { 62 | _buffer[timestamp] = rd; 63 | } 64 | 65 | 66 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/misc.h: -------------------------------------------------------------------------------- 1 | #ifndef STUFF_MISC_H 2 | #define STUFF_MISC_H 3 | 4 | #include 5 | 6 | /** @addtogroup utils **/ 7 | // @{ 8 | 9 | /** \file misc.h 10 | * \brief some general case utility functions 11 | * 12 | * This file specifies some general case utility functions 13 | **/ 14 | 15 | namespace Ais3dTools { 16 | 17 | /** 18 | * return the square value 19 | */ 20 | template 21 | inline T square(T x) 22 | { 23 | return x*x; 24 | } 25 | 26 | /** 27 | * return the hypot of x and y 28 | */ 29 | template 30 | inline T hypot(T x, T y) 31 | { 32 | return (T) (sqrt(x*x + y*y)); 33 | } 34 | 35 | /** 36 | * return the squared hypot of x and y 37 | */ 38 | template 39 | inline T hypot_sqr(T x, T y) 40 | { 41 | return x*x + y*y; 42 | } 43 | 44 | /** 45 | * convert from degree to radian 46 | */ 47 | inline double deg2rad(double degree) 48 | { 49 | return degree * 0.01745329251994329576; 50 | } 51 | 52 | /** 53 | * convert from radian to degree 54 | */ 55 | inline double rad2deg(double rad) 56 | { 57 | return rad * 57.29577951308232087721; 58 | } 59 | 60 | /** 61 | * normalize the angle 62 | */ 63 | template 64 | inline real normalize_theta(real theta) 65 | { 66 | real multiplier; 67 | 68 | if (theta >= -M_PI && theta < M_PI) 69 | return theta; 70 | 71 | multiplier = floor(theta / (2*M_PI)); 72 | theta = theta - multiplier*2*M_PI; 73 | if (theta >= M_PI) 74 | theta -= 2*M_PI; 75 | if (theta < -M_PI) 76 | theta += 2*M_PI; 77 | 78 | return theta; 79 | } 80 | 81 | /** 82 | * inverse of an angle, i.e., +180 degree 83 | */ 84 | inline double inverse_theta(double th) 85 | { 86 | return normalize_theta(th + M_PI); 87 | } 88 | 89 | /** 90 | * average two angles 91 | */ 92 | inline double average_angle(double theta1, double theta2) 93 | { 94 | double x, y; 95 | 96 | x = cos(theta1) + cos(theta2); 97 | y = sin(theta1) + sin(theta2); 98 | if(x == 0 && y == 0) 99 | return 0; 100 | else 101 | return std::atan2(y, x); 102 | } 103 | 104 | /** 105 | * sign function. 106 | * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0 107 | */ 108 | template 109 | inline int sign(T x) 110 | { 111 | if (x > 0) 112 | return 1; 113 | else if (x < 0) 114 | return -1; 115 | else 116 | return 0; 117 | } 118 | 119 | /** 120 | * clamp x to the interval [l, u] 121 | */ 122 | template 123 | inline T clamp(T l, T x, T u) 124 | { 125 | if (x < l) 126 | return l; 127 | if (x > u) 128 | return u; 129 | return x; 130 | } 131 | 132 | /** 133 | * wrap x to be in the interval [l, u] 134 | */ 135 | template 136 | inline T wrap(T l, T x, T u) 137 | { 138 | T intervalWidth = u - l; 139 | while (x < l) 140 | x += intervalWidth; 141 | while (x > u) 142 | x -= intervalWidth; 143 | return x; 144 | } 145 | 146 | } // end namespace 147 | 148 | // @} 149 | 150 | #endif 151 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/os_specific.c: -------------------------------------------------------------------------------- 1 | #include "os_specific.h" 2 | 3 | #ifdef WINDOWS 4 | 5 | int vasprintf(char** strp, const char* fmt, va_list ap){ 6 | int n; 7 | int size = 100; 8 | char* p; 9 | char* np; 10 | 11 | if ((p = (char*)malloc(size * sizeof(char))) == NULL) 12 | return -1; 13 | 14 | while (1) { 15 | n = vsnprintf(p, size, fmt, ap); 16 | if (n > -1 && n < size) { 17 | *strp = p; 18 | return n; 19 | } 20 | if (n > -1) 21 | size = n+1; 22 | else 23 | size *= 2; 24 | if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { 25 | free(p); 26 | return -1; 27 | } else 28 | p = np; 29 | } 30 | } 31 | 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/os_specific.h: -------------------------------------------------------------------------------- 1 | #ifndef _OS_SPECIFIC_HH_ 2 | #define _OS_SPECIFIC_HH_ 3 | 4 | //defines drand48 and getCurrentTime 5 | 6 | #ifdef WINDOWS 7 | #include 8 | #include 9 | #include 10 | typedef unsigned int uint; 11 | #define drand48() ((double) rand()/(double)RAND_MAX) 12 | 13 | #ifdef __cplusplus 14 | extern "C" { 15 | #endif 16 | 17 | int vasprintf(char** strp, const char* fmt, va_list ap); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | 23 | #endif 24 | 25 | #ifdef LINUX 26 | #include 27 | // nothing to do on real operating systems 28 | #endif 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/parameters_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "parameters_manager.h" 2 | 3 | #include 4 | //#include 5 | //#include 6 | 7 | //////////////////////////////////////////////////////////// 8 | 9 | Ais3dTools::ParametersManager::ParametersManager(const std::string& filename) : filename_(filename), last_file_modification_time_(0) 10 | { 11 | } 12 | 13 | //////////////////////////////////////////////////////////// 14 | 15 | void Ais3dTools::ParametersManager::updateParameters() { 16 | struct stat fileStats; 17 | stat(filename_.c_str(), &fileStats); 18 | if (fileStats.st_mtime != last_file_modification_time_) { 19 | //std::cout << "File changed.\n"; 20 | readParametersFile(); 21 | last_file_modification_time_ = fileStats.st_mtime; 22 | } 23 | else { 24 | //std::cout << "File remains unchanged.\n"; 25 | } 26 | } 27 | 28 | //////////////////////////////////////////////////////////// 29 | 30 | void Ais3dTools::ParametersManager::readParametersFile() { 31 | std::ifstream file(filename_.c_str()); 32 | if (!file) { 33 | std::cerr << "Could not read parameters file \""<> token; 44 | //std::cout << PVARN(token); 45 | if (token.empty()) 46 | continue; 47 | if (token[0]=='[') { 48 | if (token[token.size()-1] != ']' || token.size()<3) { 49 | std::cerr << "Error reading category tag \""<> parameterValue; 62 | if (parameterValue.empty()) { 63 | std::cerr << "Error reading value of parameter \""<first<<"]\n"; 74 | //for (ParameterEntries::const_iterator it2=it->second.begin(); it2!=it->second.end(); ++it2) { 75 | //std::cout << " "<first<<" = "<second<<"\n"; 76 | //} 77 | //} 78 | //std::cout << "\n\n"; 79 | } 80 | 81 | //////////////////////////////////////////////////////////// 82 | 83 | void Ais3dTools::ParametersManager::clear() { 84 | parameters_.clear(); 85 | } 86 | 87 | //////////////////////////////////////////////////////////// 88 | 89 | void Ais3dTools::ParametersManager::writeToFile(const std::string& filename) const { 90 | std::ofstream file(filename.c_str(), std::ios_base::out|std::ios_base::trunc); 91 | if (!file) { 92 | std::cerr << "Could not open file \""<first<<"]\n"; 97 | for (ParameterEntries::const_iterator it2=it->second.begin(); it2!=it->second.end(); ++it2) { 98 | file << " "<first<<" "<second<<"\n"; 99 | } 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/parameters_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef AIS3DTOOLS_PARAMETERS_MANAGER_H 2 | #define AIS3DTOOLS_PARAMETERS_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace Ais3dTools { 10 | 11 | class ParametersManager { 12 | public: 13 | ////////// CONSTRUCTOR ////////// 14 | ParametersManager(const std::string& filename); 15 | 16 | ////////// PUBLIC MEMBER FUNCTIONS ////////// 17 | //! Clears all the saved parameters 18 | void clear(); 19 | 20 | //! Gets the current value of a parameter. Returns true if the value was changed 21 | template 22 | bool getParameterValue(const std::string& category, const std::string& parameterName, ParameterType& parameter) const; 23 | 24 | /** Add/changes the current parameter. Returns true if it either changed or added the parameter 25 | * \param onlyIfNew controls if the value should also be changed if an entry already exists */ 26 | template 27 | bool addParameterValue(const std::string& category, const std::string& parameterName, const ParameterType parameter, bool onlyIfNew=false); 28 | 29 | //! Checks if the parameters in the file changed 30 | void updateParameters(); 31 | 32 | //! Writes the current parameter set to a file 33 | void writeToFile(const std::string& filename) const; 34 | 35 | protected: 36 | ////////// PROTECTED MEMBER FUNCTIONS ////////// 37 | void readParametersFile(); 38 | 39 | 40 | ////////// PROTECTED MEMBER VARIABLES ////////// 41 | typedef std::map ParameterEntries; 42 | typedef std::map ParameterEntriesPerCategory; 43 | ParameterEntriesPerCategory parameters_; 44 | std::string filename_; 45 | time_t last_file_modification_time_; 46 | }; 47 | 48 | } // namespace end 49 | 50 | #include "parameters_manager.hpp" 51 | 52 | #endif // AIS3DTOOLS_PARAMETERS_MANAGER_H 53 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/parameters_manager.hpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////// 2 | 3 | template 4 | bool Ais3dTools::ParametersManager::getParameterValue(const std::string& category, const std::string& parameterName, ParameterType& parameter) const { 5 | const ParameterEntriesPerCategory::const_iterator it = parameters_.find(category); 6 | if (it==parameters_.end()) { 7 | std::cerr << "Unknown category \""<second.find(parameterName); 11 | if (it2==it->second.end()) { 12 | std::cerr << "Unknown parameter \""<second); 17 | ss >> newParamValue; 18 | //std::cout << "Parameter \""<second<<"\".\n"; 19 | if (newParamValue == parameter) { 20 | //std::cout << "Parameter \""< 29 | bool Ais3dTools::ParametersManager::addParameterValue(const std::string& category, const std::string& parameterName, 30 | const ParameterType parameter, bool onlyIfNew) 31 | { 32 | ParameterEntries& entriesInCategory = parameters_[category]; 33 | const ParameterEntries::const_iterator it = entriesInCategory.find(parameterName); 34 | if (it==entriesInCategory.end()) { 35 | std::stringstream tmpStringStream; 36 | tmpStringStream << parameter; 37 | entriesInCategory[parameterName] = tmpStringStream.str(); 38 | return true; 39 | } 40 | 41 | if (onlyIfNew) 42 | return false; 43 | 44 | ParameterType oldParamValue; 45 | std::stringstream tmpStringStream(it->second); 46 | tmpStringStream >> oldParamValue; 47 | if (oldParamValue == parameter) 48 | return false; 49 | 50 | //std::cout << "Parameter \""< 42 | 43 | namespace pcl 44 | { 45 | /** 46 | * \brief Provides functionalities to efficiently return values for piecewise linear function 47 | * \ingroup common 48 | */ 49 | class PiecewiseLinearFunction 50 | { 51 | public: 52 | // =====CONSTRUCTOR & DESTRUCTOR===== 53 | //! Constructor 54 | PiecewiseLinearFunction (float factor, float offset); 55 | 56 | // =====PUBLIC METHODS===== 57 | //! Get the list of known data points 58 | std::vector& 59 | getDataPoints () 60 | { 61 | return data_points_; 62 | } 63 | 64 | //! Get the value of the function at the given point 65 | inline float 66 | getValue (float point) const; 67 | 68 | // =====PUBLIC MEMBER VARIABLES===== 69 | 70 | protected: 71 | // =====PROTECTED MEMBER VARIABLES===== 72 | std::vector data_points_; 73 | float factor_; 74 | float offset_; 75 | }; 76 | 77 | } // end namespace pcl 78 | 79 | #include "piecewise_linear_function.hpp" 80 | 81 | #endif //#ifndef PCL_PIECEWISE_LINEAR_FUNCTION_H_ 82 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/piecewise_linear_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder(s) nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | */ 35 | 36 | /* \author Bastian Steder */ 37 | 38 | 39 | namespace pcl { 40 | 41 | PiecewiseLinearFunction::PiecewiseLinearFunction(float factor, float offset) : factor_(factor), offset_(offset) 42 | { 43 | } 44 | 45 | inline float PiecewiseLinearFunction::getValue(float point) const 46 | { 47 | float vector_pos = factor_*point + offset_; 48 | float floored_vector_pos = floor(vector_pos); 49 | float interpolation_size = vector_pos-floored_vector_pos; 50 | int data_point_before = (std::max)(0, (std::min)(int(data_points_.size())-2, int(lrint(floored_vector_pos)))); 51 | //cout << "Interpolating between "< 6 | #include 7 | #include 8 | #include "basics/macros.h" 9 | #include "basics/timeutil.h" 10 | 11 | void pngWriteDataHandler(png_structp pngPtr, png_bytep data, png_size_t length) 12 | { 13 | //std::vector& output = *(std::vector*)png_ptr->io_ptr; 14 | std::vector& output = *(std::vector*)png_get_io_ptr(pngPtr); 15 | output.insert(output.end(), data, data+length); 16 | } 17 | void pngFlushHandler(png_structp pngPtr) 18 | { 19 | (void)pngPtr; // No warning for not being used 20 | } 21 | 22 | struct InputData { 23 | const unsigned char* data; 24 | size_t pos; 25 | }; 26 | 27 | void pngReadDataHandler(png_structp pngPtr, png_bytep data, png_size_t length) { 28 | InputData& input = *(InputData*)png_get_io_ptr(pngPtr); 29 | memcpy(data, input.data+input.pos, length); 30 | input.pos += length; 31 | } 32 | 33 | bool Ais3dTools::PngIO::createPng(const unsigned char* imageData, int width, int height, 34 | int bitDepth, int colorType, std::vector& outputData, 35 | const std::vector* keys, const std::vector* values) 36 | { 37 | //AIS3DTOOLS_MEASURE_FUNCTION_TIME; 38 | 39 | png_structp png_ptr = NULL; 40 | png_infop info_ptr = NULL; 41 | png_bytepp row_pointers = NULL; 42 | 43 | png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); 44 | if (png_ptr == NULL) { 45 | return false; 46 | } 47 | 48 | outputData.clear(); 49 | png_set_write_fn(png_ptr, &outputData, pngWriteDataHandler, pngFlushHandler); 50 | 51 | info_ptr = png_create_info_struct(png_ptr); 52 | if (info_ptr == NULL) { 53 | png_destroy_write_struct(&png_ptr, NULL); 54 | return false; 55 | } 56 | 57 | if (setjmp(png_jmpbuf(png_ptr))) { 58 | png_free(png_ptr, row_pointers); 59 | png_destroy_write_struct(&png_ptr, &info_ptr); 60 | return false; 61 | } 62 | 63 | png_set_IHDR(png_ptr, info_ptr, 64 | width, height, 65 | bitDepth, // Bit depth 66 | colorType, // Color encoding 67 | PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT // Default values 68 | ); 69 | 70 | if (keys!=NULL && values!=NULL) { 71 | if (keys->size()!=values->size()) { 72 | std::cerr << __PRETTY_FUNCTION__<<": Error, keys->size()!=values->size()\n"; 73 | } 74 | else if (!keys->empty()) { 75 | std::vector texts; 76 | texts.resize(keys->size()); 77 | for (size_t textIdx=0; textIdx(keys->at(textIdx).c_str()); 80 | texts[textIdx].text = const_cast(values->at(textIdx).c_str()); 81 | texts[textIdx].text_length = values->at(textIdx).size(); 82 | } 83 | png_set_text(png_ptr, info_ptr, &texts[0], keys->size()); 84 | } 85 | } 86 | 87 | 88 | //[> png_get_text also returns the number of text chunks in *num_text <] 89 | //extern PNG_EXPORT(png_uint_32,png_get_text) PNGARG((png_structp png_ptr, 90 | //png_infop info_ptr, png_textp *text_ptr, int *num_text)); 91 | //#endif 92 | 93 | row_pointers = (png_bytepp)png_malloc(png_ptr, height*sizeof(png_bytep)); 94 | size_t rowSize = png_get_rowbytes(png_ptr,info_ptr); 95 | for (int y=0; y& outputData, 110 | const std::vector* keys, const std::vector* values) 111 | { 112 | return createPng(imageData, width, height, 16, PNG_COLOR_TYPE_GRAY, outputData, keys, values); 113 | } 114 | 115 | bool Ais3dTools::PngIO::decompressPngFromDisk(const std::string& filename, std::vector& outputData, 116 | int& width, int& height, int& colorType, int& bitDepth, 117 | std::vector* keys, std::vector* values) 118 | { 119 | std::ifstream pngFile(filename.c_str()); 120 | if (!pngFile) 121 | return false; 122 | 123 | pngFile.seekg(0,std::ios_base::end); 124 | int fileSize = pngFile.tellg(); 125 | pngFile.seekg(0,std::ios_base::beg); 126 | 127 | std::vector data; 128 | data.resize(fileSize); 129 | pngFile.read((char*)&data[0], fileSize); 130 | pngFile.close(); 131 | bool ret = decompressPng(&data[0], outputData, width, height, colorType, bitDepth, keys, values); 132 | //if (keys!=NULL && values!=NULL) { 133 | //if (keys->empty()) 134 | //std::cout << "No text information stored in PNG.\n"; 135 | //for (size_t keyIdx=0; keyIdxsize(); ++keyIdx) 136 | //std::cout << keys->at(keyIdx)<<" = "<at(keyIdx)<<"\n"; 137 | //} 138 | return ret; 139 | } 140 | 141 | bool Ais3dTools::PngIO::decompressPng(const unsigned char* data, std::vector& outputData, 142 | int& width, int& height, int& colorType, int& bitDepth, 143 | std::vector* keys, std::vector* values) 144 | { 145 | //AIS3DTOOLS_MEASURE_FUNCTION_TIME; 146 | 147 | if (keys!=NULL) 148 | keys->clear(); 149 | if (values!=NULL) 150 | values->clear(); 151 | 152 | png_structp png_ptr; 153 | png_infop info_ptr; 154 | //int number_of_passes; 155 | png_bytep * row_pointers; 156 | 157 | //unsigned char header[8]; // 8 is the maximum size that can be checked 158 | 159 | InputData source; 160 | source.data = data; 161 | source.pos = 0; 162 | 163 | if (png_sig_cmp((unsigned char*) source.data, 0, 8)) { 164 | std::cerr << __PRETTY_FUNCTION__<<": Not a valid PNG header.\n"; 165 | return false; 166 | } 167 | 168 | png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL); 169 | if (png_ptr==NULL) { 170 | std::cerr << __PRETTY_FUNCTION__<<": Could not create read struct.\n"; 171 | return false; 172 | } 173 | 174 | info_ptr = png_create_info_struct(png_ptr); 175 | if (info_ptr==NULL) { 176 | std::cerr << __PRETTY_FUNCTION__<<": Could not create info struct.\n"; 177 | png_destroy_read_struct(&png_ptr, &info_ptr, NULL); 178 | return false; 179 | } 180 | 181 | png_set_read_fn(png_ptr, (png_voidp)&source, pngReadDataHandler); 182 | 183 | if (setjmp(png_jmpbuf(png_ptr))) { 184 | std::cerr << __PRETTY_FUNCTION__<<": Something went wrong while reading the PNG information.\n"; 185 | png_destroy_read_struct(&png_ptr, &info_ptr, NULL); 186 | return false; 187 | } 188 | png_read_info(png_ptr, info_ptr); 189 | 190 | width = png_get_image_width(png_ptr, info_ptr); 191 | height = png_get_image_height(png_ptr, info_ptr); 192 | colorType = png_get_color_type(png_ptr, info_ptr); 193 | bitDepth = png_get_bit_depth(png_ptr, info_ptr); 194 | //std::cout << PVARC(width)<push_back(texts[textIdx].key); 202 | values->push_back(texts[textIdx].text); 203 | } 204 | } 205 | 206 | //number_of_passes = png_set_interlace_handling(png_ptr); 207 | png_read_update_info(png_ptr, info_ptr); 208 | 209 | if (setjmp(png_jmpbuf(png_ptr))) { 210 | std::cerr << __PRETTY_FUNCTION__<<": Something went wrong while decompressing the PNG.\n"; 211 | png_destroy_read_struct(&png_ptr, &info_ptr, NULL); 212 | return false; 213 | } 214 | 215 | row_pointers = (png_bytepp)png_malloc(png_ptr, height*sizeof(png_bytep)); 216 | size_t rowSize = png_get_rowbytes(png_ptr,info_ptr); 217 | outputData.resize(height*rowSize); 218 | for (int y=0; y 7 | #include 8 | 9 | namespace Ais3dTools { 10 | 11 | /** 12 | * \brief Capsulates libPNG functionalities 13 | */ 14 | class PngIO 15 | { 16 | public: 17 | //-----PUBLIC STATIC FUNCTIONS----- 18 | //! Create a PNG from the input data - keys and values can be used to write meta data to the PNG 19 | static bool createPng(const unsigned char* imageData, int width, int height, 20 | int bitDepth, int colorType, std::vector& outputData, 21 | const std::vector* keys=NULL, const std::vector* values=NULL); 22 | 23 | //! Create PNG from the 16Bit grayscale input data - useful to compress Openni depth images 24 | static bool create16BitGrayscalePng(const unsigned char* imageData, int width, int height, std::vector& outputData, 25 | const std::vector* keys=NULL, const std::vector* values=NULL); 26 | 27 | //! Decompress a PNG - keys and values can be used to read meta data from the PNG 28 | static bool decompressPng(const unsigned char* data, std::vector& outputData, 29 | int& width, int& height, int& colorType, int& bitDepth, 30 | std::vector* keys=NULL, std::vector* values=NULL); 31 | 32 | //! Same as above, but using a file from the disk 33 | static bool decompressPngFromDisk(const std::string& filename, std::vector& outputData, 34 | int& width, int& height, int& colorType, int& bitDepth, 35 | std::vector* keys=NULL, std::vector* values=NULL); 36 | 37 | protected: 38 | //-----PROTECTED MEMBER VARIABLES----- 39 | //std::vector outputData_; 40 | }; 41 | 42 | } // namespace end 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/runtime_error.cpp: -------------------------------------------------------------------------------- 1 | #include "runtime_error.h" 2 | #include "os_specific.h" 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | 8 | RuntimeError::RuntimeError(const char* fmt, ...) : 9 | std::exception() 10 | { 11 | char* auxPtr = NULL; 12 | va_list arg_list; 13 | va_start(arg_list, fmt); 14 | int b = vasprintf(&auxPtr, fmt, arg_list); 15 | va_end(arg_list); 16 | if (b > 0) 17 | _errorMsg = auxPtr; 18 | else 19 | _errorMsg = ""; 20 | free(auxPtr); 21 | } 22 | 23 | RuntimeError::~RuntimeError() throw() 24 | { 25 | } 26 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/runtime_error.h: -------------------------------------------------------------------------------- 1 | #ifndef RUNTIME_ERROR_H 2 | #define RUNTIME_ERROR_H 3 | 4 | #include 5 | #include 6 | 7 | /** 8 | * \brief a run time error exception 9 | */ 10 | class RuntimeError : public std::exception 11 | { 12 | public: 13 | /** 14 | * constructor which allows to give a error message 15 | * with printf like syntax 16 | */ 17 | explicit RuntimeError(const char* fmt, ...) ; //__attribute__ ((format (printf, 2, 3))) works only with gcc 18 | virtual ~RuntimeError() throw(); 19 | virtual const char* what() const throw() {return _errorMsg.c_str();} 20 | 21 | protected: 22 | std::string _errorMsg; 23 | }; 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/sparsePointCloud.h: -------------------------------------------------------------------------------- 1 | #ifndef AIS3TOOLS_SPARSE_POINT_CLOUD_H 2 | #define AIS3TOOLS_SPARSE_POINT_CLOUD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace Ais3dTools { 8 | 9 | struct SparsePointCloud { 10 | //-----STRUCTS&TYPEDEFS----- 11 | typedef std::set ContainerZ; 12 | typedef std::map ContainerYZ; 13 | typedef std::map ContainerXYZ; 14 | typedef ContainerXYZ::iterator IteratorX; 15 | typedef ContainerYZ::iterator IteratorY; 16 | typedef ContainerZ::iterator IteratorZ; 17 | typedef ContainerXYZ::const_iterator ConstIteratorX; 18 | typedef ContainerYZ::const_iterator ConstIteratorY; 19 | typedef ContainerZ::const_iterator ConstIteratorZ; 20 | 21 | struct const_iterator { 22 | void initialize(const SparsePointCloud* newCloud) { 23 | cloud = newCloud; 24 | bool done = false; 25 | //for (itX=cloud->points.begin(); itX!=cloud.points.end()&&!done; ++itX) 26 | //for (itY=itX->second.begin(); itY!=itX->second.end()&&!done; ++itY) 27 | //for (itX=itX->second.begin(); itX!=itY->second.end()&&!done; ++itZ) 28 | //done = true; 29 | for (itX=cloud->points.begin(); itX!=cloud->points.end()&&!done;) { 30 | for (itY=itX->second.begin(); itY!=itX->second.end()&&!done;) { 31 | itZ = itY->second.begin(); 32 | if (itZ!=itY->second.end()) { 33 | done = true; 34 | break; 35 | } 36 | ++itY; 37 | } 38 | if (!done) 39 | ++itX; 40 | } 41 | //if (!atEnd()) 42 | //std::cout << x()<<";"<second.end()) { 49 | ++itY; 50 | while (itY==itX->second.end()) { 51 | ++itX; 52 | if (atEnd()) 53 | return *this; 54 | itY = itX->second.begin(); 55 | } 56 | itZ = itY->second.begin(); 57 | } 58 | return *this; 59 | } 60 | inline bool atEnd() const { return itX==cloud->points.end(); } 61 | inline float x() const { return float(itX->first)*cloud->cellSize; } 62 | inline float y() const { return float(itY->first)*cloud->cellSize; } 63 | inline float z() const { return float(*itZ)*cloud->cellSize; } 64 | //reference operator*() const; 65 | 66 | const SparsePointCloud* cloud; 67 | ConstIteratorX itX; 68 | ConstIteratorY itY; 69 | ConstIteratorZ itZ; 70 | }; 71 | 72 | //-----CONSTRUCTOR----- 73 | SparsePointCloud(float cellSize=0.01f) : cellSize(cellSize) {} 74 | 75 | //-----MEMBER FUNCTIONS----- 76 | bool add(float x, float y, float z) { 77 | return points[lrintf(x/cellSize)][lrintf(y/cellSize)].insert(lrintf(z/cellSize)).second; 78 | } 79 | void getIndicesFromCoordinates(float x, float y, float z, int& idxX, int& idxY, int& idxZ) const { 80 | idxX=lrintf(x/cellSize), idxY=lrintf(y/cellSize), idxZ=lrintf(z/cellSize); 81 | } 82 | void getCoordinatesFromIndices(int idxX, int idxY, int idxZ, float& x, float& y, float& z) const { 83 | x = float(idxX)*cellSize, y = float(idxY)*cellSize, z = float(idxZ)*cellSize; 84 | } 85 | bool checkExistence(float x, float y, float z) const { 86 | ConstIteratorX itX = points.find(lrintf(x/cellSize)); 87 | if (itX==points.end()) return false; 88 | ConstIteratorY itY = itX->second.find(lrintf(y/cellSize)); 89 | if (itY==itX->second.end()) return false; 90 | ConstIteratorZ itZ = itY->second.find(lrintf(z/cellSize)); 91 | if (itZ==itY->second.end()) return false; 92 | return true; 93 | } 94 | void clear() { points.clear(); } 95 | bool empty() const { return points.empty(); } 96 | const_iterator begin() const { const_iterator it; it.initialize(this); return it; } 97 | 98 | //-----MEMBER VARIABLES----- 99 | float cellSize; 100 | ContainerXYZ points; 101 | }; 102 | 103 | template 104 | struct SparsePointCloudT { 105 | //-----STRUCTS&TYPEDEFS----- 106 | typedef PointType_ PointType; 107 | typedef std::map ContainerZ; 108 | typedef std::map ContainerYZ; 109 | typedef std::map ContainerXYZ; 110 | typedef typename ContainerXYZ::iterator IteratorX; 111 | typedef typename ContainerYZ::iterator IteratorY; 112 | typedef typename ContainerZ::iterator IteratorZ; 113 | typedef typename ContainerXYZ::const_iterator ConstIteratorX; 114 | typedef typename ContainerYZ::const_iterator ConstIteratorY; 115 | typedef typename ContainerZ::const_iterator ConstIteratorZ; 116 | 117 | //-----CONSTRUCTOR----- 118 | SparsePointCloudT(float cellSize=0.01f) : cellSize(cellSize) {} 119 | 120 | //-----MEMBER FUNCTIONS----- 121 | void add(float x, float y, float z, const PointType_& point) { 122 | points[lrintf(x/cellSize)][lrintf(y/cellSize)][lrintf(z/cellSize)] = point; 123 | } 124 | void getIndicesFromCoordinates(float x, float y, float z, int& idxX, int& idxY, int& idxZ) const { 125 | idxX=lrintf(x/cellSize), idxY=lrintf(y/cellSize), idxZ=lrintf(z/cellSize); 126 | } 127 | void getCoordinatesFromIndices(int idxX, int idxY, int idxZ, float& x, float& y, float& z) const { 128 | x = float(idxX)*cellSize, y = float(idxY)*cellSize, z = float(idxZ)*cellSize; 129 | } 130 | void clear() { points.clear(); } 131 | bool empty() const { return points.empty(); } 132 | 133 | 134 | 135 | //-----MEMBER VARIABLES----- 136 | float cellSize; 137 | ContainerXYZ points; 138 | }; 139 | 140 | } // Namespace end 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/string_tools.cpp: -------------------------------------------------------------------------------- 1 | #include "string_tools.h" 2 | #include "os_specific.h" 3 | #include "macros.h" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #ifdef UNIX 15 | #include 16 | #endif 17 | 18 | using namespace std; 19 | 20 | std::string trim(const std::string& s) 21 | { 22 | if(s.length() == 0) 23 | return s; 24 | string::size_type b = s.find_first_not_of(" \t\n"); 25 | string::size_type e = s.find_last_not_of(" \t\n"); 26 | if(b == string::npos) 27 | return ""; 28 | return std::string(s, b, e - b + 1); 29 | } 30 | 31 | std::string trimLeft(const std::string& s) 32 | { 33 | if(s.length() == 0) 34 | return s; 35 | string::size_type b = s.find_first_not_of(" \t\n"); 36 | string::size_type e = s.length() - 1; 37 | if(b == string::npos) 38 | return ""; 39 | return std::string(s, b, e - b + 1); 40 | } 41 | 42 | std::string trimRight(const std::string& s) 43 | { 44 | if(s.length() == 0) 45 | return s; 46 | string::size_type b = 0; 47 | string::size_type e = s.find_last_not_of(" \t\n"); 48 | if(b == string::npos) 49 | return ""; 50 | return std::string(s, b, e - b + 1); 51 | } 52 | 53 | std::string strToLower(const std::string& s) 54 | { 55 | string ret; 56 | std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::tolower); 57 | return ret; 58 | } 59 | 60 | std::string strToUpper(const std::string& s) 61 | { 62 | string ret; 63 | std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::toupper); 64 | return ret; 65 | } 66 | 67 | std::string formatString(const char* fmt, ...) 68 | { 69 | char* auxPtr = NULL; 70 | va_list arg_list; 71 | va_start(arg_list, fmt); 72 | int numChar = vasprintf(&auxPtr, fmt, arg_list); 73 | va_end(arg_list); 74 | string retString; 75 | if (numChar != -1) 76 | retString = auxPtr; 77 | else 78 | throw RuntimeError("Error while allocating memory"); 79 | free(auxPtr); 80 | return retString; 81 | } 82 | 83 | int strPrintf(std::string& str, const char* fmt, ...) 84 | { 85 | char* auxPtr = NULL; 86 | va_list arg_list; 87 | va_start(arg_list, fmt); 88 | int numChars = vasprintf(&auxPtr, fmt, arg_list); 89 | va_end(arg_list); 90 | str = auxPtr; 91 | free(auxPtr); 92 | return numChars; 93 | } 94 | 95 | std::string strExpandFilename(const std::string& filename) 96 | { 97 | 98 | #ifdef UNIX 99 | string result = filename; 100 | wordexp_t p; 101 | 102 | wordexp(filename.c_str(), &p, 0); 103 | if(p.we_wordc > 0) { 104 | result = p.we_wordv[0]; 105 | } 106 | wordfree(&p); 107 | return result; 108 | #endif 109 | 110 | #ifdef WINDOWS 111 | std::cerr << "WARNING: " << __PRETTY_FUNCTION__ << " not implemented" << std::endl; 112 | return std::string(); 113 | #endif 114 | 115 | } 116 | 117 | std::vector strSplit(const std::string& str, const std::string& delimiters) 118 | { 119 | std::vector tokens; 120 | string::size_type lastPos = 0; 121 | string::size_type pos = 0; 122 | 123 | do { 124 | pos = str.find_first_of(delimiters, lastPos); 125 | tokens.push_back(str.substr(lastPos, pos - lastPos)); 126 | lastPos = pos + 1; 127 | } while (string::npos != pos); 128 | 129 | return tokens; 130 | } 131 | 132 | bool strStartsWith(const std::string& s, const std::string& start) 133 | { 134 | if (s.size() < start.size()) 135 | return false; 136 | return equal(start.begin(), start.end(), s.begin()); 137 | } 138 | 139 | bool strEndsWith(const std::string& s, const std::string& end) 140 | { 141 | if (s.size() < end.size()) 142 | return false; 143 | return equal(end.rbegin(), end.rend(), s.rbegin()); 144 | } 145 | 146 | int readLine(std::istream& is, std::stringstream& currentLine) 147 | { 148 | if (is.eof()) 149 | return -1; 150 | currentLine.str(""); 151 | currentLine.clear(); 152 | is.get(*currentLine.rdbuf()); 153 | if (is.fail()) // fail is set on empty lines 154 | is.clear(); 155 | FSKIP_LINE(is); // read \n not read by get() 156 | return currentLine.str().size(); 157 | } 158 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/string_tools.h: -------------------------------------------------------------------------------- 1 | #ifndef AIS3DTOOLS_STRING_TOOLS_H 2 | #define AIS3DTOOLS_STRING_TOOLS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "runtime_error.h" 9 | 10 | 11 | namespace Ais3dTools { 12 | 13 | /** @addtogroup utils **/ 14 | // @{ 15 | 16 | /** \file stringTools.h 17 | * \brief utility functions for handling strings 18 | */ 19 | 20 | /** 21 | * remove whitespaces from the start/end of a string 22 | */ 23 | std::string trim(const std::string& s); 24 | 25 | /** 26 | * remove whitespaces from the left side of the string 27 | */ 28 | std::string trimLeft(const std::string& s); 29 | 30 | /** 31 | * remove whitespaced from the right side of the string 32 | */ 33 | std::string trimRight(const std::string& s); 34 | 35 | /** 36 | * convert the string to lower case 37 | */ 38 | std::string strToLower(const std::string& s); 39 | 40 | /** 41 | * convert a string to upper case 42 | */ 43 | std::string strToUpper(const std::string& s); 44 | 45 | /** 46 | * read integer values (seperated by spaces) from a string and store 47 | * them in the given OutputIterator. 48 | */ 49 | template 50 | OutputIterator readInts(const char* str, OutputIterator out) 51 | { 52 | char* cl = (char*)str; 53 | char* cle = cl; 54 | while (1) { 55 | int id = strtol(cl, &cle, 10); 56 | if (cl == cle) 57 | break; 58 | *out++ = id; 59 | cl = cle; 60 | } 61 | return out; 62 | } 63 | 64 | /** 65 | * read float values (seperated by spaces) from a string and store 66 | * them in the given OutputIterator. 67 | */ 68 | template 69 | OutputIterator readFloats(const char* str, OutputIterator out) 70 | { 71 | char* cl = (char*)str; 72 | char* cle = cl; 73 | while (1) { 74 | double val = strtod(cl, &cle); 75 | if (cl == cle) 76 | break; 77 | *out++ = val; 78 | cl = cle; 79 | } 80 | return out; 81 | } 82 | 83 | /** 84 | * format a string and return a std::string. 85 | * Format is just like printf, see man 3 printf 86 | */ 87 | std::string formatString(const char* fmt, ...); // __attribute__ ((format (printf, 1, 2))); 88 | 89 | /** 90 | * replacement function for sprintf which fills a std::string instead of a char* 91 | */ 92 | int strPrintf(std::string& str, const char* fmt, ...);// __attribute__ ((format (printf, 2, 3))); 93 | 94 | /** 95 | * convert a string into an other type. 96 | */ 97 | template 98 | void convertString(const std::string& s, T& x, bool failIfLeftoverChars = true) 99 | { 100 | std::istringstream i(s); 101 | char c; 102 | if (!(i >> x) || (failIfLeftoverChars && i.get(c))) 103 | throw RuntimeError("%s: Unable to convert %s", __PRETTY_FUNCTION__, s.c_str()); 104 | } 105 | 106 | /** 107 | * convert a string into an other type. 108 | * Return the converted value. Throw error if parsing is wrong. 109 | */ 110 | template 111 | T stringToType(const std::string& s, bool failIfLeftoverChars = true) 112 | { 113 | T x; 114 | convertString(s, x, failIfLeftoverChars); 115 | return x; 116 | } 117 | 118 | /** 119 | * return true, if str starts with substr 120 | */ 121 | bool strStartsWith(const std::string & str, const std::string& substr); 122 | 123 | /** 124 | * return true, if str ends with substr 125 | */ 126 | bool strEndsWith(const std::string & str, const std::string& substr); 127 | 128 | /** 129 | * expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other will get expanded. 130 | * Also command substitution, e.g. `pwd` will give the current directory. 131 | */ 132 | std::string strExpandFilename(const std::string& filename); 133 | 134 | /** 135 | * split a string into token based on the characters given in delim 136 | */ 137 | std::vector strSplit(const std::string& s, const std::string& delim); 138 | 139 | /** 140 | * read a line from is into currentLine. 141 | * @return the number of characters read into currentLine (excluding newline), -1 on eof() 142 | */ 143 | int readLine(std::istream& is, std::stringstream& currentLine); 144 | 145 | // @} 146 | 147 | } //end namespace 148 | #endif 149 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/timeutil.cpp: -------------------------------------------------------------------------------- 1 | #include "timeutil.h" 2 | #include 3 | 4 | #ifdef UNIX 5 | #include 6 | #endif 7 | 8 | namespace Ais3dTools{ 9 | 10 | #ifdef _WINDOWS 11 | #include 12 | #include 13 | 14 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 15 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 16 | #else 17 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 18 | #endif 19 | 20 | struct timezone 21 | { 22 | int tz_minuteswest; /* minutes W of Greenwich */ 23 | int tz_dsttime; /* type of dst correction */ 24 | }; 25 | 26 | int gettimeofday(struct timeval *tv, struct timezone *tz) 27 | { 28 | // Define a structure to receive the current Windows filetime 29 | FILETIME ft; 30 | 31 | // Initialize the present time to 0 and the timezone to UTC 32 | unsigned __int64 tmpres = 0; 33 | static int tzflag = 0; 34 | 35 | if (NULL != tv) 36 | { 37 | GetSystemTimeAsFileTime(&ft); 38 | 39 | // The GetSystemTimeAsFileTime returns the number of 100 nanosecond 40 | // intervals since Jan 1, 1601 in a structure. Copy the high bits to 41 | // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. 42 | tmpres |= ft.dwHighDateTime; 43 | tmpres <<= 32; 44 | tmpres |= ft.dwLowDateTime; 45 | 46 | // Convert to microseconds by dividing by 10 47 | tmpres /= 10; 48 | 49 | // The Unix epoch starts on Jan 1 1970. Need to subtract the difference 50 | // in seconds from Jan 1 1601. 51 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 52 | 53 | // Finally change microseconds to seconds and place in the seconds value. 54 | // The modulus picks up the microseconds. 55 | tv->tv_sec = (long)(tmpres / 1000000UL); 56 | tv->tv_usec = (long)(tmpres % 1000000UL); 57 | } 58 | 59 | if (NULL != tz) { 60 | if (!tzflag) { 61 | _tzset(); 62 | tzflag++; 63 | } 64 | 65 | long sec; 66 | int hours; 67 | _get_timezone(&sec); 68 | _get_daylight(&hours); 69 | 70 | // Adjust for the timezone west of Greenwich 71 | tz->tz_minuteswest = sec / 60; 72 | tz->tz_dsttime = hours; 73 | } 74 | 75 | return 0; 76 | } 77 | #endif 78 | 79 | std::string getTimeAsString(time_t time) 80 | { 81 | std::string dateStr = ctime(&time); 82 | if (dateStr.size() == 0) 83 | return ""; 84 | // remove trailing newline 85 | dateStr.erase(dateStr.size()-1, 1); 86 | return dateStr; 87 | } 88 | 89 | std::string getCurrentTimeAsString() 90 | { 91 | return getTimeAsString(time(NULL)); 92 | } 93 | 94 | ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} 95 | 96 | ScopeTime::~ScopeTime() { 97 | std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; 98 | } 99 | 100 | double get_monotonic_time() 101 | { 102 | #if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK)) 103 | struct timespec ts; 104 | clock_gettime(CLOCK_MONOTONIC, &ts); 105 | return ts.tv_sec + ts.tv_nsec*1e-9; 106 | #else 107 | return get_time(); 108 | #endif 109 | } 110 | 111 | }//end namespace -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/timeutil.h: -------------------------------------------------------------------------------- 1 | #ifndef AIS3DTOOLS_TIMEUTIL_H 2 | #define AIS3DTOOLS_TIMEUTIL_H 3 | 4 | #include 5 | 6 | #ifdef _WINDOWS 7 | #include 8 | #include 9 | #include "ais3DTools_stuff_api.h" 10 | #else 11 | #include 12 | #endif 13 | 14 | /** @addtogroup utils **/ 15 | // @{ 16 | 17 | /** \file timeutil.h 18 | * \brief utility functions for handling time related stuff 19 | */ 20 | 21 | /// Executes code, only if secs are gone since last exec. 22 | /// extended version, in which the current time is given, e.g., timestamp of IPC message 23 | #ifndef DO_EVERY_TS 24 | #define DO_EVERY_TS(secs, currentTime, code) \ 25 | if (1) {\ 26 | static double s_lastDone_ = (currentTime); \ 27 | double s_now_ = (currentTime); \ 28 | if (s_lastDone_ > s_now_) \ 29 | s_lastDone_ = s_now_; \ 30 | if (s_now_ - s_lastDone_ > (secs)) { \ 31 | code; \ 32 | s_lastDone_ = s_now_; \ 33 | }\ 34 | } else \ 35 | (void)0 36 | #endif 37 | 38 | /// Executes code, only if secs are gone since last exec. 39 | #ifndef DO_EVERY 40 | #define DO_EVERY(secs, code) DO_EVERY_TS(secs, Ais3dTools::get_time(), code) 41 | #endif 42 | 43 | #ifndef MEASURE_TIME 44 | #define MEASURE_TIME(text, code) \ 45 | if(1) { \ 46 | double _start_time_ = get_time(); \ 47 | code; \ 48 | fprintf(stderr, "%s took %f sec\n", text, get_time() - _start_time_); \ 49 | } else \ 50 | (void) 0 51 | #endif 52 | 53 | 54 | namespace Ais3dTools{ 55 | #ifdef _WINDOWS 56 | AIS3DTOOLS_STUFF_API int gettimeofday(struct timeval *tv, struct timezone *tz); 57 | #endif 58 | 59 | 60 | /** 61 | * return the current time in seconds since 1. Jan 1970 62 | */ 63 | inline double get_time() { 64 | struct timeval ts; 65 | gettimeofday(&ts,0); 66 | 67 | return ts.tv_sec + ts.tv_usec*1e-6; 68 | } 69 | 70 | /** 71 | * return the current time in seconds since 1. Jan 1970 72 | */ 73 | inline double getTime() { 74 | return get_time(); 75 | } 76 | 77 | /** 78 | * monotonic time since some point in time, get_time() above returns 79 | * the wall time which can go backwards for small intervals between 80 | * the calls. 81 | */ 82 | double get_monotonic_time(); 83 | 84 | /** 85 | * return the string of a given time 86 | */ 87 | std::string getTimeAsString(time_t time); 88 | 89 | /** 90 | * return the default string of the current time 91 | */ 92 | std::string getCurrentTimeAsString(); 93 | 94 | /** 95 | * \brief Class to measure the time spent in a scope 96 | * 97 | * To use this class, e.g. to measure the time spent in a function, 98 | * just create and instance at the beginning of the function. 99 | */ 100 | class ScopeTime { 101 | public: 102 | ScopeTime(const char* title); 103 | ~ScopeTime(); 104 | private: 105 | std::string _title; 106 | double _startTime; 107 | }; 108 | 109 | } //end namespace 110 | 111 | #define AIS3DTOOLS_MEASURE_FUNCTION_TIME \ 112 | Ais3dTools::ScopeTime scopeTime(__PRETTY_FUNCTION__) 113 | 114 | 115 | #ifndef AIS3DTOOLS_MEASURE_TIME 116 | #define AIS3DTOOLS_MEASURE_TIME(text, code) \ 117 | { \ 118 | double _start_time_ = Ais3dTools::get_time(); \ 119 | code; \ 120 | std::cout << text<<" took "< 43 | #include 44 | 45 | namespace Ais3dTools { 46 | 47 | /** 48 | * \brief Calculates the weighted average and the covariance matrix 49 | * 50 | * A class to calculate the weighted average and the covariance matrix of a set of vectors with given weights. 51 | * The original data is not saved. Mean and covariance are calculated iteratively. 52 | */ 53 | template 54 | class VectorAverage 55 | { 56 | public: 57 | //-----CONSTRUCTOR&DESTRUCTOR----- 58 | /** Constructor - dimension gives the size of the vectors to work with. */ 59 | VectorAverage(); 60 | /** Destructor */ 61 | ~VectorAverage() {} 62 | 63 | //-----METHODS----- 64 | /** Reset the object to work with a new data set */ 65 | inline void reset(); 66 | 67 | /** Get the mean of the added vectors */ 68 | inline const Eigen::Matrix& getMean() const { return mean_;} 69 | 70 | /** Get the covariance matrix of the added vectors */ 71 | inline const Eigen::Matrix& getCovariance() const { return covariance_;} 72 | 73 | /** Get the summed up weight of all added vectors */ 74 | inline real getAccumulatedWeight() const { return accumulatedWeight_;} 75 | 76 | /** Get the number of added vectors */ 77 | inline unsigned int getNoOfSamples() { return noOfSamples_;} 78 | 79 | /** Add a new sample */ 80 | inline void add(const Eigen::Matrix& sample, real weight=1.0); 81 | 82 | /** Do Principal component analysis for dimension=2 */ 83 | inline void doPCA(Eigen::Matrix& eigen_values, 84 | Eigen::Matrix& eigen_vector1, Eigen::Matrix& eigen_vector2) const; 85 | 86 | /** Do Principal component analysis for dimension=3 */ 87 | inline void doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, 88 | Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const; 89 | 90 | /** Do Principal component analysis */ 91 | inline void doPCA(Eigen::Matrix& eigen_values) const; 92 | 93 | /** Get the eigenvector corresponding to the smallest eigenvalue */ 94 | inline void getEigenVector1(Eigen::Matrix& eigen_vector1) const; 95 | 96 | //-----VARIABLES----- 97 | 98 | protected: 99 | //-----METHODS----- 100 | //-----VARIABLES----- 101 | unsigned int noOfSamples_; 102 | real accumulatedWeight_; 103 | Eigen::Matrix mean_; 104 | Eigen::Matrix covariance_; 105 | }; 106 | 107 | typedef VectorAverage VectorAverage2f; 108 | typedef VectorAverage VectorAverage3f; 109 | typedef VectorAverage VectorAverage4f; 110 | 111 | #include "vector_average.hpp" 112 | 113 | } // namespace end 114 | 115 | #endif // #ifndef PCL_VECTOR_AVERAGE_H 116 | 117 | -------------------------------------------------------------------------------- /EXTERNALS/ais3dTools/basics/vector_average.hpp: -------------------------------------------------------------------------------- 1 | template 2 | VectorAverage::VectorAverage() 3 | { 4 | reset(); 5 | } 6 | 7 | template 8 | inline void VectorAverage::reset() 9 | { 10 | noOfSamples_ = 0; 11 | accumulatedWeight_ = 0.0; 12 | mean_.fill(0); 13 | covariance_.fill(0); 14 | } 15 | 16 | template 17 | inline void VectorAverage::add(const Eigen::Matrix& sample, real weight) { 18 | if (weight == 0.0f) 19 | return; 20 | 21 | ++noOfSamples_; 22 | accumulatedWeight_ += weight; 23 | real alpha = weight/accumulatedWeight_; 24 | 25 | Eigen::Matrix diff = sample - mean_; 26 | covariance_ = (1.0-alpha)*(covariance_ + alpha * (diff * diff.transpose())); 27 | 28 | mean_ += alpha*(diff); 29 | 30 | //if (pcl_isnan(covariance_(0,0))) 31 | //{ 32 | //cout << PVARN(weight); 33 | //exit(0); 34 | //} 35 | } 36 | 37 | template 38 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, 39 | Eigen::Matrix& eigen_vector1, Eigen::Matrix& eigen_vector2) const 40 | { 41 | Eigen::SelfAdjointEigenSolver > ei_symm(covariance_); 42 | eigen_values = ei_symm.eigenvalues(); 43 | Eigen::Matrix eigen_vectors = ei_symm.eigenvectors(); 44 | 45 | eigen_vector1 = eigen_vectors.col(0); 46 | eigen_vector2 = eigen_vectors.col(1); 47 | } 48 | 49 | template 50 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, 51 | Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const 52 | { 53 | Eigen::SelfAdjointEigenSolver > ei_symm(covariance_); 54 | eigen_values = ei_symm.eigenvalues(); 55 | Eigen::Matrix eigen_vectors = ei_symm.eigenvectors(); 56 | 57 | eigen_vector1 = eigen_vectors.col(0); 58 | eigen_vector2 = eigen_vectors.col(1); 59 | eigen_vector3 = eigen_vectors.col(2); 60 | } 61 | 62 | template 63 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values) const 64 | { 65 | Eigen::SelfAdjointEigenSolver > ei_symm(covariance_, false); 66 | //Eigen::SelfAdjointEigenSolver > ei_symm(covariance_); 67 | eigen_values = ei_symm.eigenvalues(); 68 | } 69 | 70 | template 71 | inline void VectorAverage::getEigenVector1(Eigen::Matrix& eigen_vector1) const 72 | { 73 | Eigen::SelfAdjointEigenSolver > ei_symm(covariance_); 74 | Eigen::Matrix eigen_vectors = ei_symm.eigenvectors(); 75 | eigen_vector1 = eigen_vectors.col(0); 76 | } 77 | 78 | #if 0 // Fuck... Does not work always... 79 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 80 | // Special cases for real=float & dimension=3 -> Partial specialization does not work with class templates. :( // 81 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 82 | /////////// 83 | // float // 84 | /////////// 85 | template <> 86 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, 87 | Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const 88 | { 89 | //cout << "Using specialized 3x3 version of doPCA!\n"; 90 | Eigen::Matrix eigen_vectors; 91 | eigen33(covariance_, eigen_vectors, eigen_values); 92 | eigen_vector1 = eigen_vectors.col(0); 93 | eigen_vector2 = eigen_vectors.col(1); 94 | eigen_vector3 = eigen_vectors.col(2); 95 | } 96 | template <> 97 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values) const 98 | { 99 | //cout << "Using specialized 3x3 version of doPCA!\n"; 100 | computeRoots (covariance_, eigen_values); 101 | } 102 | template <> 103 | inline void VectorAverage::getEigenVector1(Eigen::Matrix& eigen_vector1) const 104 | { 105 | //cout << "Using specialized 3x3 version of doPCA!\n"; 106 | Eigen::Matrix eigen_values; 107 | Eigen::Matrix eigen_vectors; 108 | eigen33(covariance_, eigen_vectors, eigen_values); 109 | eigen_vector1 = eigen_vectors.col(0); 110 | } 111 | 112 | //////////// 113 | // double // 114 | //////////// 115 | template <> 116 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, 117 | Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const 118 | { 119 | //cout << "Using specialized 3x3 version of doPCA!\n"; 120 | Eigen::Matrix eigen_vectors; 121 | eigen33(covariance_, eigen_vectors, eigen_values); 122 | eigen_vector1 = eigen_vectors.col(0); 123 | eigen_vector2 = eigen_vectors.col(1); 124 | eigen_vector3 = eigen_vectors.col(2); 125 | } 126 | template <> 127 | inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values) const 128 | { 129 | //cout << "Using specialized 3x3 version of doPCA!\n"; 130 | computeRoots (covariance_, eigen_values); 131 | } 132 | template <> 133 | inline void VectorAverage::getEigenVector1(Eigen::Matrix& eigen_vector1) const 134 | { 135 | //cout << "Using specialized 3x3 version of doPCA!\n"; 136 | Eigen::Matrix eigen_values; 137 | Eigen::Matrix eigen_vectors; 138 | eigen33(covariance_, eigen_vectors, eigen_values); 139 | eigen_vector1 = eigen_vectors.col(0); 140 | } 141 | #endif 142 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | This code corresponds to the paper 'Maximum Likelihood Remission Calibration for Groups of Heterogeneous Laser Scanners' (IROS 2015). If you use this software for scientific publication please cite the paper: 2 | 3 | @inproceedings{steder15icra, 4 | author = {Steder, Bastian and Ruhnke, Michael and K{\"u}mmerle, Rainer and Burgard, 5 | Wolfram}, 6 | booktitle = {Proc.~of the IEEE Int.~Conf.~on Robotics \& Automation (ICRA)}, 7 | year = {2015}, 8 | title = {Maximum Likelihood Remission Calibration for Groups of Heterogeneous Laser 9 | Scanners} 10 | } 11 | 12 | ----- 13 | 14 | How to compile (tested under Ubuntu 14.04, 64Bit): 15 | You will need to have the Point Cloud Library (PCL) installed (http://pointclouds.org/) 16 | - Unpack the code and go into the folder aisRemissionCalibration 17 | - mkdir build 18 | - cd build 19 | - cmake .. 20 | - make 21 | 22 | Prepare a dataset: 23 | An example dataset can be downloaded here: 24 | http://www.informatik.uni-freiburg.de/~steder/datasets.html 25 | The format of the point clouds is *.pcd, which is the format of the Point Cloud Library (PCL). 26 | Necessary fields in the point cloud are x, y, z, intensity, laser, which ist the 3D position for each point, 27 | the measured remission value and the laser id. In addition, vp_x, vp_y, vp_z can be used to provide the sensor 28 | position from which the point was observed. If this is not provided, the system uses the VIEWPOINT of the 29 | point cloud as the sensor position for all included points. 30 | The example dataset also includes the field dynProb. This is the probability that a point belongs to a dynamic 31 | object and the system will ignore every point with a value above 0.3. 32 | The only entry necessary in the *_info.dat files is 'SLAM:', which gives the SLAM solution for the dataset 33 | as x, y, z, roll, pitch, yaw (in meters and rad). 34 | 35 | Run the code: 36 | - bin/mapCellsExtractor -d 37 | This call extracts datapoints for the optimization from the dataset 38 | - bin/mapCellsExtractor -d 39 | - bin/remissionCalibration -d -m 4 40 | This call performs the actual optimization procedure and might take a few minutes. 41 | The -m parameter defines the mode of the optimization as described in the paper. 42 | We recommend mode 2. bin/remissionCalibration -h shows a help with more options. 43 | By default the calibration uses only one cell for the incidence angle, effectively ignoring it. 44 | This is useful if you only want to calibrate regarding range and laser id, without the need to 45 | extract normals. Alternatively you can use 46 | - bin/remissionCalibration -d -m 4 -y 10 47 | This performs the optimization including incidence angles. 48 | 49 | The result is stored in calibrationValues_mode2.txt. 50 | You can use the following command to write out corrected point clouds: 51 | 52 | - bin/remissionCalibration -d -n -c calibrationValues_mode2.txt 53 | 54 | To visualize a *.pcd you can use 'pcl_viewer', which comes with PCL. 55 | 56 | To use calibrated remissions in your code, you have to use the class RemissionCalibrationResult 57 | from src/remissionCalibrationResult.h. 58 | 59 | bool readCalibrationValuesFromFile(const std::string& fileName); 60 | reads a calibration file and 61 | float getCalibratedIntensity(float range, float incidenceAngle, float intensity, int laser) const; 62 | calculates the corrected remission for a point. 63 | 64 | This software is licenced under the Creative Commons (Attribution-NonCommercial-ShareAlike): 65 | http://creativecommons.org/licenses/by-nc-sa/4.0 66 | 67 | Author: Bastian Steder 68 | -------------------------------------------------------------------------------- /remissionCalibrationMapCells.dat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/soytony/IntensityCalibration/0e027ef1d25ec0d385e25483a219480e6d45740a/remissionCalibrationMapCells.dat -------------------------------------------------------------------------------- /src/BackwardCpp/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: LLVM 3 | -------------------------------------------------------------------------------- /src/BackwardCpp/.gitignore: -------------------------------------------------------------------------------- 1 | build*/ 2 | *.pyc 3 | -------------------------------------------------------------------------------- /src/BackwardCpp/.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | compiler: 3 | - gcc 4 | - clang 5 | 6 | addons: 7 | apt: 8 | packages: 9 | - valgrind 10 | 11 | install: 12 | - DEPS_DIR="${TRAVIS_BUILD_DIR}/deps" 13 | - mkdir ${DEPS_DIR} && cd ${DEPS_DIR} 14 | - CMAKE_URL="http://www.cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.tar.gz" 15 | - mkdir cmake && travis_retry wget --no-check-certificate --quiet -O - ${CMAKE_URL} | tar --strip-components=1 -xz -C cmake 16 | - export PATH=${DEPS_DIR}/cmake/bin:${PATH} 17 | - pip install --user conan && export PATH=$PATH:$HOME/.local/bin 18 | - cd ${TRAVIS_BUILD_DIR} 19 | - mkdir build && cd build 20 | - cmake .. -DBACKWARD_TESTS=ON 21 | - cmake --build . 22 | 23 | script: 24 | - valgrind ctest .. --verbose 25 | - cd ${TRAVIS_BUILD_DIR} && conan create . Manu343726/testing --build=outdated 26 | -------------------------------------------------------------------------------- /src/BackwardCpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # CMakeLists.txt 3 | # Copyright 2013 Google Inc. All Rights Reserved. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | cmake_minimum_required(VERSION 3.0) 24 | project(backward CXX) 25 | 26 | # Introduce variables: 27 | # * CMAKE_INSTALL_LIBDIR 28 | # * CMAKE_INSTALL_BINDIR 29 | # * CMAKE_INSTALL_INCLUDEDIR 30 | include(GNUInstallDirs) 31 | 32 | include(BackwardConfig.cmake) 33 | 34 | # check if compiler is nvcc or nvcc_wrapper 35 | set(COMPILER_IS_NVCC false) 36 | get_filename_component(COMPILER_NAME ${CMAKE_CXX_COMPILER} NAME) 37 | if (COMPILER_NAME MATCHES "^nvcc") 38 | set(COMPILER_IS_NVCC true) 39 | endif() 40 | 41 | if (DEFINED ENV{OMPI_CXX} OR DEFINED ENV{MPICH_CXX}) 42 | if ( ($ENV{OMPI_CXX} MATCHES "nvcc") OR ($ENV{MPICH_CXX} MATCHES "nvcc") ) 43 | set(COMPILER_IS_NVCC true) 44 | endif() 45 | endif() 46 | 47 | # set CXX standard 48 | set(CMAKE_CXX_STANDARD_REQUIRED True) 49 | set(CMAKE_CXX_STANDARD 11) 50 | if (${COMPILER_IS_NVCC}) 51 | # GNU CXX extensions are not supported by nvcc 52 | set(CMAKE_CXX_EXTENSIONS OFF) 53 | endif() 54 | 55 | ############################################################################### 56 | # COMPILER FLAGS 57 | ############################################################################### 58 | 59 | if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_COMPILER_IS_GNUCXX) 60 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 61 | if (NOT ${COMPILER_IS_NVCC}) 62 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pedantic-errors") 63 | endif() 64 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 65 | endif() 66 | 67 | ############################################################################### 68 | # BACKWARD OBJECT 69 | ############################################################################### 70 | 71 | add_library(backward_object OBJECT backward.cpp) 72 | target_compile_definitions(backward_object PRIVATE ${BACKWARD_DEFINITIONS}) 73 | target_include_directories(backward_object PRIVATE ${BACKWARD_INCLUDE_DIRS}) 74 | set(BACKWARD_ENABLE $ CACHE STRING 75 | "Link with this object to setup backward automatically") 76 | 77 | 78 | ############################################################################### 79 | # BACKWARD LIBRARY (Includes backward.cpp) 80 | ############################################################################### 81 | option(BACKWARD_SHARED "Build dynamic backward-cpp shared lib" OFF) 82 | 83 | if(BACKWARD_SHARED) 84 | set(libtype SHARED) 85 | endif() 86 | add_library(backward ${libtype} backward.cpp) 87 | target_compile_definitions(backward PUBLIC ${BACKWARD_DEFINITIONS}) 88 | target_include_directories(backward PUBLIC ${BACKWARD_INCLUDE_DIRS}) 89 | 90 | ############################################################################### 91 | # TESTS 92 | ############################################################################### 93 | 94 | if(BACKWARD_TESTS) 95 | enable_testing() 96 | 97 | add_library(test_main OBJECT test/_test_main.cpp) 98 | 99 | macro(backward_add_test src) 100 | get_filename_component(name ${src} NAME_WE) 101 | set(test_name "test_${name}") 102 | 103 | add_executable(${test_name} ${src} ${ARGN} $) 104 | 105 | target_link_libraries(${test_name} PRIVATE Backward::Backward) 106 | 107 | add_test(NAME ${name} COMMAND ${test_name}) 108 | endmacro() 109 | 110 | # Tests without backward.cpp 111 | set(TESTS 112 | test 113 | stacktrace 114 | rectrace 115 | select_signals 116 | ) 117 | 118 | foreach(test ${TESTS}) 119 | backward_add_test(test/${test}.cpp) 120 | endforeach() 121 | 122 | # Tests with backward.cpp 123 | set(TESTS 124 | suicide 125 | ) 126 | 127 | foreach(test ${TESTS}) 128 | backward_add_test(test/${test}.cpp ${BACKWARD_ENABLE}) 129 | endforeach() 130 | endif() 131 | 132 | install( 133 | FILES "backward.hpp" 134 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 135 | ) 136 | install( 137 | FILES "BackwardConfig.cmake" 138 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/backward 139 | ) 140 | -------------------------------------------------------------------------------- /src/BackwardCpp/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2013 Google Inc. All Rights Reserved. 2 | 3 | The MIT License (MIT) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is furnished to do 10 | so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/BackwardCpp/backward.cpp: -------------------------------------------------------------------------------- 1 | // Pick your poison. 2 | // 3 | // On GNU/Linux, you have few choices to get the most out of your stack trace. 4 | // 5 | // By default you get: 6 | // - object filename 7 | // - function name 8 | // 9 | // In order to add: 10 | // - source filename 11 | // - line and column numbers 12 | // - source code snippet (assuming the file is accessible) 13 | 14 | // Install one of the following libraries then uncomment one of the macro (or 15 | // better, add the detection of the lib and the macro definition in your build 16 | // system) 17 | 18 | // - apt-get install libdw-dev ... 19 | // - g++/clang++ -ldw ... 20 | // #define BACKWARD_HAS_DW 1 21 | 22 | // - apt-get install binutils-dev ... 23 | // - g++/clang++ -lbfd ... 24 | // #define BACKWARD_HAS_BFD 1 25 | 26 | // - apt-get install libdwarf-dev ... 27 | // - g++/clang++ -ldwarf ... 28 | // #define BACKWARD_HAS_DWARF 1 29 | 30 | // Regardless of the library you choose to read the debug information, 31 | // for potentially more detailed stack traces you can use libunwind 32 | // - apt-get install libunwind-dev 33 | // - g++/clang++ -lunwind 34 | // #define BACKWARD_HAS_LIBUNWIND 1 35 | 36 | #include "backward.hpp" 37 | 38 | namespace backward { 39 | 40 | backward::SignalHandling sh; 41 | 42 | } // namespace backward 43 | -------------------------------------------------------------------------------- /src/BackwardCpp/builds.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | COMPILERS_CXX98=`cat</dev/null 28 | ( 29 | cd "$builddir" 30 | cmake -DCMAKE_BUILD_TYPE=$buildtype -DBACKWARD_TESTS=ON .. 31 | ) 32 | } 33 | 34 | function build() { 35 | local builddir=$1 36 | shift 37 | make -C "$builddir" $@ 38 | } 39 | 40 | function dotest() { 41 | local builddir=$1 42 | shift 43 | make -C "$builddir" test $@ 44 | return 0 45 | } 46 | 47 | function do_action() { 48 | local lang=$1 49 | local action=$2 50 | shift 2 51 | 52 | for compiler in $COMPILERS; do 53 | local builddir="build_${lang}_${compiler}" 54 | 55 | if [[ $action == "cmake" ]]; then 56 | buildtype=$1 57 | mkbuild $compiler $lang "$buildtype" "$builddir" 58 | [[ $? != 0 ]] && exit 59 | elif [[ $action == "make" ]]; then 60 | build "$builddir" $@ 61 | [[ $? != 0 ]] && exit 62 | elif [[ $action == "test" ]]; then 63 | dotest "$builddir" $@ 64 | [[ $? != 0 ]] && exit 65 | elif [[ $action == "clean" ]]; then 66 | rm -r "$builddir" 67 | else 68 | echo "usage: $0 cmake [debug|release|relwithdbg]|make|test|clean" 69 | exit 255 70 | fi 71 | done 72 | } 73 | 74 | COMPILERS=$COMPILERS_CXX98 75 | do_action c++98 $@ 76 | COMPILERS=$COMPILERS_CXX11 77 | do_action c++11 $@ 78 | -------------------------------------------------------------------------------- /src/BackwardCpp/conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | import os 3 | 4 | class BackwardCpp(ConanFile): 5 | settings = 'os', 'compiler', 'build_type', 'arch' 6 | name = 'backward' 7 | url = 'https://github.com/bombela/backward-cpp' 8 | license = 'MIT' 9 | version = '1.3.0' 10 | options = { 11 | 'stack_walking_unwind': [True, False], 12 | 'stack_walking_backtrace': [True, False], 13 | 'stack_details_auto_detect': [True, False], 14 | 'stack_details_backtrace_symbol': [True, False], 15 | 'stack_details_dw': [True, False], 16 | 'stack_details_bfd': [True, False], 17 | 'shared': [True, False] 18 | } 19 | default_options = ( 20 | 'stack_walking_unwind=True', 21 | 'stack_walking_backtrace=False', 22 | 'stack_details_auto_detect=True', 23 | 'stack_details_backtrace_symbol=False', 24 | 'stack_details_dw=False', 25 | 'stack_details_bfd=False', 26 | 'shared=False' 27 | ) 28 | exports = 'backward.cpp', 'backward.hpp', 'test/*', 'CMakeLists.txt', 'BackwardConfig.cmake' 29 | generators = 'cmake' 30 | 31 | def build(self): 32 | cmake = CMake(self) 33 | 34 | cmake.configure(defs={'BACKWARD_' + name.upper(): value for name, value in self.options.values.as_list()}) 35 | cmake.build() 36 | 37 | def package(self): 38 | self.copy('backward.hpp', os.path.join('include', 'backward')) 39 | self.copy('*.a', dst='lib') 40 | self.copy('*.so', dst='lib') 41 | self.copy('*.lib', dst='lib') 42 | self.copy('*.dll', dst='lib') 43 | 44 | def package_info(self): 45 | self.cpp_info.libs = ['backward'] 46 | -------------------------------------------------------------------------------- /src/BackwardCpp/doc/nice.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/soytony/IntensityCalibration/0e027ef1d25ec0d385e25483a219480e6d45740a/src/BackwardCpp/doc/nice.png -------------------------------------------------------------------------------- /src/BackwardCpp/doc/pretty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/soytony/IntensityCalibration/0e027ef1d25ec0d385e25483a219480e6d45740a/src/BackwardCpp/doc/pretty.png -------------------------------------------------------------------------------- /src/BackwardCpp/doc/rude.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/soytony/IntensityCalibration/0e027ef1d25ec0d385e25483a219480e6d45740a/src/BackwardCpp/doc/rude.png -------------------------------------------------------------------------------- /src/BackwardCpp/test/_test_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _test_main.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "test.hpp" 25 | #include 26 | #include 27 | 28 | #ifdef _WIN32 29 | #include 30 | #define strcasecmp _stricmp 31 | #else 32 | #include 33 | #include 34 | #endif 35 | 36 | #if defined(__has_include) && __has_include() 37 | #include 38 | #else 39 | #include 40 | 41 | #ifdef _WIN32 42 | char argv0[MAX_PATH]; 43 | inline const char *getprogname() { 44 | return GetModuleFileName(NULL, argv0, sizeof(argv0)) ? argv0 : NULL; 45 | } 46 | #elif !defined(__APPLE__) 47 | // N.B. getprogname() is an Apple/BSD-ism. 48 | // program_invocation_name is a GLIBC-ism, but it's also 49 | // supported by libmusl. 50 | #define getprogname() program_invocation_name 51 | #endif 52 | 53 | void error(int status, int errnum, const char *format, ...) { 54 | fflush(stdout); 55 | fprintf(stderr, "%s: ", getprogname()); 56 | 57 | va_list args; 58 | va_start(args, format); 59 | vfprintf(stderr, format, args); 60 | va_end(args); 61 | 62 | if (errnum != 0) { 63 | fprintf(stderr, ": %s\n", strerror(errnum)); 64 | } else { 65 | fprintf(stderr, "\n"); 66 | } 67 | if (status != 0) { 68 | exit(status); 69 | } 70 | } 71 | #endif 72 | 73 | using namespace test; 74 | 75 | bool run_test(TestBase &test, bool use_child_process = true) { 76 | if (!use_child_process) { 77 | exit(static_cast(test.run())); 78 | } 79 | 80 | printf("-- running test case: %s\n", test.name); 81 | 82 | fflush(stdout); 83 | 84 | test::TestStatus status = test::SUCCESS; 85 | 86 | #ifdef _WIN32 87 | char filename[256]; 88 | GetModuleFileName(NULL, filename, 256); // TODO: check for error 89 | std::string cmd_line = filename; 90 | cmd_line += " --nofork "; 91 | cmd_line += test.name; 92 | 93 | STARTUPINFO si; 94 | PROCESS_INFORMATION pi; 95 | ZeroMemory(&si, sizeof(si)); 96 | si.cb = sizeof(si); 97 | ZeroMemory(&pi, sizeof(pi)); 98 | 99 | if (!CreateProcessA(nullptr, const_cast(cmd_line.c_str()), nullptr, 100 | nullptr, FALSE, 0, nullptr, nullptr, &si, &pi)) { 101 | printf("unable to create process\n"); 102 | exit(-1); 103 | } 104 | 105 | WaitForSingleObject(pi.hProcess, INFINITE); 106 | 107 | DWORD exit_code; 108 | GetExitCodeProcess(pi.hProcess, &exit_code); 109 | switch (exit_code) { 110 | case 3: 111 | status = test::SIGNAL_ABORT; 112 | break; 113 | case 5: 114 | status = test::EXCEPTION_UNCAUGHT; 115 | break; 116 | case EXCEPTION_ACCESS_VIOLATION: 117 | status = test::SIGNAL_SEGFAULT; 118 | break; 119 | case EXCEPTION_STACK_OVERFLOW: 120 | status = test::SIGNAL_SEGFAULT; 121 | break; 122 | case EXCEPTION_INT_DIVIDE_BY_ZERO: 123 | status = test::SIGNAL_DIVZERO; 124 | break; 125 | } 126 | printf("Exit code: %lu\n", exit_code); 127 | 128 | CloseHandle(pi.hProcess); 129 | CloseHandle(pi.hThread); 130 | 131 | if (test.expected_status == test::ASSERT_FAIL) { 132 | // assert calls abort on windows 133 | return (status & test::SIGNAL_ABORT); 134 | } 135 | 136 | #else 137 | 138 | pid_t child_pid = fork(); 139 | if (child_pid == 0) { 140 | exit(static_cast(test.run())); 141 | } 142 | if (child_pid == -1) { 143 | error(EXIT_FAILURE, 0, "unable to fork"); 144 | } 145 | 146 | int child_status = 0; 147 | waitpid(child_pid, &child_status, 0); 148 | 149 | if (WIFEXITED(child_status)) { 150 | int exit_status = WEXITSTATUS(child_status); 151 | if (exit_status & ~test::STATUS_MASK) { 152 | status = test::FAILED; 153 | } else { 154 | status = static_cast(exit_status); 155 | } 156 | } else if (WIFSIGNALED(child_status)) { 157 | const int signum = WTERMSIG(child_status); 158 | printf("!! signal (%d) %s\n", signum, strsignal(signum)); 159 | switch (signum) { 160 | case SIGABRT: 161 | status = test::SIGNAL_ABORT; 162 | break; 163 | case SIGSEGV: 164 | case SIGBUS: 165 | status = test::SIGNAL_SEGFAULT; 166 | break; 167 | case SIGFPE: 168 | status = test::SIGNAL_DIVZERO; 169 | break; 170 | default: 171 | status = test::SIGNAL_UNCAUGHT; 172 | } 173 | } 174 | 175 | #endif 176 | 177 | if (test.expected_status == test::FAILED) { 178 | return (status & test::FAILED); 179 | } 180 | 181 | if (test.expected_status == test::SIGNAL_UNCAUGHT) { 182 | return (status & test::SIGNAL_UNCAUGHT); 183 | } 184 | 185 | return status == test.expected_status; 186 | } 187 | 188 | int main(int argc, const char *const argv[]) { 189 | 190 | #ifdef _WIN32 191 | _set_abort_behavior(0, _WRITE_ABORT_MSG | _CALL_REPORTFAULT); 192 | #endif 193 | 194 | if (argc == 3 && strcmp("--nofork", argv[1]) == 0) { 195 | // Windows has no fork, so we simulate it 196 | // we only execute one test, without forking 197 | for (test_registry_t::iterator it = test_registry().begin(); 198 | it != test_registry().end(); ++it) { 199 | TestBase &test = **it; 200 | if (strcasecmp(argv[2], test.name) == 0) { 201 | run_test(test, false); 202 | 203 | return 0; 204 | } 205 | } 206 | return -1; 207 | } 208 | 209 | size_t success_cnt = 0; 210 | size_t total_cnt = 0; 211 | for (test_registry_t::iterator it = test_registry().begin(); 212 | it != test_registry().end(); ++it) { 213 | TestBase &test = **it; 214 | 215 | bool consider_test = (argc <= 1); 216 | for (int i = 1; i < argc; ++i) { 217 | if (strcasecmp(argv[i], test.name) == 0) { 218 | consider_test = true; 219 | break; 220 | } 221 | } 222 | if (!consider_test) { 223 | continue; 224 | } 225 | 226 | total_cnt += 1; 227 | if (run_test(test)) { 228 | printf("-- test case success: %s\n", test.name); 229 | success_cnt += 1; 230 | } else { 231 | printf("** test case FAILED : %s\n", test.name); 232 | } 233 | } 234 | printf("-- tests passing: %zu/%zu", success_cnt, total_cnt); 235 | if (total_cnt) { 236 | printf(" (%zu%%)\n", success_cnt * 100 / total_cnt); 237 | } else { 238 | printf("\n"); 239 | } 240 | return (success_cnt == total_cnt) ? EXIT_SUCCESS : EXIT_FAILURE; 241 | } 242 | -------------------------------------------------------------------------------- /src/BackwardCpp/test/rectrace.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/rectrace.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "backward.hpp" 25 | #include "test/test.hpp" 26 | #include 27 | 28 | using namespace backward; 29 | 30 | typedef StackTrace stacktrace_t; 31 | 32 | void end_of_our_journey(stacktrace_t &st) { 33 | if (!st.size()) { 34 | st.load_here(); 35 | } 36 | } 37 | 38 | int rec(stacktrace_t &st, int level) { 39 | if (level <= 1) { 40 | end_of_our_journey(st); 41 | return 0; 42 | } 43 | return rec(st, level - 1); 44 | } 45 | 46 | namespace toto { 47 | 48 | namespace titi { 49 | 50 | struct foo { 51 | 52 | union bar { 53 | NOINLINE static int trampoline(stacktrace_t &st, int level) { 54 | return rec(st, level); 55 | } 56 | }; 57 | }; 58 | 59 | } // namespace titi 60 | 61 | } // namespace toto 62 | 63 | TEST(recursion) { 64 | { // lexical scope. 65 | stacktrace_t st; 66 | const int input = 3; 67 | int r = toto::titi::foo::bar::trampoline(st, input); 68 | 69 | std::cout << "rec(" << input << ") == " << r << std::endl; 70 | 71 | Printer printer; 72 | // printer.address = true; 73 | printer.object = true; 74 | printer.print(st, stdout); 75 | } 76 | } 77 | 78 | int fib(StackTrace &st, int level) { 79 | if (level == 2) { 80 | return 1; 81 | } 82 | if (level <= 1) { 83 | end_of_our_journey(st); 84 | return 0; 85 | } 86 | return fib(st, level - 1) + fib(st, level - 2); 87 | } 88 | 89 | TEST(fibrecursive) { 90 | StackTrace st; 91 | const int input = 6; 92 | int r = fib(st, input); 93 | 94 | std::cout << "fib(" << input << ") == " << r << std::endl; 95 | 96 | Printer printer; 97 | printer.print(st, stdout); 98 | } 99 | -------------------------------------------------------------------------------- /src/BackwardCpp/test/select_signals.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/segfault.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "backward.hpp" 25 | 26 | #include "test/test.hpp" 27 | #include 28 | #include 29 | 30 | using namespace backward; 31 | 32 | void badass_function() { 33 | char *ptr = (char *)42; 34 | *ptr = 42; 35 | } 36 | 37 | TEST_SEGFAULT(pprint_sigsev) { 38 | std::vector signals; 39 | signals.push_back(SIGSEGV); 40 | SignalHandling sh(signals); 41 | std::cout << std::boolalpha << "sh.loaded() == " << sh.loaded() << std::endl; 42 | badass_function(); 43 | } 44 | 45 | TEST_SEGFAULT(wont_pprint) { 46 | std::vector signals; 47 | signals.push_back(SIGABRT); 48 | SignalHandling sh(signals); 49 | std::cout << std::boolalpha << "sh.loaded() == " << sh.loaded() << std::endl; 50 | badass_function(); 51 | } 52 | -------------------------------------------------------------------------------- /src/BackwardCpp/test/stacktrace.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/stacktrace.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "backward.hpp" 25 | #include "test/test.hpp" 26 | #include 27 | #include 28 | 29 | using namespace backward; 30 | 31 | void collect_trace(StackTrace &st) { st.load_here(); } 32 | 33 | TEST(minitrace) { 34 | Printer printer; 35 | 36 | StackTrace st; 37 | collect_trace(st); 38 | 39 | printer.print(st, std::cout); 40 | } 41 | 42 | void d(StackTrace &st) { st.load_here(); } 43 | 44 | void c(StackTrace &st) { return d(st); } 45 | 46 | void b(StackTrace &st) { return c(st); } 47 | 48 | NOINLINE void a(StackTrace &st) { return b(st); } 49 | 50 | TEST(smalltrace) { 51 | Printer printer; 52 | 53 | StackTrace st; 54 | a(st); 55 | 56 | printer.print(st, std::cout); 57 | } 58 | -------------------------------------------------------------------------------- /src/BackwardCpp/test/suicide.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/suicide.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "backward.hpp" 25 | 26 | #include "test/test.hpp" 27 | #include 28 | 29 | #ifndef _WIN32 30 | #include 31 | #endif 32 | 33 | using namespace backward; 34 | 35 | void badass_function() { 36 | char *ptr = (char *)42; 37 | *ptr = 42; 38 | } 39 | 40 | TEST_SEGFAULT(invalid_write) { badass_function(); } 41 | 42 | int you_shall_not_pass() { 43 | char *ptr = (char *)42; 44 | int v = *ptr; 45 | return v; 46 | } 47 | 48 | TEST_SEGFAULT(invalid_read) { 49 | int v = you_shall_not_pass(); 50 | std::cout << "v=" << v << std::endl; 51 | } 52 | 53 | void abort_abort_I_repeat_abort_abort() { 54 | std::cout << "Jumping off the boat!" << std::endl; 55 | abort(); 56 | } 57 | 58 | TEST_ABORT(calling_abort) { abort_abort_I_repeat_abort_abort(); } 59 | 60 | // aarch64 and mips does not trap Division by zero 61 | #if !defined(__aarch64__) && !defined(__mips__) 62 | volatile int zero = 0; 63 | 64 | int divide_by_zero() { 65 | std::cout << "And the wild black hole appears..." << std::endl; 66 | int v = 42 / zero; 67 | return v; 68 | } 69 | 70 | TEST_DIVZERO(divide_by_zero) { 71 | int v = divide_by_zero(); 72 | std::cout << "v=" << v << std::endl; 73 | } 74 | #endif 75 | 76 | // Darwin does not allow RLIMIT_STACK to be reduced 77 | #ifndef __APPLE__ 78 | int bye_bye_stack(int i) { return bye_bye_stack(i + 1) + bye_bye_stack(i * 2); } 79 | 80 | TEST_SEGFAULT(stackoverflow) { 81 | #ifndef _WIN32 82 | struct rlimit limit; 83 | limit.rlim_max = 8096; 84 | setrlimit(RLIMIT_STACK, &limit); 85 | #endif 86 | int r = bye_bye_stack(42); 87 | std::cout << "r=" << r << std::endl; 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /src/BackwardCpp/test/test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/test.cpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "test/test.hpp" 25 | #include 26 | #include 27 | #include 28 | 29 | TEST(empty_test) {} 30 | 31 | TEST_FAIL_ASSERT(fail_assert) { ASSERT(1 == 2); } 32 | 33 | TEST_FAIL_ASSERT(fail_assert_ge) { ASSERT_GE(4, 5); } 34 | 35 | TEST_UNCAUGHT_EXCEPTION(uncaught_exception) { 36 | throw std::runtime_error("some random runtime error"); 37 | } 38 | 39 | TEST_UNCAUGHT_EXCEPTION(uncaught_exception_int) { throw 42; } 40 | 41 | TEST_SEGFAULT(segfault) { 42 | char *a = 0; 43 | char b = a[42]; 44 | std::cout << "result: " << b << std::endl; 45 | } 46 | 47 | TEST_ABORT(abort) { abort(); } 48 | 49 | TEST(catch_int) { 50 | ASSERT_THROW({ throw 42; }, int); 51 | } 52 | 53 | TEST_FAIL_ASSERT(fail_catch_int) { ASSERT_THROW({}, int); } 54 | 55 | TEST_FAIL_ASSERT(fail_no_throw) { 56 | ASSERT_NO_THROW({ throw 42; }); 57 | } 58 | 59 | TEST(any_throw) { 60 | ASSERT_ANY_THROW({ throw 42; }); 61 | } 62 | 63 | TEST_FAIL_ASSERT(fail_any_throw) { ASSERT_ANY_THROW({}); } 64 | -------------------------------------------------------------------------------- /src/BackwardCpp/test/test.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test/test.hpp 3 | * Copyright 2013 Google Inc. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in 13 | * all copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #pragma once 25 | #ifndef H_54E531F7_9154_454B_BEB9_257408429470 26 | #define H_54E531F7_9154_454B_BEB9_257408429470 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace test { 36 | 37 | struct AssertFailedError : std::exception { 38 | ~AssertFailedError() throw() {} 39 | 40 | AssertFailedError(const char *filename, int _line, const char *_errmsg) 41 | : basename(_basename(filename)), line(_line), errmsg(_errmsg) {} 42 | 43 | const char *what() const throw() { 44 | if (!_what.size()) { 45 | std::ostringstream ss; 46 | ss << "assertion failed (" << basename << ":" << line; 47 | ss << ") " << errmsg; 48 | _what = ss.str(); 49 | } 50 | return _what.c_str(); 51 | } 52 | 53 | const char *basename; 54 | int line; 55 | const char *errmsg; 56 | 57 | mutable std::string _what; 58 | 59 | static const char *_basename(const char *filename) { 60 | const char *basename = filename + strlen(filename); 61 | while (basename != filename && *basename != '/') { 62 | basename -= 1; 63 | } 64 | return basename + 1; 65 | } 66 | }; 67 | 68 | enum TestStatus { 69 | SUCCESS = 0 << 0, 70 | FAILED = 1 << 0, 71 | 72 | ASSERT_FAIL = FAILED | 1 << 1, 73 | EXCEPTION_UNCAUGHT = FAILED | 2 << 1, 74 | SIGNAL_UNCAUGHT = FAILED | 3 << 1, 75 | SIGNAL_SEGFAULT = SIGNAL_UNCAUGHT | 1 << 3, 76 | SIGNAL_ABORT = SIGNAL_UNCAUGHT | 2 << 3, 77 | SIGNAL_DIVZERO = SIGNAL_UNCAUGHT | 2 << 3, 78 | 79 | STATUS_MASK = 0x1F 80 | }; 81 | 82 | struct TestBase { 83 | const char *name; 84 | TestStatus expected_status; 85 | 86 | virtual ~TestBase() {} 87 | TestBase(const char *, TestStatus); 88 | virtual void do_test() = 0; 89 | 90 | TestStatus run() { 91 | try { 92 | do_test(); 93 | return SUCCESS; 94 | } catch (const AssertFailedError &e) { 95 | printf("!! %s\n", e.what()); 96 | return ASSERT_FAIL; 97 | } catch (const std::exception &e) { 98 | printf("!! exception: %s\n", e.what()); 99 | return EXCEPTION_UNCAUGHT; 100 | } catch (...) { 101 | printf("!! unknown exception\n"); 102 | return EXCEPTION_UNCAUGHT; 103 | } 104 | } 105 | }; 106 | 107 | typedef std::vector test_registry_t; 108 | inline test_registry_t &test_registry() { 109 | static test_registry_t reg; 110 | return reg; 111 | } 112 | 113 | inline TestBase::TestBase(const char *n, TestStatus s) 114 | : name(n), expected_status(s) { 115 | test_registry().push_back(this); 116 | } 117 | 118 | } // namespace test 119 | 120 | #define _TEST_STATUS(name, status) \ 121 | struct TEST_##name : ::test::TestBase { \ 122 | TEST_##name() : TestBase(#name, status) {} \ 123 | void do_test(); \ 124 | } TEST_##name; \ 125 | void TEST_##name::do_test() 126 | 127 | #define TEST(name) _TEST_STATUS(name, ::test::SUCCESS) 128 | #define TEST_FAIL(name) _TEST_STATUS(name, ::test::FAILED) 129 | #define TEST_FAIL_ASSERT(name) _TEST_STATUS(name, ::test::ASSERT_FAIL) 130 | #define TEST_UNCAUGHT_EXCEPTION(name) \ 131 | _TEST_STATUS(name, ::test::EXCEPTION_UNCAUGHT) 132 | #define TEST_UNCAUGHT_SIGNAL(name) _TEST_STATUS(name, ::test::SIGNAL_UNCAUGHT) 133 | #define TEST_SEGFAULT(name) _TEST_STATUS(name, ::test::SIGNAL_SEGFAULT) 134 | #define TEST_ABORT(name) _TEST_STATUS(name, ::test::SIGNAL_ABORT) 135 | #define TEST_DIVZERO(name) _TEST_STATUS(name, ::test::SIGNAL_DIVZERO) 136 | 137 | #define ASSERT(expr) \ 138 | (expr) ? static_cast(0) \ 139 | : throw ::test::AssertFailedError(__FILE__, __LINE__, #expr) 140 | 141 | #define _ASSERT_BINOP(a, b, cmp) \ 142 | (!(a cmp b)) ? static_cast(0) \ 143 | : throw ::test::AssertFailedError( \ 144 | __FILE__, __LINE__, "because " #a " " #cmp " " #b) 145 | 146 | #define ASSERT_EQ(a, b) _ASSERT_BINOP(a, b, !=) 147 | #define ASSERT_NE(a, b) _ASSERT_BINOP(a, b, ==) 148 | #define ASSERT_LT(a, b) _ASSERT_BINOP(a, b, >=) 149 | #define ASSERT_LE(a, b) _ASSERT_BINOP(a, b, >) 150 | #define ASSERT_GT(a, b) _ASSERT_BINOP(a, b, <=) 151 | #define ASSERT_GE(a, b) _ASSERT_BINOP(a, b, <) 152 | 153 | #define ASSERT_THROW(expr, e_type) \ 154 | do { \ 155 | try { \ 156 | expr \ 157 | } catch (const e_type &) { \ 158 | break; \ 159 | } \ 160 | throw ::test::AssertFailedError(__FILE__, __LINE__, \ 161 | "expected exception " #e_type); \ 162 | } while (0) 163 | 164 | #define ASSERT_ANY_THROW(expr) \ 165 | do { \ 166 | try { \ 167 | expr \ 168 | } catch (...) { \ 169 | break; \ 170 | } \ 171 | throw ::test::AssertFailedError(__FILE__, __LINE__, \ 172 | "expected any exception"); \ 173 | } while (0) 174 | 175 | #define ASSERT_NO_THROW(expr) \ 176 | try { \ 177 | expr \ 178 | } catch (...) { \ 179 | throw ::test::AssertFailedError(__FILE__, __LINE__, \ 180 | "no exception expected"); \ 181 | } 182 | 183 | #endif /* H_GUARD */ 184 | -------------------------------------------------------------------------------- /src/BackwardCpp/test_package/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(backward-package-test) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake) 5 | conan_basic_setup(TARGETS) 6 | 7 | add_executable(example main.cpp) 8 | target_link_libraries(example PRIVATE ${CONAN_TARGETS}) 9 | -------------------------------------------------------------------------------- /src/BackwardCpp/test_package/conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | import os 3 | 4 | class TestBackward(ConanFile): 5 | settings = 'os', 'compiler', 'build_type', 'arch' 6 | generators = 'cmake' 7 | 8 | def build(self): 9 | cmake = CMake(self) 10 | cmake.configure(defs={'CMAKE_VERBOSE_MAKEFILE': 'ON'}) 11 | cmake.build() 12 | 13 | def test(self): 14 | self.run(os.path.join('.', 'bin', 'example')) 15 | -------------------------------------------------------------------------------- /src/BackwardCpp/test_package/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace backward; 7 | 8 | class TracedException : public std::runtime_error { 9 | public: 10 | TracedException() : std::runtime_error(_get_trace()) {} 11 | 12 | private: 13 | std::string _get_trace() { 14 | std::ostringstream ss; 15 | 16 | StackTrace stackTrace; 17 | TraceResolver resolver; 18 | stackTrace.load_here(); 19 | resolver.load_stacktrace(stackTrace); 20 | 21 | for (std::size_t i = 0; i < stackTrace.size(); ++i) { 22 | const ResolvedTrace trace = resolver.resolve(stackTrace[i]); 23 | 24 | ss << "#" << i << " at " << trace.object_function << "\n"; 25 | } 26 | 27 | return ss.str(); 28 | } 29 | }; 30 | 31 | void f(int i) { 32 | if (i >= 42) { 33 | throw TracedException(); 34 | } else { 35 | std::cout << "i=" << i << "\n"; 36 | f(i + 1); 37 | } 38 | } 39 | 40 | int main() { 41 | try { 42 | f(0); 43 | } catch (const TracedException &ex) { 44 | std::cout << ex.what(); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /src/g2oGraphElements.h: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | #ifndef REMISSION_LEARNER_G2O_GRAPH_ELEMENTS_H 9 | #define REMISSION_LEARNER_G2O_GRAPH_ELEMENTS_H 10 | 11 | #include "g2o/core/base_vertex.h" 12 | #include "g2o/core/base_unary_edge.h" 13 | #include "g2o/core/base_binary_edge.h" 14 | #include "g2o/core/base_multi_edge.h" 15 | #include "g2o/core/hyper_graph_action.h" 16 | //#include "aislib/stuff/macros.h" 17 | 18 | //#include 19 | //#include "ais3dTools/visualization/aluGLViewer/alu_glviewer.h" 20 | #include "remissionCalibrationHelper.h" 21 | #include "remissionCalibrationResult.h" 22 | 23 | class RemissionLearnerVertex : public g2o::BaseVertex<1, double> 24 | { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | typedef BaseVertex<1, double> BaseClass; 29 | 30 | RemissionLearnerVertex(); 31 | 32 | virtual void setToOriginImpl() { 33 | _estimate = 1.0; 34 | } 35 | 36 | virtual void oplusImpl(const double* update) 37 | { 38 | _estimate += *update; 39 | } 40 | virtual bool read(std::istream& is); 41 | virtual bool write(std::ostream& os) const; 42 | }; 43 | 44 | class RemissionLearnerUnaryEdge : public g2o::BaseUnaryEdge<1, double, RemissionLearnerVertex> 45 | { 46 | public: 47 | typedef g2o::BaseUnaryEdge<1, double, RemissionLearnerVertex> BaseClass; 48 | 49 | RemissionLearnerUnaryEdge(); 50 | 51 | static inline double jacobian(double vertexValue); 52 | inline void computeError(); 53 | inline void linearizeOplusNumeric(); 54 | inline void linearizeOplusJacobian(); 55 | virtual void linearizeOplus(); 56 | virtual bool read(std::istream& is); 57 | virtual bool write(std::ostream& os) const; 58 | 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 60 | }; 61 | 62 | class RemissionLearnerPriorEdge : public g2o::BaseUnaryEdge<1, double, RemissionLearnerVertex> 63 | { 64 | public: 65 | typedef g2o::BaseUnaryEdge<1, double, RemissionLearnerVertex> BaseClass; 66 | 67 | RemissionLearnerPriorEdge(float value=1.0f, float weight=1.0f); 68 | 69 | static inline double jacobian(double vertexValue); 70 | inline void computeError(); 71 | virtual void linearizeOplus(); 72 | virtual bool read(std::istream& is); 73 | virtual bool write(std::ostream& os) const; 74 | 75 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 76 | }; 77 | 78 | 79 | class RemissionLearnerMultiEdge_n : public g2o::BaseMultiEdge<1, double> { 80 | public: 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 82 | 83 | typedef g2o::BaseMultiEdge<1, double> BaseClass; 84 | 85 | RemissionLearnerMultiEdge_n(int dimension); 86 | 87 | inline void computeError(); 88 | 89 | inline void linearizeOplusNumeric(); 90 | inline void linearizeOplusJacobian(); 91 | virtual void linearizeOplus(); 92 | virtual bool read(std::istream& is); 93 | virtual bool write(std::ostream& os) const; 94 | }; 95 | 96 | class RemissionCalibrationGridEdge : public g2o::BaseBinaryEdge<1, double, RemissionLearnerVertex, RemissionLearnerVertex> 97 | { 98 | public: 99 | typedef g2o::BaseBinaryEdge<1, double, RemissionLearnerVertex, RemissionLearnerVertex> BaseClass; 100 | RemissionCalibrationGridEdge(RemissionLearnerVertex* v1, RemissionLearnerVertex* v2, double weight); 101 | inline void computeError(); 102 | virtual void linearizeOplus(); 103 | virtual bool read(std::istream& is); 104 | virtual bool write(std::ostream& os) const; 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 106 | }; 107 | 108 | class RemissionCalibrationEdge_2 : public g2o::BaseBinaryEdge<1, double, RemissionLearnerVertex, RemissionLearnerVertex> 109 | { 110 | public: 111 | typedef g2o::BaseBinaryEdge<1, double, RemissionLearnerVertex, RemissionLearnerVertex> BaseClass; 112 | RemissionCalibrationEdge_2(); 113 | inline void computeError(); 114 | virtual void linearizeOplus(); 115 | inline void linearizeOplusNumeric(); 116 | inline void linearizeOplusJacobian(); 117 | virtual bool read(std::istream& is); 118 | virtual bool write(std::ostream& os) const; 119 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 120 | }; 121 | 122 | class RemissionCalibrationEdgeMode2 : public g2o::BaseMultiEdge<1, double> 123 | { 124 | public: 125 | typedef g2o::BaseMultiEdge<1, double> BaseClass; 126 | RemissionCalibrationEdgeMode2(); 127 | inline void computeError(); 128 | virtual void linearizeOplus(); 129 | inline void linearizeOplusNumeric(); 130 | inline void linearizeOplusJacobian(); 131 | virtual bool read(std::istream& is); 132 | virtual bool write(std::ostream& os) const; 133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 134 | }; 135 | 136 | class RemissionCalibrationEdgeMode3 : public g2o::BaseMultiEdge<1, double> 137 | { 138 | public: 139 | typedef g2o::BaseMultiEdge<1, double> BaseClass; 140 | RemissionCalibrationEdgeMode3(); 141 | inline void computeError(); 142 | virtual void linearizeOplus(); 143 | inline void linearizeOplusNumeric(); 144 | inline void linearizeOplusJacobian(); 145 | virtual bool read(std::istream& is); 146 | virtual bool write(std::ostream& os) const; 147 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 148 | }; 149 | 150 | typedef std::map > VerticesMode1; 151 | 152 | struct CalibIndependentVertices { 153 | std::vector verticesRange, 154 | verticesIncidenceAngle, 155 | verticesIntensity; 156 | }; 157 | typedef std::map VerticesMode2; 158 | 159 | struct LaserGroupVertices { 160 | std::vector verticesRange; 161 | std::vector verticesIncidenceAngle; 162 | std::vector verticesIntensity; 163 | std::map verticesLaser; 164 | }; 165 | typedef std::vector VerticesMode3; 166 | 167 | class G2oPostIterationAction : public g2o::HyperGraphAction { 168 | public: 169 | G2oPostIterationAction() : remissionCalibrationResult(NULL), 170 | verticesMode1(NULL), verticesMode2(NULL), verticesMode3(NULL), verticesForMapCells(NULL), 171 | mapCells(NULL) {} //, qApplication(NULL), viewer(NULL) {} 172 | 173 | virtual g2o::HyperGraphAction* operator()(const g2o::HyperGraph* graph, g2o::HyperGraphAction::Parameters* parameters=NULL); 174 | 175 | RemissionCalibrationResult* remissionCalibrationResult; 176 | const VerticesMode1* verticesMode1; 177 | const VerticesMode2* verticesMode2; 178 | const VerticesMode3* verticesMode3; 179 | const std::vector* verticesForMapCells; 180 | RemissionCalibrationHelper::CollectedPointsMap* mapCells; 181 | //QApplication* qApplication; 182 | //Ais3dTools::ALUGLViewer* viewer; 183 | }; 184 | 185 | 186 | 187 | #include "g2oGraphElements.hpp" 188 | 189 | #endif 190 | -------------------------------------------------------------------------------- /src/g2oGraphElements.hpp: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | void RemissionLearnerUnaryEdge::computeError() { 9 | double& e = _error[0]; 10 | double v = static_cast(_vertices[0])->estimate(); 11 | e = 0.0; 12 | if (v < 1.0) 13 | e = (1.0/v) + v - 2.0; 14 | } 15 | 16 | void RemissionLearnerUnaryEdge::linearizeOplusNumeric() { 17 | BaseClass::linearizeOplus(); 18 | } 19 | 20 | void RemissionLearnerUnaryEdge::linearizeOplusJacobian() { 21 | double& dx = _jacobianOplusXi(0,0); 22 | double v = static_cast(_vertices[0])->estimate(); 23 | dx = 0.0; 24 | if (v < 1.0) 25 | dx = (-1.0/(v*v)) + 1; 26 | } 27 | 28 | void RemissionLearnerPriorEdge::computeError() { 29 | double& e = _error[0]; 30 | double v = static_cast(_vertices[0])->estimate(); 31 | e = v-_measurement; 32 | } 33 | 34 | void RemissionLearnerMultiEdge_n::computeError() { 35 | double averageNormalizationFactor = 0; 36 | for (size_t i=0; i<_vertices.size(); ++i) { 37 | averageNormalizationFactor += static_cast(_vertices[i])->estimate(); 38 | //std::cout << static_cast(_vertices[i])->estimate()<<"+"; 39 | } 40 | //std::cout << " = "<(_vertices[0])->estimate(), 58 | normalizationFactor2 = static_cast(_vertices[1])->estimate(); 59 | //_error[0] = (normalizationFactor1-normalizationFactor2); 60 | _error[0] = ((normalizationFactor1-normalizationFactor2)-_measurement)/(normalizationFactor1+normalizationFactor2); 61 | } 62 | 63 | void RemissionCalibrationEdge_2::computeError() { 64 | double normalizationFactor = static_cast(_vertices[0])->estimate(), 65 | cellIntensity = static_cast(_vertices[1])->estimate(), 66 | measuredIntensity = _measurement; 67 | _error[0] = normalizationFactor*measuredIntensity - cellIntensity; 68 | } 69 | 70 | void RemissionCalibrationEdge_2::linearizeOplusNumeric() { 71 | BaseClass::linearizeOplus(); 72 | } 73 | 74 | void RemissionCalibrationEdge_2::linearizeOplusJacobian() { 75 | //double normalizationFactor = static_cast(_vertices[0])->estimate(), // x 76 | //cellIntensity = static_cast(_vertices[1])->estimate(); // y 77 | double measuredIntensity = _measurement; 78 | double& dx = _jacobianOplusXi(0,0), 79 | & dy = _jacobianOplusXj(0,0); 80 | dx = measuredIntensity; 81 | dy = 0; 82 | } 83 | 84 | void RemissionCalibrationEdgeMode2::computeError() { 85 | double normalizationFactorRange = static_cast(_vertices[0])->estimate(), 86 | normalizationFactorAngle = static_cast(_vertices[1])->estimate(), 87 | normalizationFactorIntensity = static_cast(_vertices[2])->estimate(), 88 | cellIntensity = static_cast(_vertices[3])->estimate(), 89 | measuredIntensity = _measurement; 90 | _error[0] = normalizationFactorRange*normalizationFactorAngle*normalizationFactorIntensity*measuredIntensity - cellIntensity; 91 | } 92 | 93 | void RemissionCalibrationEdgeMode2::linearizeOplusJacobian() { 94 | double normalizationFactorRange = static_cast(_vertices[0])->estimate(), // x1 95 | normalizationFactorAngle = static_cast(_vertices[1])->estimate(), // x2 96 | normalizationFactorIntensity = static_cast(_vertices[2])->estimate(), // x3 97 | //cellIntensity = static_cast(_vertices[3])->estimate(), // y 98 | measuredIntensity = _measurement; 99 | double& dx1 = _jacobianOplus[0](0,0), 100 | & dx2 = _jacobianOplus[1](0,0), 101 | & dx3 = _jacobianOplus[2](0,0), 102 | & dy = _jacobianOplus[3](0,0); 103 | dx1 = normalizationFactorAngle*normalizationFactorIntensity*measuredIntensity; 104 | dx2 = normalizationFactorRange*normalizationFactorIntensity*measuredIntensity; 105 | dx3 = normalizationFactorRange*normalizationFactorRange*measuredIntensity; 106 | dy = 0; 107 | } 108 | 109 | void RemissionCalibrationEdgeMode2::linearizeOplusNumeric() { 110 | BaseClass::linearizeOplus(); 111 | } 112 | 113 | void RemissionCalibrationEdgeMode3::computeError() { 114 | double normalizationFactorRange = static_cast(_vertices[0])->estimate(), 115 | normalizationFactorAngle = static_cast(_vertices[1])->estimate(), 116 | normalizationFactorIntensity = static_cast(_vertices[2])->estimate(), 117 | normalizationFactorLaser = static_cast(_vertices[3])->estimate(), 118 | cellIntensity = static_cast(_vertices[4])->estimate(), 119 | measuredIntensity = _measurement; 120 | _error[0] = normalizationFactorRange*normalizationFactorAngle*normalizationFactorIntensity*normalizationFactorLaser*measuredIntensity - 121 | cellIntensity; 122 | } 123 | 124 | void RemissionCalibrationEdgeMode3::linearizeOplusJacobian() { 125 | double normalizationFactorRange = static_cast(_vertices[0])->estimate(), // x1 126 | normalizationFactorAngle = static_cast(_vertices[1])->estimate(), // x2 127 | normalizationFactorIntensity = static_cast(_vertices[2])->estimate(), // x3 128 | normalizationFactorLaser = static_cast(_vertices[3])->estimate(), // x4 129 | //cellIntensity = static_cast(_vertices[4])->estimate(), // y 130 | measuredIntensity = _measurement; 131 | double& dx1 = _jacobianOplus[0](0,0), 132 | & dx2 = _jacobianOplus[1](0,0), 133 | & dx3 = _jacobianOplus[2](0,0), 134 | & dx4 = _jacobianOplus[3](0,0), 135 | & dy = _jacobianOplus[4](0,0); 136 | dx1 = normalizationFactorAngle*normalizationFactorIntensity*normalizationFactorLaser*measuredIntensity; 137 | dx2 = normalizationFactorRange*normalizationFactorIntensity*normalizationFactorLaser*measuredIntensity; 138 | dx3 = normalizationFactorRange*normalizationFactorAngle*normalizationFactorLaser*measuredIntensity; 139 | dx4 = normalizationFactorRange*normalizationFactorAngle*normalizationFactorIntensity*measuredIntensity; 140 | dy = 0; 141 | 142 | //std::cout << "-----\n"; 143 | //std::cout << dx1<<", "< 6 | 7 | 8 | //#include 9 | #include 10 | #include "remissionCalibrationHelper.h" 11 | 12 | 13 | #define BACKWARD_HAS_DW 1 14 | 15 | //bool useGui = false; 16 | 17 | size_t approximateNumber = 1e6; 18 | std::string& skipLasersString = RemissionCalibrationHelper::skipLasersString; 19 | 20 | void printUsage(const char* progName) { 21 | std::cout << "\n\nUsage: "< Resolution of the map (default: "< Minimum number of points for averaging (default: " 27 | << RemissionCalibrationHelper::parameters.minNoOfMeasurementsForDataPointMean<<")\n" 28 | << "-s Skip lasers. The listed lasers will be ignored. E.g.: \"1,{2,3,5,7},4-8,{4,6,101,103-107}\".\n" 29 | << "-d Folder\n" 30 | << "-h This help\n" 31 | << "\n\n"; 32 | } 33 | 34 | int main(int argc, char** argv) 35 | { 36 | setlocale(LC_NUMERIC, "C"); 37 | 38 | std::string folder = "."; 39 | 40 | // -------------------------------------- 41 | // -----Parse Command Line Arguments----- 42 | // -------------------------------------- 43 | for (char c; (c = getopt(argc, argv, "r:n:s:d:h")) != -1; ) { 44 | switch (c) { 45 | //case 'g': 46 | //useGui = true; 47 | //std::cout << "Using GUI.\n"; 48 | //break; 49 | case 'r': 50 | RemissionCalibrationHelper::parameters.mapCellSize = atof(optarg); 51 | std::cout << "Setting map resolution to "<processEvents(); 78 | //} 79 | 80 | std::string mapCellsFilename = folder+"/remissionCalibrationMapCells.dat"; 81 | 82 | RemissionCalibrationHelper::CollectedPointsMap mapCells; 83 | if (RemissionCalibrationHelper::calculateMapCells(folder, mapCells, mapCellsFilename)) 84 | return 0; 85 | else 86 | return 1; 87 | } 88 | -------------------------------------------------------------------------------- /src/pointXYZIVPL.h: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | #ifndef POINTXYZIVPL_H 9 | #define POINTXYZIVPL_H 10 | 11 | #include 12 | 13 | namespace pcl { 14 | 15 | struct EIGEN_ALIGN16 _PointXYZIVPL { 16 | PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) 17 | union 18 | { 19 | struct 20 | { 21 | float intensity; 22 | float vp_x; 23 | float vp_y; 24 | float vp_z; 25 | }; 26 | float data2[4]; 27 | }; 28 | union 29 | { 30 | struct 31 | { 32 | int laser; 33 | int unused1; 34 | int unused2; 35 | int unused3; 36 | }; 37 | float data3[4]; 38 | }; 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | }; 41 | struct EIGEN_ALIGN16 PointXYZIVPL : public _PointXYZIVPL 42 | { 43 | inline PointXYZIVPL (const _PointXYZIVPL &p) 44 | { 45 | x = p.x; y = p.y; z = p.z; data[3] = 1.0f; 46 | intensity = p.intensity; 47 | vp_x = p.vp_x; vp_y = p.vp_y; vp_z = p.vp_z; 48 | laser = p.laser; 49 | } 50 | 51 | inline PointXYZIVPL (float _x = 0.0f, float _y = 0.0f, float _z = 0.0f, 52 | float _intensity = 0.0f, float _vp_x = 0.0f, float _vp_y = 0.0f, float _vp_z = 0.0f, int _laser=0) 53 | { 54 | x = _x; y = _y; z = _z; 55 | data[3] = 1.0f; 56 | intensity = _intensity; 57 | vp_x = _vp_x; vp_y = _vp_y; vp_z = _vp_z; 58 | laser = _laser; 59 | data3[1]=data3[2]=data3[3] = 1.0f; 60 | } 61 | }; 62 | inline std::ostream& operator << (std::ostream& os, const PointXYZIVPL& p) 63 | { 64 | os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.intensity 65 | << " - " << p.vp_x << "," << p.vp_y << "," << p.vp_z << " - " << p.laser << ")"; 66 | return (os); 67 | } 68 | 69 | } 70 | 71 | POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZIVPL, 72 | (float, x, x) 73 | (float, y, y) 74 | (float, z, z) 75 | (float, intensity, intensity) 76 | (float, vp_x, vp_x) 77 | (float, vp_y, vp_y) 78 | (float, vp_z, vp_z) 79 | (int, laser, laser) 80 | ) 81 | POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZIVPL, pcl::_PointXYZIVPL) 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /src/remissionCalibration.cpp: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | //#include 9 | #include 10 | #include "remissionCalibrationHelper.h" 11 | 12 | //bool useGui = false; 13 | 14 | RemissionCalibrationResult& calibration = RemissionCalibrationHelper::remissionCalibrationResult; 15 | int& noOfRangeCells = calibration.noOfCellsX, 16 | & noOfIncidenceAngleCells = calibration.noOfCellsY, 17 | & noOfIntensityCells = calibration.noOfCellsZ; 18 | float & outlierPercentage = RemissionCalibrationHelper::parameters.outlierPercentage; 19 | int mode = calibration.mode; 20 | 21 | float fakeDatasetNoise = 0.05f; 22 | 23 | void printUsage(const char* progName) { 24 | std::cout << "\n\nUsage: "< Folder\n" 29 | << "-i Number of G2O iterations (default: "< Number of EM iterations (default: "< Calibration values file\n" 34 | << "-x Number of cells in x, determining in how many steps the range is divided " 35 | << "(default "< Number of cells in y, determining in how many steps the incidence angle is divided " 37 | << "(default "< Number of cells in z, determining in how many steps the intensity is divided " 39 | << "(default "< mode - 1: full graph. 2: range&angle independency.\n" 41 | << " 3: range&angle dependency plus laser grouping.\n" 42 | << " 4: mode 3, followed by mode 2. (default: "< laser groups. Only applicable for mode 3. E.g.: \"1,{2,3,5,7},4-8,{4,6,101,103-107}\".\n" 44 | << " All lasers not in the string get their own group.\n" 45 | << "-s skip lasers. The listed lasers will be ignored. Same format as for -l.\n" 46 | << "-o outlierPercentage (default: "<processEvents(); 173 | //} 174 | 175 | if (writeFakePointClouds) { 176 | std::cerr << "I will now write point clouds with fake intensities from the point clouds in folder \"" 177 | << folder<<".\n"; 178 | if (!RemissionCalibrationHelper::writeFakeDataset(folder, fakeDatasetNoise)) { 179 | std::cerr << "Something went wrong...\n"; 180 | return 1; 181 | } 182 | return 0; 183 | } 184 | 185 | if (normalizePointClouds) { 186 | if (calibrationValuesFileName.empty()) { 187 | std::cerr << "I am supposed to normalize the intensities of the point clouds in folder \"" 188 | << folder<<"\", but no calibration file is given. :-(\n"; 189 | return 1; 190 | } 191 | std::cout << "I will now normalize the intensities of the point clouds in folder \"" 192 | << folder<<"\" using the calibration file \""< 6 | 7 | 8 | #ifndef REMISSION_CALIBRATION_HELPER_H 9 | #define REMISSION_CALIBRATION_HELPER_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "g2o/core/sparse_optimizer.h" 17 | 18 | #include "remissionCalibrationResult.h" 19 | #include "pointXYZIVPL.h" 20 | 21 | // Forward declarations 22 | class QApplication; 23 | namespace pcl { 24 | template 25 | class PointCloud; 26 | } 27 | 28 | class RemissionCalibrationHelper { 29 | public: 30 | //-----STRUCTS & TYPEDEFS----- 31 | typedef pcl::PointXYZIVPL PointType; 32 | typedef pcl::PointCloud PointCloudType; 33 | 34 | struct Parameters { 35 | Parameters() : mapCellSize(0.5f), maxIntensityDeviation(0.02), minNoOfMeasurementsForDataPointMean(10), 36 | rangeStepFactorForPointAveraging(1.1), noOfIncidenceAngleCellsForAveraging(10), 37 | noOfIterationsForG2O(-1), noOfIterationsForEM(-1), useExpectationMaximizationMethod(true), 38 | gridWeight(1.0f), measurementWeight(1.0f), useRobustKernel(false), outlierPercentage(1.0f), 39 | autoStopRmseChange(1e-4), useMode3ResultToInitializeMode2(false) {} 40 | 41 | float mapCellSize; //!< Cell size in meters, used for clustering points together 42 | float maxIntensityDeviation; //!< Maximum allowed deviation for intensity values in a cell (for values normalized to [0,1]) 43 | size_t minNoOfMeasurementsForDataPointMean; //!< Minimum number of points to average before adding to map 44 | float rangeStepFactorForPointAveraging; //!< Range factor to split up averaging 45 | float noOfIncidenceAngleCellsForAveraging; //!< no of cells to split up averaging 46 | int noOfIterationsForG2O; //!< No of iterations performed by the optimizer. -1 means automatic. 47 | int noOfIterationsForEM; //!< No of iterations performed by the EM algorithm. 48 | bool useExpectationMaximizationMethod; //!< Should the calibration procedure be based on EM? 49 | float gridWeight; //!< Weight of the edges defining the cell grid 50 | float measurementWeight; //!< Weight of the actual measurement edges 51 | bool useRobustKernel; //!< Use robust kernel for the optimization 52 | float outlierPercentage; //!< Percentage of edges with highest error to be ignored 53 | float autoStopRmseChange; //!< Maximum change in RMSE to stop the EM iterations 54 | bool useMode3ResultToInitializeMode2; //!< Use result from mode 3 to initialize mode 2 optimization 55 | }; 56 | 57 | //#pragma pack(1) 58 | struct CollectedPoint { 59 | CollectedPoint() : x(0),y(0),z(0), 60 | //nX(0),nY(0),nZ(0), 61 | range(0),incidenceAngle(0),intensity(0),laser(0) {} 62 | 63 | void projectOntoCellMiddle(const RemissionCalibrationResult& calibration); 64 | 65 | float x, y, z; 66 | //float nX, nY, nZ; 67 | float range; 68 | float incidenceAngle; 69 | float intensity; 70 | int laser; 71 | }; 72 | class CollectedPoints : public std::vector { 73 | public: 74 | CollectedPoints() : std::vector(), cellValue(-1), cellBad(false) {} 75 | float getAverageCalibratedIntensity(const RemissionCalibrationResult& calibration) const; 76 | pcl::PointXYZI getPointRepresentingCell() const; 77 | float getChi2(const RemissionCalibrationResult& calibration) const; 78 | float getRMSE(const RemissionCalibrationResult& calibration) const; 79 | float cellValue; 80 | bool cellBad; 81 | }; 82 | class CollectedPointsMap : public std::vector { 83 | public: 84 | std::set getLaserIds() const; 85 | std::map > getMinMaxRanges() const; 86 | std::map getMaxIntensities() const; 87 | float getMaxIntensity() const; 88 | std::map getAverageIntensities() const; 89 | float getAverageIntensity() const; 90 | float getAverageCalibratedIntensity(const RemissionCalibrationResult& calibration) const; 91 | float getMaxCalibratedIntensity(const RemissionCalibrationResult& calibration) const; 92 | void calibrateIntensities(const RemissionCalibrationResult& calibration); 93 | void normalizeIntensities(const std::map& normalizationValues, bool reciprocal=false); 94 | void normalizeIntensities(float normalizationValue); 95 | void setCellValuesToCalibratedMeans(const RemissionCalibrationResult& calibration); 96 | float getChi2(const RemissionCalibrationResult& calibration) const; 97 | size_t getNoOfDataPoints() const; 98 | float getRMSE(const RemissionCalibrationResult& calibration) const; 99 | void projectOntoCellMiddles(const RemissionCalibrationResult& calibration); 100 | void removeExtremeIntensities(float percentage); 101 | void removeOutliers(float percentage, const RemissionCalibrationResult& calibration); 102 | void removeOutlierCells(float percentage, const RemissionCalibrationResult& calibration); 103 | std::map getErrorDistribution(const RemissionCalibrationResult& calibration, float stepSize=0.1) const; 104 | std::map getErrorPercentages(const RemissionCalibrationResult& calibration) const; 105 | 106 | void writeMapCloudToDisk() const; 107 | void writeCalibratedMapCloudToDisk(const RemissionCalibrationResult& calibration) const; 108 | void writeToDisk(const std::string& filename) const; 109 | bool readFromDisk(const std::string& filename); 110 | }; 111 | 112 | //-----CONSTRUCTOR & DESTRUCTOR----- 113 | RemissionCalibrationHelper(); 114 | 115 | //-----PUBLIC STATIC FUNCTIONS----- 116 | static inline float getCosIncidenceAngle(const PointType& point, const Eigen::Vector3f& normal); 117 | static void runG2oOptimization(CollectedPointsMap& mapCells, int noOfIterations, 118 | bool performIntensityInfluenceCalibration=false); 119 | static void runG2oOptimizationMode1(const CollectedPointsMap& mapCells, g2o::SparseOptimizer& optimizer); 120 | static void runG2oOptimizationMode2(const CollectedPointsMap& mapCells, g2o::SparseOptimizer& optimizer, 121 | bool performIntensityInfluenceCalibration=false); 122 | static void runG2oOptimizationMode3(const CollectedPointsMap& mapCells, g2o::SparseOptimizer& optimizer, 123 | bool performIntensityInfluenceCalibration=false); 124 | static void polynomialApproximation(); 125 | //static void showCalibrationValues(); 126 | static void setCalibrationToInitialGuess(const CollectedPointsMap& mapCells); 127 | static bool calculateCalibrationBasedOnMapCells(const std::string& filename, bool skipInitialization=false); 128 | static bool calculateMapCells(const std::string& folder, CollectedPointsMap& mapCells, const std::string& mapCellsFilename=""); 129 | static bool writeCalibratedClouds(const std::string& folder, const std::string& calibrationValuesFilename); 130 | static bool writeFakeDataset(const std::string& folder, float standardDeviation=0.0f); 131 | static std::vector getScanFilenames(const std::string& folder); 132 | static void initializeCalibrationValues(const std::set& laserIds); 133 | static void initializeCalibrationValues(const std::map& initialValuesPerLaser); 134 | static void calculateInitialRMSE(float& maxNormalizationRMSE, float& avgNormalizationRMSE); 135 | static std::vector > parseLaserGroupString(const std::string& inputString); 136 | static std::set getSkipLaserSet(); 137 | 138 | //-----PUBLIC MEMBERS----- 139 | static QApplication* qApplication; 140 | static RemissionCalibrationResult remissionCalibrationResult; 141 | static Parameters parameters; 142 | static std::string laserGroupString; 143 | static std::string skipLasersString; 144 | }; 145 | 146 | #include "remissionCalibrationHelper.hpp" 147 | 148 | #endif 149 | -------------------------------------------------------------------------------- /src/remissionCalibrationHelper.hpp: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | float RemissionCalibrationHelper::getCosIncidenceAngle(const PointType& point, const Eigen::Vector3f& normal) { 9 | //return 1.0f; // HACK! 10 | 11 | Eigen::Vector3f p = point.getVector3fMap(), 12 | sensorPos(point.vp_x, point.vp_y, point.vp_z); 13 | Eigen::Vector3f viewingDirection = (sensorPos-p).normalized(); 14 | float cosIncidenceAngleToUse = std::abs(viewingDirection.dot(normal)); 15 | //std::cout << PVARN(cosIncidenceAngleToUse); 16 | cosIncidenceAngleToUse = std::min(1.0f, cosIncidenceAngleToUse); 17 | 18 | return cosIncidenceAngleToUse; 19 | } 20 | 21 | -------------------------------------------------------------------------------- /src/remissionCalibrationResult.h: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | #ifndef REMISSION_CALIBRATION_RESULT_H 9 | #define REMISSION_CALIBRATION_RESULT_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | //#include "aislib/stuff/macros.h" 17 | 18 | class RemissionCalibrationResult { 19 | public: 20 | //-----STRUCTS & TYPEDEFS----- 21 | 22 | //-----CONSTRUCTOR & DESTRUCTOR----- 23 | RemissionCalibrationResult(); 24 | 25 | //-----PUBLIC STATIC FUNCTIONS----- 26 | 27 | //-----PUBLIC FUNCTIONS----- 28 | void setNoOfCells(int newNoOfCellsX=50, int newNoOfCellsY=1, int newNoOfCellsZ=1); 29 | void saveCalibrationValues(const std::string& filename) const; 30 | void normalizeCalibrationFactors(float normalization); 31 | bool readCalibrationValuesFromFile(const std::string& fileName); 32 | 33 | void setMaxUncalibratedIntensities(const std::map& newMaxUncalibratedIntensities); 34 | void updateMaxUncalibratedIntensitiesForLaserGroups(); 35 | 36 | inline float getCalibratedIntensity(float range, float incidenceAngle, float intensity, int laser) const; 37 | 38 | inline float getCalibrationFactor(float cellX, float cellY, float cellZ, int laser) const; 39 | 40 | //! value>1.0 (range_0=0.0, range_(i+1)=rangeStepFactor*(range_i-1.0)) 41 | void setRangeStepFactor(float newRangeStepFactor=1.1f) { rangeStepFactor = newRangeStepFactor; } 42 | 43 | inline void rangeToCell(float range, float& cell) const; 44 | inline void rangeToCell(float range, int& cell) const; 45 | inline void cellToRange(float cell, float& range) const; 46 | 47 | inline void incidenceAngleToCell(float incidenceAngle, float& cell) const; 48 | inline void incidenceAngleToCell(float incidenceAngle, int& cell) const; 49 | inline void cellToIncidenceAngle(float cell, float& incidenceAngle) const; 50 | 51 | inline void normalizedIntensityToCell(float intensity, float& cell) const; 52 | inline void normalizedIntensityToCell(float intensity, int& cell) const; 53 | inline void cellToNormalizedIntensity(float cell, float& intensity) const; 54 | inline void intensityToCell(float intensity, int laser, float& cell) const; 55 | inline void intensityToCell(float intensity, int laser, int& cell) const; 56 | inline void cellToIntensity(float cell, int laser, float& intensity) const; 57 | 58 | inline void rangeAndIncidenceAngleToCell(float range, float incidenceAngle, float& cellX, float& cellY) const; 59 | inline void rangeAndIncidenceAngleToCell(float range, float incidenceAngle, int& cellX, int& cellY) const; 60 | 61 | std::set getAvailableLaserIds() const; 62 | 63 | void printCalibrationValues() const; 64 | void saveGnuplotDataFiles() const; 65 | void copyMode3ToMode2(); 66 | 67 | //-----PUBLIC MEMBERS----- 68 | typedef std::map > CalibPerLaser; 69 | 70 | struct CalibIndependent { 71 | std::vector calibrationValuesRange, 72 | calibrationValuesIncidenceAngle, 73 | calibrationValuesIntensity; 74 | }; 75 | typedef std::map CalibIndependentPerLaser; 76 | 77 | struct LaserGroup { 78 | std::vector calibrationValuesRange; 79 | std::vector calibrationValuesIncidenceAngle; 80 | std::vector calibrationValuesIntensity; 81 | std::map calibrationValuesLaser; 82 | float maxUncalibratedIntensity; 83 | }; 84 | struct LaserGroups : public std::vector { 85 | std::map laserIndices; 86 | void updateLaserIndices(); 87 | }; 88 | 89 | CalibPerLaser calibrationValuesMode1; 90 | //calibrationValuesRangePerLaser, calibrationValuesIncidenceAnglePerLaser; 91 | CalibIndependentPerLaser calibrationValuesMode2; 92 | LaserGroups calibrationValuesMode3; 93 | int mode; // Mode 1: Treat influence of lasers, angles, and ranges individually (full graph) 94 | // Mode 2: Treat lasers individually and influence of angles, ranges, and intensities as independent 95 | // Mode 3: Treat lasers in groups and angles and ranges as independent. 96 | int noOfCellsX, noOfCellsY, noOfCellsZ; // x range, y angle, z intensity 97 | float incidenceAngleToCellFactor, cellToIncidenceAngleFactor; 98 | float rangeStepFactor; 99 | 100 | std::map > minMaxRanges; 101 | std::map maxUncalibratedIntensities; 102 | 103 | protected: 104 | void updateDependingValues(); 105 | }; 106 | 107 | #include "remissionCalibrationResult.hpp" 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /src/remissionCalibrationResult.hpp: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | float RemissionCalibrationResult::getCalibrationFactor(float cellX, float cellY, float cellZ, int laser) const { 9 | float normalizationFactor = 1.0f; 10 | if (mode==1) { 11 | float leftX = floorf(cellX), 12 | topY = floorf(cellY), 13 | d1 = cellX-leftX, 14 | d2 = cellY-topY; 15 | 16 | //std::cout << PVARC(cellX)< >::const_iterator it = calibrationValuesMode1.find(laser); 21 | const std::vector& calibrationValues = it->second; 22 | size_t topLeftIdx = lrintf(topY*noOfCellsX+leftX); 23 | topLeft = calibrationValues[topLeftIdx]; 24 | topRight = (leftX+1>=noOfCellsX ? topLeft : calibrationValues[topLeftIdx+1]); 25 | bottomLeft = (topY+1>=noOfCellsY ? topLeft : calibrationValues[topLeftIdx+noOfCellsX]); 26 | bottomRight = (leftX+1>=noOfCellsX ? bottomLeft : 27 | (topY+1>=noOfCellsY ? topRight : calibrationValues[topLeftIdx+noOfCellsX+1])); 28 | normalizationFactor = (1.0f-d1)*(1.0f-d2) * topLeft + 29 | d1*(1.0f-d2) * topRight + 30 | (1.0f-d1)*d2 * bottomLeft + 31 | d1*d2 * bottomRight; 32 | } 33 | else if (mode==2) { // no full graph 34 | const CalibIndependent& c = calibrationValuesMode2.find(laser)->second; 35 | int lowerCellX = cellX, 36 | lowerCellY = cellY, 37 | lowerCellZ = cellZ; 38 | float normalizationFactorX, normalizationFactorY, normalizationFactorZ; 39 | if (lowerCellX >= noOfCellsX-1) 40 | normalizationFactorX = c.calibrationValuesRange[noOfCellsX-1]; 41 | else { 42 | float factor = cellX-lowerCellX; 43 | normalizationFactorX = (1.0f-factor)*c.calibrationValuesRange[lowerCellX] + factor*c.calibrationValuesRange[lowerCellX+1]; 44 | } 45 | if (lowerCellY >= noOfCellsY-1) 46 | normalizationFactorY = c.calibrationValuesIncidenceAngle[noOfCellsY-1]; 47 | else { 48 | float factor = cellY-lowerCellY; 49 | normalizationFactorY = (1.0f-factor)*c.calibrationValuesIncidenceAngle[lowerCellY] + factor*c.calibrationValuesIncidenceAngle[lowerCellY+1]; 50 | } 51 | if (lowerCellZ >= noOfCellsZ-1) 52 | normalizationFactorZ = c.calibrationValuesIntensity[noOfCellsZ-1]; 53 | else { 54 | float factor = cellZ-lowerCellZ; 55 | normalizationFactorZ = (1.0f-factor)*c.calibrationValuesIntensity[lowerCellZ] + factor*c.calibrationValuesIntensity[lowerCellZ+1]; 56 | } 57 | normalizationFactor = normalizationFactorX*normalizationFactorY*normalizationFactorZ; 58 | } 59 | else if (mode==3) { // no full graph with laser groups 60 | int laserGroupIdx = calibrationValuesMode3.laserIndices.find(laser)->second; 61 | const LaserGroup& lg = calibrationValuesMode3[laserGroupIdx]; 62 | int lowerCellX = cellX, 63 | lowerCellY = cellY, 64 | lowerCellZ = cellZ; 65 | float normalizationFactorX, normalizationFactorY, normalizationFactorZ; 66 | if (lowerCellX >= noOfCellsX-1) 67 | normalizationFactorX = lg.calibrationValuesRange[noOfCellsX-1]; 68 | else { 69 | float factor = cellX-lowerCellX; 70 | normalizationFactorX = (1.0f-factor)*lg.calibrationValuesRange[lowerCellX] + factor*lg.calibrationValuesRange[lowerCellX+1]; 71 | } 72 | if (lowerCellY >= noOfCellsY-1) 73 | normalizationFactorY = lg.calibrationValuesIncidenceAngle[noOfCellsY-1]; 74 | else { 75 | float factor = cellY-lowerCellY; 76 | normalizationFactorY = (1.0f-factor)*lg.calibrationValuesIncidenceAngle[lowerCellY] + factor*lg.calibrationValuesIncidenceAngle[lowerCellY+1]; 77 | } 78 | if (lowerCellZ >= noOfCellsZ-1) 79 | normalizationFactorZ = lg.calibrationValuesIntensity[noOfCellsZ-1]; 80 | else { 81 | float factor = cellZ-lowerCellZ; 82 | normalizationFactorZ = (1.0f-factor)*lg.calibrationValuesIntensity[lowerCellZ] + factor*lg.calibrationValuesIntensity[lowerCellZ+1]; 83 | } 84 | normalizationFactor = lg.calibrationValuesLaser.find(laser)->second*normalizationFactorX*normalizationFactorY*normalizationFactorZ; 85 | } 86 | 87 | return normalizationFactor; 88 | } 89 | 90 | float RemissionCalibrationResult::getCalibratedIntensity(float range, float incidenceAngle, float intensity, int laser) const { 91 | float cellX, cellY, cellZ; 92 | rangeAndIncidenceAngleToCell(range, incidenceAngle, cellX, cellY); 93 | intensityToCell(intensity, laser, cellZ); 94 | float normalizationFactor = getCalibrationFactor(cellX, cellY, cellZ, laser); 95 | float normalizedIntensity = normalizationFactor*intensity; 96 | //std::cout << PVARC(cellX)<second]; 134 | normalizedIntensity = intensity/lg.maxUncalibratedIntensity; 135 | } 136 | else 137 | normalizedIntensity = intensity/maxUncalibratedIntensities.find(laser)->second; 138 | normalizedIntensityToCell(normalizedIntensity, cell); 139 | } 140 | void RemissionCalibrationResult::intensityToCell(float intensity, int laser, int& cell) const { 141 | float cellf; 142 | intensityToCell(intensity, laser, cellf); 143 | cell = lrintf(cellf); 144 | } 145 | void RemissionCalibrationResult::cellToNormalizedIntensity(float cell, float& intensity) const { 146 | intensity = cell/std::max(1, noOfCellsZ-1); 147 | } 148 | void RemissionCalibrationResult::cellToIntensity(float cell, int laser, float& intensity) const { 149 | cellToNormalizedIntensity(cell, intensity); 150 | if (mode==3) 151 | intensity *= calibrationValuesMode3[calibrationValuesMode3.laserIndices.find(laser)->second].maxUncalibratedIntensity; 152 | else 153 | intensity *= maxUncalibratedIntensities.find(laser)->second; 154 | } 155 | 156 | void RemissionCalibrationResult::rangeAndIncidenceAngleToCell(float range, float incidenceAngle, float& cellX, float& cellY) const { 157 | rangeToCell(range, cellX); 158 | incidenceAngleToCell(incidenceAngle, cellY); 159 | } 160 | 161 | // the cell id that this range and incident angle falls into 162 | void RemissionCalibrationResult::rangeAndIncidenceAngleToCell(float range, float incidenceAngle, int& cellX, int& cellY) const { 163 | rangeToCell(range, cellX); 164 | incidenceAngleToCell(incidenceAngle, cellY); 165 | } 166 | -------------------------------------------------------------------------------- /src/statisticsGenerator.cpp: -------------------------------------------------------------------------------- 1 | // This software is licenced under the Creative Commons 2 | // (Attribution-NonCommercial-ShareAlike): 3 | // http://creativecommons.org/licenses/by-nc-sa/4.0 4 | 5 | // Author: Bastian Steder 6 | 7 | 8 | #include "remissionCalibrationHelper.h" 9 | 10 | std::string calibrationValuesFileName = ""; 11 | float errorStepSize = 0.1; 12 | std::string& skipLasersString = RemissionCalibrationHelper::skipLasersString; 13 | 14 | void printUsage(const char* progName) { 15 | std::cout << "\n\nUsage: "< Calibration values file\n" 19 | << "-e Error step size (default: "< Folder\n" 21 | << "-s skip lasers. The listed lasers will be ignored. E.g.: \"1,{2,3,5,7},4-8,{4,6,101,103-107}\".\n" 22 | << "-h this help\n" 23 | << "\n\n"; 24 | } 25 | 26 | int main(int argc, char** argv) 27 | { 28 | setlocale(LC_NUMERIC, "C"); 29 | 30 | std::string folder = "."; 31 | 32 | // -------------------------------------- 33 | // -----Parse Command Line Arguments----- 34 | // -------------------------------------- 35 | for (char c; (c = getopt(argc, argv, "c:d:e:s:h")) != -1; ) { 36 | switch (c) { 37 | case 'c': 38 | calibrationValuesFileName = optarg; 39 | std::cout << "Calibration values file \""< errorDistribution = mapCells.getErrorDistribution(calibration, errorStepSize); 108 | for (std::map::const_iterator it=errorDistribution.begin(); it!=errorDistribution.end(); ++it) { 109 | //std::cout << it->first*errorStepSize << ": "<second<<"\n"; 110 | //file << it->first*errorStepSize << " " << it->second << "\n"; 111 | } 112 | //file.close(); 113 | 114 | ss.str(""); 115 | ss << "errorDistribution"; 116 | if (step==0) 117 | ss << "_initialGuess.dat"; 118 | else 119 | ss << "_calibratedMode"< errorPercentages = mapCells.getErrorPercentages(calibration); 122 | for (std::map::const_iterator it=errorPercentages.begin(); it!=errorPercentages.end(); ++it) { 123 | //std::cout << it->first << "%: "<second<<"\n"; 124 | file << it->first << " "<second<<"\n"; 125 | } 126 | file.close(); 127 | 128 | if (step==0) 129 | file.open("medianError_initial.txt"); 130 | else 131 | file.open("medianError.txt"); 132 | std::cout << "Median error: "<::const_iterator it=lg.calibrationValuesLaser.begin(); it!=lg.calibrationValuesLaser.end(); ++it) 143 | //file << (it==lg.calibrationValuesLaser.begin()&&laserGroupIdx==0 ? "" : " & ") << it->second; 144 | //} 145 | //file << "\n"; 146 | //file.close(); 147 | //} 148 | 149 | std::cout << "Average calibrated intensity: "<