├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── apps ├── benchmark.cpp ├── calibrate.cpp ├── create_video.cpp ├── dataset_statistics.cpp ├── label_rawlog.cpp ├── label_scene.cpp ├── mapping.cpp ├── process_rawlog.cpp ├── segmentation.cpp ├── tests.cpp └── visualize_reconstruction.cpp ├── libs ├── core │ ├── CMakeLists.txt │ ├── core.cpp │ └── core.hpp ├── labeling │ ├── CMakeLists.txt │ └── todo.cpp ├── mapping │ ├── CMakeLists.txt │ └── todo.cpp └── processing │ ├── CAnalyzer.cpp │ ├── CAnalyzer.hpp │ ├── CEditor.cpp │ ├── CEditor.hpp │ ├── CMakeLists.txt │ └── processing.hpp ├── license-GPLv3 ├── share └── olt │ └── config_files │ ├── Dataset_statistics.conf │ ├── Label_rawlog.conf │ ├── Label_scene.conf │ └── Process_rawlog.conf ├── third_party └── difodo_multi │ ├── CDifodo_multi.cpp │ ├── CDifodo_multi.h │ ├── Difodo_multi_datasets.cpp │ ├── Difodo_multi_datasets.h │ └── legend.xpm └── version_prefix.txt /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | src/tests.cpp 3 | third_party/CLAMS -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | #---------------------------------------------------------------------------# 3 | # Object Labeling Toolkit # 4 | # A set of software components for the management and # 5 | # labeling of RGB-D datasets # 6 | # # 7 | # Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento # 8 | # University of Malaga # 9 | # MAPIR Group: # 10 | # # 11 | # This program is free software: you can redistribute it and/or modify # 12 | # it under the terms of the GNU General Public License as published by # 13 | # the Free Software Foundation, either version 3 of the License, or # 14 | # (at your option) any later version. # 15 | # # 16 | # This program is distributed in the hope that it will be useful, # 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of # 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # 19 | # GNU General Public License for more details. # 20 | # # 21 | # # 22 | #---------------------------------------------------------------------------# 23 | 24 | # Project name 25 | PROJECT(OLT) 26 | 27 | # Required commands in newer CMake versions: 28 | CMAKE_MINIMUM_REQUIRED(VERSION 2.4) 29 | if(COMMAND cmake_policy) 30 | cmake_policy(SET CMP0003 NEW) 31 | endif(COMMAND cmake_policy) 32 | 33 | # Loads the current version number (e.g "0.5.1") 34 | FILE(READ "${CMAKE_SOURCE_DIR}/version_prefix.txt" VERSION_NUMBER) 35 | 36 | STRING(SUBSTRING "${VERSION_NUMBER}" 0 1 VERSION_NUMBER_MAJOR) 37 | STRING(SUBSTRING "${VERSION_NUMBER}" 2 1 VERSION_NUMBER_MINOR) 38 | STRING(SUBSTRING "${VERSION_NUMBER}" 4 1 VERSION_NUMBER_PATCH) 39 | 40 | #------------------------------------------------------------------------------# 41 | # DEPENDENCIES 42 | #------------------------------------------------------------------------------# 43 | 44 | # -------------------------------------------- 45 | # MRPT library: 46 | # -------------------------------------------- 47 | 48 | FIND_PACKAGE( MRPT REQUIRED slam;gui;hwdrivers;gui;vision;topography) 49 | 50 | # -------------------------------------------- 51 | # PCL library: 52 | # -------------------------------------------- 53 | 54 | find_package(PCL 1.7 REQUIRED) 55 | IF (PCL_FOUND) 56 | INCLUDE_DIRECTORIES(${PCL_INCLUDE_DIRS}) 57 | link_directories(${PCL_LIBRARY_DIRS}) 58 | add_definitions(${PCL_DEFINITIONS}) 59 | ENDIF(PCL_FOUND) 60 | 61 | #message(PCL_LIBS: ${PCL_LIBRARIES}) 62 | 63 | # -------------------------------------------- 64 | # OpenCV 65 | # -------------------------------------------- 66 | 67 | set(OLT_USING_OPENCV "FALSE" CACHE BOOL 68 | "Check if you want to use OpenCV at different parts of OLT (no mandatory).") 69 | 70 | IF (OLT_USING_OPENCV) 71 | FIND_PACKAGE( OpenCV REQUIRED ) 72 | add_definitions(-DUSING_OPENCV) 73 | ENDIF (OLT_USING_OPENCV) 74 | 75 | # -------------------------------------------- 76 | # Third party 77 | # -------------------------------------------- 78 | 79 | # Difodo multi sensor 80 | SET( DIFODO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/third_party/difodo_multi/ ) 81 | 82 | FILE(GLOB difodo_sources ${DIFODO_DIR}*.cpp) 83 | FILE(GLOB difodo_headers ${DIFODO_DIR}*.h) 84 | 85 | INCLUDE_DIRECTORIES(${DIFODO_DIR}) 86 | 87 | ADD_LIBRARY(DIFODO ${difodo_sources} ${difodo_headers} ) 88 | 89 | # Tell CMake that the linker language is C++ 90 | SET_TARGET_PROPERTIES(DIFODO PROPERTIES LINKER_LANGUAGE CXX) 91 | 92 | #------------------------------------------------------------------------------# 93 | # TARGET 94 | #------------------------------------------------------------------------------# 95 | 96 | # Create UPGM++ libraries and add include directories to the examples 97 | SET( OLT_LIBRARIES "core;processing;mapping;labeling" ) 98 | 99 | SET(INC_DIR "") 100 | SET(LIBRARIES "") 101 | 102 | FOREACH( LIBRARY ${OLT_LIBRARIES} ) 103 | ADD_SUBDIRECTORY(libs/${LIBRARY}) 104 | INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/libs/${LIBRARY}) 105 | SET(INC_DIR ${INC_DIR}${CMAKE_SOURCE_DIR}/libs/${LIBRARY}\;) 106 | SET(LIBRARIES ${LIBRARIES}${PROJECT_BINARY_DIR}/libs/libOLT-${LIBRARY}.so\;) 107 | set_target_properties(${LIBRARY} PROPERTIES PREFIX "libOLT-") 108 | ENDFOREACH( LIBRARY ${OLT_LIBRARIES} ) 109 | 110 | # -------------------------------------------- 111 | # Compilation flags 112 | # -------------------------------------------- 113 | 114 | set(OLT_USING_OMPENMP "FALSE" CACHE BOOL 115 | "Check if you want to parallelize some parts of the code using OpenMP.") 116 | 117 | IF (OLT_USING_OMPENMP) 118 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 119 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp -O3 -mtune=native -march=native ") 120 | ENDIF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 121 | ELSE (OLT_USING_OMPENMP) 122 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 123 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -mtune=native -march=native ") 124 | ENDIF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 125 | ENDIF (OLT_USING_OMPENMP) 126 | 127 | 128 | # The debug post-fix of .dll /.so libs 129 | # ------------------------------------------ 130 | set(CMAKE_DEBUG_POSTFIX "-dbg") 131 | 132 | #------------------------------------------------------------------------------# 133 | # Enable GCC profiling (GCC only) 134 | #------------------------------------------------------------------------------# 135 | 136 | #IF(CMAKE_COMPILER_IS_GNUCXX) 137 | # SET(ENABLE_PROFILING OFF CACHE BOOL "Enable profiling in the GCC compiler (Add flags: -g -pg)") 138 | #ENDIF(CMAKE_COMPILER_IS_GNUCXX) 139 | 140 | #IF(ENABLE_PROFILING) 141 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -pg") 142 | #ENDIF(ENABLE_PROFILING) 143 | 144 | #IF(UNIX) 145 | # LINK_DIRECTORIES("${CMAKE_CURRENT_SOURCE_DIR}") 146 | #ENDIF(UNIX) 147 | 148 | #------------------------------------------------------------------------------# 149 | # Using CLAMS intrinsic calibration? 150 | #------------------------------------------------------------------------------# 151 | 152 | set(OLT_USING_CLAMS_INTRINSIC_CALIBRATION "FALSE" CACHE BOOL 153 | "Check if an intrinsic calibration by CLAMS of the RGB-D sensors within the dataset is available.") 154 | 155 | IF (OLT_USING_CLAMS_INTRINSIC_CALIBRATION) 156 | MESSAGE("Using CLAMS intrinsitc calibration of RGB-D sensors") 157 | add_definitions(-DUSING_CLAMS_INTRINSIC_CALIBRATION) 158 | 159 | #INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/third_party/CLAMS/include) 160 | 161 | set(CLAMS_INCLUDE_DIR "" CACHE PATH 162 | "Path to the CLAMS include directory") 163 | 164 | INCLUDE_DIRECTORIES(${CLAMS_INCLUDE_DIR}) 165 | 166 | set(CLAMS_DISCRETE_DEPTH_DISTORTION_MODEL_HEADER "" CACHE FILEPATH 167 | "File called discrete_depth_distortion_model.h") 168 | 169 | set(CLAMS_DISCRETE_DEPTH_DISTORTION_MODEL_SOURCE "" CACHE FILEPATH 170 | "File called discrete_depth_distortion_model.cpp") 171 | 172 | ADD_LIBRARY(Undistort 173 | ${CLAMS_DISCRETE_DEPTH_DISTORTION_MODEL_HEADER} 174 | ${CLAMS_DISCRETE_DEPTH_DISTORTION_MODEL_SOURCE} 175 | ) 176 | 177 | TARGET_LINK_LIBRARIES(Undistort libboost_system.so libboost_thread.so) 178 | 179 | # Tell CMake that the linker language is C++ 180 | SET_TARGET_PROPERTIES(Undistort PROPERTIES LINKER_LANGUAGE CXX) 181 | 182 | ELSE (OLT_USING_CLAMS_INTRINSIC_CALIBRATION) 183 | MESSAGE("Non using CLAMS intrisic calibration of RGB-D sensors") 184 | ENDIF(OLT_USING_CLAMS_INTRINSIC_CALIBRATION) 185 | 186 | 187 | #------------------------------------------------------------------------------# 188 | # Toolkit-executables 189 | #------------------------------------------------------------------------------# 190 | 191 | ADD_EXECUTABLE(Process_rawlog ${CMAKE_SOURCE_DIR}/apps/process_rawlog.cpp) 192 | 193 | IF (OLT_USING_CLAMS_INTRINSIC_CALIBRATION) 194 | TARGET_LINK_LIBRARIES(Process_rawlog ${PCL_LIBRARIES} ${MRPT_LIBS} Undistort core processing) 195 | ELSE (OLT_USING_CLAMS_INTRINSIC_CALIBRATION) 196 | TARGET_LINK_LIBRARIES(Process_rawlog ${PCL_LIBRARIES} ${MRPT_LIBS} core processing) 197 | ENDIF(OLT_USING_CLAMS_INTRINSIC_CALIBRATION) 198 | 199 | ADD_EXECUTABLE(Mapping ${CMAKE_SOURCE_DIR}/apps/mapping.cpp) 200 | TARGET_LINK_LIBRARIES(Mapping ${PCL_LIBRARIES} ${MRPT_LIBS} DIFODO ) 201 | 202 | ADD_EXECUTABLE(Visualize_reconstruction ${CMAKE_SOURCE_DIR}/apps/visualize_reconstruction.cpp) 203 | TARGET_LINK_LIBRARIES(Visualize_reconstruction ${PCL_LIBRARIES} ${MRPT_LIBS} processing) 204 | 205 | ADD_EXECUTABLE(Label_scene ${CMAKE_SOURCE_DIR}/apps/label_scene.cpp) 206 | TARGET_LINK_LIBRARIES(Label_scene ${PCL_LIBRARIES} ${MRPT_LIBS} ) 207 | 208 | ADD_EXECUTABLE(Label_rawlog ${CMAKE_SOURCE_DIR}/apps/label_rawlog.cpp) 209 | TARGET_LINK_LIBRARIES(Label_rawlog ${PCL_LIBRARIES} ${MRPT_LIBS} ) 210 | 211 | ADD_EXECUTABLE(Segmentation ${CMAKE_SOURCE_DIR}/apps/segmentation.cpp) 212 | TARGET_LINK_LIBRARIES(Segmentation ${PCL_LIBRARIES} ${MRPT_LIBS} ) 213 | 214 | ADD_EXECUTABLE(Create_video ${CMAKE_SOURCE_DIR}/apps/create_video.cpp) 215 | TARGET_LINK_LIBRARIES(Create_video ${PCL_LIBRARIES} ${MRPT_LIBS} ) 216 | 217 | ADD_EXECUTABLE(Dataset_statistics ${CMAKE_SOURCE_DIR}/apps/dataset_statistics.cpp) 218 | TARGET_LINK_LIBRARIES(Dataset_statistics ${PCL_LIBRARIES} ${MRPT_LIBS} processing) 219 | 220 | ADD_EXECUTABLE(Benchmark ${CMAKE_SOURCE_DIR}/apps/benchmark.cpp) 221 | TARGET_LINK_LIBRARIES(Benchmark ${PCL_LIBRARIES} ${MRPT_LIBS} ) 222 | 223 | ADD_EXECUTABLE(Calibrate ${CMAKE_SOURCE_DIR}/apps/calibrate.cpp) 224 | TARGET_LINK_LIBRARIES(Calibrate ${PCL_LIBRARIES} ${MRPT_LIBS} ) 225 | 226 | 227 | #------------------------------------------------------------------------------# 228 | # Status messages 229 | #------------------------------------------------------------------------------# 230 | 231 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 232 | MESSAGE(STATUS "Compiler flags: " ${CMAKE_CXX_FLAGS_RELEASE}) 233 | ENDIF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 234 | 235 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | +--------------------------------------+---------------------------------------+ 2 | | LICENSE | 3 | +------------------------------------------------------------------------------+ 4 | 5 | * The Object Labeling Toolkit (OLT) is released under a GPLv3 license. 6 | Read license-GPLv3, or if not present, . 7 | 8 | * For a closed-source version of OLT 9 | for commercial purposes, please contact the authors. 10 | 11 | * If you use OLT in an academic work, 12 | please cite the most relevant publication associated by visiting: 13 | , or if any, please cite the 14 | Machine Perception and Intelligent Robotics (MAPIR) 15 | research group direclty. 16 | 17 | 18 | 19 | --- END OF FILE --- -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Object-Labeling-Toolkit 2 | 3 | Toolkit for the labeling of objects within sequences of RGB-D observations. 4 | 5 | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento 6 | 7 | University of Malaga (jotaraul@uma.es) 8 | 9 | http://mapir.isa.uma.es/jotaraul 10 | 11 | If you use this software, please read the LICENSE file. If the utilization is for academic purposes, please cite the work: 12 | 13 | > @INPROCEEDINGS{Ruiz-Sarmiento-ECMR-2015, 14 | 15 | > author = {Ruiz-Sarmiento, J. R. and Galindo, Cipriano and Gonz{\'{a}}lez-Jim{\'{e}}nez, Javier}, 16 | 17 | > title = {OLT: A Toolkit for Object Labeling Applied to Robotic RGB-D Datasets}, 18 | 19 | > booktitle = {European Conference on Mobile Robots}, 20 | 21 | > year = {2015}, 22 | 23 | > location = {Lincoln, UK} 24 | 25 | > } 26 | 27 | 28 | Toolkit structure 29 | -------- 30 | This project is structured as follows: 31 | * _This directory_: Rules to build the toolkit using CMake, and the license file (GNU General Public License v3). 32 | * _src_: Source files to be compiled, which build the following toolkit applications (listed in the same order as they should be run): 33 | * *Process_rawlog*: Sets the extrinsic and intrinsic parameters of the sensors used within the dataset. 34 | * *Mapping*: Localizes the poses/locations from which the observations within the datset were taken in order to create a map of the scene. 35 | * *Visualize_reconstruction*: Visually shows a 3D reconstruction of the collected data, and stores it as a _scene_. 36 | * *Label_scene*: Permits us to effortlessly label a reconstructed scene. Example of scene being labeled: 37 | 38 | ![Scene being labeled with the OLT tool](http://mapir.isa.uma.es/jotaraul/Resources/example_scene.png "Scene being labeled with the OLT tool") 39 | 40 | * *Label_rawlog*: Propagates the annotated labels in a scene to each observation within the dataset. 41 | * *Dataset_statistics*: Shows information of the dataset, e.g. a summary of the objects appearing on it, number of times that they appear, number of pixels they occupy, etc. 42 | * *Benchmark*: Compare two labeled rawlogs and yields some performance results. 43 | * *Create_video & Segmentation*: Experimental applications under development. 44 | * _Examples of configuration files_: A directoy containing some examples of configuration files to be used with the different toolkit applications. 45 | 46 | Prerequisites 47 | -------- 48 | 49 | Mandatory dependencies: 50 | 51 | * MRPT (www.mrpt.org) 52 | * PCL (http://www.pointclouds.org/) 53 | 54 | Compiling 55 | -------- 56 | 57 | mkdir build && cd build && cmake .. && make 58 | -------------------------------------------------------------------------------- /apps/benchmark.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | using namespace mrpt; 45 | using namespace mrpt::utils; 46 | using namespace mrpt::math; 47 | using namespace mrpt::poses; 48 | using namespace mrpt::obs; 49 | 50 | using namespace std; 51 | 52 | CFileGZInputStream gtRawlog; // Rawlog with ground truth information 53 | CFileGZInputStream labeledRawlog; // Labeled rawlog to be compared 54 | 55 | string gtRawlogFilename; 56 | string labeledRawlogFilename; 57 | 58 | //----------------------------------------------------------- 59 | // 60 | // loadConfig 61 | // 62 | //----------------------------------------------------------- 63 | 64 | void loadConfig() 65 | { 66 | 67 | } 68 | 69 | 70 | //----------------------------------------------------------- 71 | // 72 | // showUsageInformation 73 | // 74 | //----------------------------------------------------------- 75 | 76 | void showUsageInformation() 77 | { 78 | cout << "Usage information. At least two expected arguments: " << endl << 79 | " (1) Rawlog file with ground truth information." << endl << 80 | " (2) Rawlog file with annotated labels." << endl; 81 | cout << "Then, optional parameters:" << endl << 82 | " -h : Shows this help." << endl; 83 | 84 | } 85 | 86 | 87 | //----------------------------------------------------------- 88 | // 89 | // loadParameters 90 | // 91 | //----------------------------------------------------------- 92 | 93 | int loadParameters(int argc, char* argv[]) 94 | { 95 | if ( argc >= 3 ) 96 | { 97 | gtRawlogFilename = argv[1]; 98 | labeledRawlogFilename = argv[2]; 99 | 100 | for ( size_t arg = 3; arg < argc; arg++ ) 101 | { 102 | if ( !strcmp(argv[arg],"-h") ) 103 | { 104 | showUsageInformation(); 105 | return -1; 106 | } 107 | else 108 | { 109 | cout << " [ERROR] Unknown option " << argv[arg] << endl; 110 | showUsageInformation(); 111 | return -1; 112 | } 113 | } 114 | 115 | } 116 | else 117 | { 118 | showUsageInformation(); 119 | 120 | return -1; 121 | } 122 | 123 | return 0; 124 | } 125 | 126 | 127 | //----------------------------------------------------------- 128 | // 129 | // benchmark 130 | // 131 | //----------------------------------------------------------- 132 | 133 | void benchmark() 134 | { 135 | // 136 | // Check rawlogs 137 | // 138 | 139 | if (!mrpt::system::fileExists(gtRawlogFilename)) 140 | { 141 | cerr << " [ERROR] A rawlog file with name " << gtRawlogFilename; 142 | cerr << " doesn't exist." << endl; 143 | return; 144 | } 145 | 146 | gtRawlog.open(gtRawlogFilename); 147 | 148 | if (!mrpt::system::fileExists(labeledRawlogFilename)) 149 | { 150 | cerr << " [ERROR] A rawlog file with name " << labeledRawlogFilename; 151 | cerr << " doesn't exist." << endl; 152 | return; 153 | } 154 | 155 | labeledRawlog.open(labeledRawlogFilename); 156 | 157 | cout << " [INFO] Working with ground truth rawlog " << gtRawlogFilename << endl; 158 | cout << " and labeled rawlog " << labeledRawlogFilename << endl; 159 | 160 | // TODO: Add tests to ensure that they are the same rawlog sequence 161 | 162 | vector v_success; 163 | 164 | // Get pairs of observations and compare their labels 165 | 166 | CActionCollectionPtr action; 167 | CSensoryFramePtr observations; 168 | CObservationPtr gtObs,labeledObs; 169 | size_t gtObsIndex = 0, labeledObsIndex = 0; 170 | 171 | cout << " Process: "; 172 | cout.flush(); 173 | 174 | while ( ( CRawlog::getActionObservationPairOrObservation(gtRawlog,action,observations,gtObs,gtObsIndex) )&& 175 | ( CRawlog::getActionObservationPairOrObservation(labeledRawlog,action,observations,labeledObs,labeledObsIndex) ) ) 176 | { 177 | // TODO: Check that the obss are 3D scans 178 | CObservation3DRangeScanPtr gt3DObs = CObservation3DRangeScanPtr(gtObs); 179 | CObservation3DRangeScanPtr labeled3DObs = CObservation3DRangeScanPtr(labeledObs); 180 | 181 | if ( !(gtObsIndex % 200) ) 182 | { 183 | if ( !(gtObsIndex % 1000) ) cout << "+ "; else cout << ". "; 184 | cout.flush(); 185 | } 186 | 187 | // Check that both observations have labels 188 | if ( !gt3DObs->hasPixelLabels() || !labeled3DObs->hasPixelLabels() ) 189 | continue; 190 | 191 | std::map::iterator labelsIt; 192 | size_t labelsAppearing = 0; 193 | 194 | for ( labelsIt = gt3DObs->pixelLabels->pixelLabelNames.begin(); 195 | labelsIt != gt3DObs->pixelLabels->pixelLabelNames.end(); 196 | labelsIt++ ) 197 | { 198 | // Get label name from the ground truth obs 199 | string label = labelsIt->second; 200 | 201 | // Check if it appears in the labeled obs 202 | if ( labeled3DObs->pixelLabels->checkLabelNameExistence(label) >= 0 ) 203 | labelsAppearing++; 204 | } 205 | 206 | size_t N_labelsGt = gt3DObs->pixelLabels->pixelLabelNames.size(); 207 | size_t N_labelsLabeled = labeled3DObs->pixelLabels->pixelLabelNames.size(); 208 | 209 | size_t maxNumOfLabels = ( (N_labelsGt > N_labelsLabeled) ? N_labelsGt : N_labelsLabeled ); 210 | 211 | if (!maxNumOfLabels) // Are there labels? 212 | continue; 213 | 214 | double success = labelsAppearing / (double)maxNumOfLabels; 215 | v_success.push_back(success); 216 | } 217 | 218 | double sumSuccess = std::accumulate(v_success.begin(), v_success.end(), 0.0); 219 | double meanSuccess = sumSuccess / (double)v_success.size(); 220 | 221 | cout << endl; 222 | cout << " [INFO] Results: " << endl; 223 | cout << " - Mean success: " << meanSuccess*100 << "%"; 224 | cout << endl << endl; 225 | } 226 | 227 | //----------------------------------------------------------- 228 | // 229 | // main 230 | // 231 | //----------------------------------------------------------- 232 | 233 | int main(int argc, char* argv[]) 234 | { 235 | try 236 | { 237 | 238 | cout << endl << "-----------------------------------------------------" << endl; 239 | cout << " Benchmark app. " << endl; 240 | cout << " [Object Labeling Tookit] " << endl; 241 | cout << "-----------------------------------------------------" << endl << endl; 242 | 243 | // 244 | // Load paramteres 245 | 246 | int res = loadParameters(argc,argv); 247 | 248 | if ( res < 0 ) 249 | return -1; 250 | 251 | // 252 | // Compare datasets 253 | 254 | benchmark(); 255 | 256 | 257 | } 258 | catch (exception &e) 259 | { 260 | cout << "Exception caught: " << e.what() << endl; 261 | return -1; 262 | } 263 | catch (...) 264 | { 265 | printf("Another exception!!"); 266 | return -1; 267 | } 268 | } 269 | 270 | 271 | 272 | -------------------------------------------------------------------------------- /apps/calibrate.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "opencv2/imgproc/imgproc.hpp" 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #ifdef USING_CLAMS_INTRINSIC_CALIBRATION 49 | #include 50 | #endif 51 | 52 | #define NUM_ASUS_SENSORS 4 53 | 54 | using namespace mrpt; 55 | using namespace mrpt::utils; 56 | using namespace mrpt::math; 57 | using namespace mrpt::poses; 58 | using namespace mrpt::obs; 59 | 60 | using namespace std; 61 | 62 | // TODOS: 63 | // Enable the use of more than a laser scanner 64 | 65 | // Visualization purposes 66 | //mrpt::gui::CDisplayWindow3D win3D; 67 | 68 | // 69 | // Useful variables 70 | // 71 | 72 | bool fromHomogeneusMatrixToPose; 73 | string homogeneusMatrixFile; 74 | 75 | string configFileName; 76 | bool computeScaleBetweenRGBDSensors = false; 77 | 78 | struct TScaleCalibrationConfig{ 79 | 80 | float resolution; 81 | float lowerRange; 82 | float higerRange; 83 | string referenceSensor; 84 | string scaledSensor; 85 | string o_scaleCalibrationFile; 86 | bool generateInverseScale; 87 | bool computeTransitiveScale; 88 | string referenceScaleCalibrationFile; 89 | string scaledScaleCalibrationFile; 90 | vector v_referenceRawlogs; 91 | vector v_scaledRawlogs; 92 | 93 | TScaleCalibrationConfig() 94 | {} 95 | } scaleCalibrationConfig; 96 | 97 | struct TScaleCalibration{ 98 | 99 | float resolution; 100 | float lowerRange; 101 | float higerRange; 102 | string referenceSensor; 103 | string scaledSensor; 104 | string i_scaleCalibrationFile; 105 | CVectorFloat scaleMultipliers; 106 | 107 | TScaleCalibration() 108 | {} 109 | }; 110 | 111 | vector v_scaleCalibrations; 112 | 113 | //----------------------------------------------------------- 114 | // 115 | // loadScaleCalibrationFromFile 116 | // 117 | //----------------------------------------------------------- 118 | 119 | void loadScaleCalibrationFromFile(const string fileName ) 120 | { 121 | TScaleCalibration sc; 122 | 123 | sc.i_scaleCalibrationFile = fileName; 124 | 125 | ifstream f( sc.i_scaleCalibrationFile.c_str() ); 126 | 127 | if ( !f.is_open() ) 128 | { 129 | cerr << endl << " [ERROR] Opening file " << sc.i_scaleCalibrationFile << endl; 130 | return; 131 | } 132 | 133 | string bin; 134 | f >> bin >> sc.referenceSensor; 135 | f >> bin >> sc.scaledSensor; 136 | f >> bin >> sc.resolution; 137 | f >> bin >> sc.lowerRange; 138 | f >> bin >> sc.higerRange; 139 | 140 | int N_scales = 1+1/sc.resolution*(sc.higerRange-sc.lowerRange); 141 | 142 | sc.scaleMultipliers.resize(N_scales); 143 | 144 | for ( int i = 0; i < N_scales; i++ ) 145 | { 146 | f >> sc.scaleMultipliers(i); 147 | //cout << "Scale:" << sc.scaleMultipliers(i) << endl; 148 | } 149 | 150 | v_scaleCalibrations.push_back(sc); 151 | } 152 | 153 | 154 | //----------------------------------------------------------- 155 | // 156 | // loadConfig 157 | // 158 | //----------------------------------------------------------- 159 | 160 | void loadConfig() 161 | { 162 | CConfigFile config( configFileName ); 163 | 164 | cout << " [INFO] Loading component options from " << configFileName << endl; 165 | 166 | // 167 | // Load execution mode 168 | 169 | 170 | computeScaleBetweenRGBDSensors = config.read_bool("GENERAL","compute_scale_between_RGBD_sensors",false,false); 171 | 172 | // 173 | // Load parameters for computing the scale between two RGBD sensors 174 | 175 | if ( computeScaleBetweenRGBDSensors ) 176 | { 177 | TScaleCalibrationConfig &scc = scaleCalibrationConfig; 178 | 179 | scc.resolution = config.read_double("SCALE_BETWEEN_RGBD_SENSORS","resolution",0,true); 180 | scc.lowerRange = config.read_double("SCALE_BETWEEN_RGBD_SENSORS","lower_range",0,true); 181 | scc.higerRange = config.read_double("SCALE_BETWEEN_RGBD_SENSORS","higher_range",0,true); 182 | scc.referenceSensor = config.read_string("SCALE_BETWEEN_RGBD_SENSORS","reference_sensor","",true); 183 | scc.scaledSensor = config.read_string("SCALE_BETWEEN_RGBD_SENSORS","scaled_sensor","",true); 184 | scc.o_scaleCalibrationFile = config.read_string("SCALE_BETWEEN_RGBD_SENSORS","scale_calibration_file","",true); 185 | scc.generateInverseScale = config.read_bool("SCALE_BETWEEN_RGBD_SENSORS","generate_inverse_scale",false,true); 186 | scc.computeTransitiveScale = config.read_bool("SCALE_BETWEEN_RGBD_SENSORS","compute_transitive_scale",false,true); 187 | 188 | if ( scc.computeTransitiveScale ) 189 | { 190 | scc.referenceScaleCalibrationFile = config.read_string("SCALE_BETWEEN_RGBD_SENSORS","reference_scale_calibration_file","",true);; 191 | scc.scaledScaleCalibrationFile = config.read_string("SCALE_BETWEEN_RGBD_SENSORS","scaled_scale_calibration_file","",true);; 192 | } 193 | 194 | if ( scc.generateInverseScale ) 195 | { 196 | string aux = scc.referenceSensor; 197 | scc.referenceSensor = scc.scaledSensor; 198 | scc.scaledSensor = aux; 199 | } 200 | 201 | vector keys; 202 | config.getAllKeys("SCALE_BETWEEN_RGBD_SENSORS",keys); 203 | 204 | int rawlogIndex = 1; 205 | bool keepLoading = true; 206 | 207 | while ( keepLoading ) 208 | { 209 | string referenceRawlog = mrpt::format("reference_rawlog_%i",rawlogIndex); 210 | string scaledRawlog = mrpt::format("scaled_rawlog_%i",rawlogIndex); 211 | 212 | if ( (std::find(keys.begin(), keys.end(), referenceRawlog) != keys.end()) 213 | && (std::find(keys.begin(), keys.end(), scaledRawlog) != keys.end() ) ) 214 | { 215 | scc.v_referenceRawlogs.push_back( 216 | config.read_string("SCALE_BETWEEN_RGBD_SENSORS",referenceRawlog,"",true) ); 217 | scc.v_scaledRawlogs.push_back( 218 | config.read_string("SCALE_BETWEEN_RGBD_SENSORS",scaledRawlog,"",true) ); 219 | 220 | rawlogIndex++; 221 | } 222 | else 223 | keepLoading = false; 224 | } 225 | 226 | if ( scc.generateInverseScale ) 227 | { 228 | vector aux = scc.v_referenceRawlogs; 229 | scc.v_referenceRawlogs = scc.v_scaledRawlogs; 230 | scc.v_scaledRawlogs = aux; 231 | } 232 | } 233 | 234 | } 235 | 236 | 237 | //----------------------------------------------------------- 238 | // 239 | // getSensorPosInScalecalib 240 | // 241 | //----------------------------------------------------------- 242 | 243 | int getSensorPosInScalecalib( const string label) 244 | { 245 | for ( size_t i = 0; i < v_scaleCalibrations.size(); i++ ) 246 | { 247 | if ( v_scaleCalibrations[i].scaledSensor == label ) 248 | return i; 249 | } 250 | 251 | return -1; 252 | } 253 | 254 | 255 | //----------------------------------------------------------- 256 | // 257 | // showUsageInformation 258 | // 259 | //----------------------------------------------------------- 260 | 261 | void showUsageInformation() 262 | { 263 | cout << "Usage information. At least one expected arguments: " << endl << 264 | " (1) Configuration file." << endl; 265 | cout << "Then, optional parameters:" << endl << 266 | " -h : Shows this help." << endl << 267 | " -fromHomogeneusMatrixToPose: Show 3D pose from a homogeneus matrix within a file." << endl; 268 | 269 | } 270 | 271 | 272 | //----------------------------------------------------------- 273 | // 274 | // computeMeanDepthFromRawlog 275 | // 276 | //----------------------------------------------------------- 277 | 278 | float computeMeanDepthFromRawlog(const string rawlogFile, const string sensorLabel) 279 | { 280 | TScaleCalibrationConfig scc = scaleCalibrationConfig; 281 | vector v_meanDepths; 282 | 283 | CFileGZInputStream rawlogStream(rawlogFile); 284 | 285 | CActionCollectionPtr action; 286 | CSensoryFramePtr observations; 287 | CObservationPtr obs; 288 | size_t obsIndex = 0; 289 | 290 | // 291 | // Get mean depth value for each RGBD observation 292 | 293 | while ( CRawlog::getActionObservationPairOrObservation(rawlogStream,action,observations,obs,obsIndex) ) 294 | { 295 | // Check if it is an observation 296 | if ( !obs ) 297 | 298 | continue; 299 | // RGBD observation? and from the sensor which data are we processing? 300 | 301 | //if ( obs->sensorLabel == sensorLabel ) 302 | { 303 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 304 | obs3D->load(); 305 | 306 | int N_elements = 0; 307 | double sum = 0.0; 308 | 309 | for ( size_t row = 0; row < obs3D->rangeImage.rows(); row++ ) 310 | for ( size_t col = 0; col < obs3D->rangeImage.cols(); col++ ) 311 | { 312 | float value = obs3D->rangeImage(row,col); 313 | if ( value ) 314 | { 315 | sum += value; 316 | N_elements++; 317 | } 318 | } 319 | 320 | float meanDistance = sum / (float)N_elements; 321 | v_meanDepths.push_back(meanDistance); 322 | } 323 | } 324 | 325 | // Compute mean of the mean distances :) 326 | 327 | float sum = std::accumulate(v_meanDepths.begin(), v_meanDepths.end(), 0.0); 328 | float mean = sum / (float)v_meanDepths.size(); 329 | 330 | return mean; 331 | 332 | } 333 | 334 | //----------------------------------------------------------- 335 | // 336 | // saveScaleCalibrationToFile 337 | // 338 | //----------------------------------------------------------- 339 | 340 | void saveScaleCalibrationToFile(CVectorFloat &v_meanDepths, 341 | CVectorFloat &v_depthScales) 342 | { 343 | TScaleCalibrationConfig &scc = scaleCalibrationConfig; 344 | 345 | ofstream f( scc.o_scaleCalibrationFile.c_str() ); 346 | 347 | if ( !f.is_open() ) 348 | cout << " [ERROR] Opening file " << scc.o_scaleCalibrationFile << endl; 349 | 350 | cout << " [INFO] Saving depth scale calibration results to " 351 | << scc.o_scaleCalibrationFile << endl; 352 | 353 | f << "Reference_sensor " << scc.referenceSensor << endl; 354 | f << "Scaled_sensor " << scc.scaledSensor << endl; 355 | f << "Resolution " << scc.resolution << endl; 356 | f << "Lower_depth " << scc.lowerRange << endl; 357 | f << "Higher_depth " << scc.higerRange << endl; 358 | 359 | CVectorFloat xs,ys; 360 | linspace(scc.lowerRange,scc.higerRange, 361 | 1+1/scc.resolution*(scc.higerRange-scc.lowerRange), 362 | xs); 363 | 364 | mrpt::math::leastSquareLinearFit(xs,ys,v_meanDepths,v_depthScales); 365 | 366 | for ( int i = 0; i < ys.rows(); i++ ) 367 | { 368 | //cout << "Depth: " << xs(i) << " scale:" << ys(i) << endl; 369 | f << ys(i) << " "; 370 | } 371 | 372 | cout << " [INFO] Done! " << endl << endl; 373 | } 374 | 375 | 376 | //----------------------------------------------------------- 377 | // 378 | // computeScale 379 | // 380 | //----------------------------------------------------------- 381 | 382 | void computeScale() 383 | { 384 | 385 | // 386 | // Show configuration parameters 387 | 388 | TScaleCalibrationConfig &scc = scaleCalibrationConfig; 389 | size_t N_rawlogs = scc.v_referenceRawlogs.size(); 390 | const size_t N_sensors = 2; 391 | 392 | cout << " [INFO] Computing depth scale between RGBD sensors with the " 393 | "following configuration:" << endl; 394 | 395 | cout << " Reference sensor: " << scc.referenceSensor << endl; 396 | cout << " Scaled sensor : " << scc.scaledSensor << endl; 397 | cout << " Resolution : " << scc.resolution << endl; 398 | cout << " Lower range : " << scc.lowerRange << endl; 399 | cout << " Higer range : " << scc.higerRange << endl; 400 | cout << " Sacale calib. file: " << scc.o_scaleCalibrationFile << endl; 401 | cout << " # of sensors : " << N_sensors << endl; 402 | cout << " # of rawlogs : " << N_rawlogs << endl; 403 | 404 | for ( size_t i_rawlog = 0; i_rawlog < N_rawlogs; i_rawlog++ ) 405 | { 406 | if (!mrpt::system::fileExists(scc.v_referenceRawlogs[i_rawlog])) 407 | { 408 | cerr << " [ERROR] Rawlog file " << scc.v_referenceRawlogs[i_rawlog] 409 | << " doesn't exist." << endl; 410 | return; 411 | } 412 | 413 | if (!mrpt::system::fileExists(scc.v_scaledRawlogs[i_rawlog])) 414 | { 415 | cerr << " [ERROR] Rawlog file " << scc.v_scaledRawlogs[i_rawlog] 416 | << " doesn't exist." << endl; 417 | return; 418 | } 419 | 420 | cout << " Rawlog of reference sensor " << i_rawlog <<" name : " << 421 | scc.v_referenceRawlogs[i_rawlog] << endl; 422 | cout << " Rawlog of scaled sensor " << i_rawlog <<" name : " << 423 | scc.v_scaledRawlogs[i_rawlog] << endl; 424 | } 425 | 426 | cout << " [INFO] Calibrating scales " << endl; 427 | 428 | // 429 | // Data to be filled 430 | 431 | CVectorFloat v_depthScales(N_rawlogs); 432 | CVectorFloat v_meanDepths(N_rawlogs); 433 | 434 | // 435 | // Process rawlogs preparing data to calibrate the scale 436 | 437 | for ( size_t i_rawlog = 0; i_rawlog < N_rawlogs; i_rawlog++ ) 438 | { 439 | 440 | double refSensorMeanDepth = computeMeanDepthFromRawlog(scc.v_referenceRawlogs[i_rawlog], 441 | scc.referenceSensor ); 442 | double scaledSensorMeanDepth = computeMeanDepthFromRawlog(scc.v_scaledRawlogs[i_rawlog], 443 | scc.scaledSensor ); 444 | v_meanDepths(i_rawlog) = scaledSensorMeanDepth; 445 | v_depthScales(i_rawlog) = refSensorMeanDepth/scaledSensorMeanDepth; 446 | 447 | cout << " refSensorMeanDepth: " << refSensorMeanDepth << 448 | " scaledSensorMeanDepth: " << scaledSensorMeanDepth << 449 | " scale: " << v_depthScales[i_rawlog] << endl; 450 | } 451 | 452 | // 453 | // Save depth scale calibration to file 454 | 455 | saveScaleCalibrationToFile(v_meanDepths,v_depthScales); 456 | } 457 | 458 | 459 | //----------------------------------------------------------- 460 | // 461 | // main 462 | // 463 | //----------------------------------------------------------- 464 | 465 | void computeTransitiveScale() 466 | { 467 | 468 | // 469 | // Show configuration parameters 470 | 471 | TScaleCalibrationConfig &scc = scaleCalibrationConfig; 472 | 473 | cout << " [INFO] Computing transitive depth scale between RGBD sensors with the " 474 | "following configuration:" << endl; 475 | 476 | cout << " Reference scale calibration file: " << scc.referenceScaleCalibrationFile << endl; 477 | cout << " Scaled scale calibration file : " << scc.scaledScaleCalibrationFile << endl; 478 | cout << " Reference sensor : " << scc.referenceSensor << endl; 479 | cout << " Scaled sensor : " << scc.scaledSensor << endl; 480 | 481 | loadScaleCalibrationFromFile( scc.referenceScaleCalibrationFile ); 482 | loadScaleCalibrationFromFile( scc.scaledScaleCalibrationFile ); 483 | 484 | if ( v_scaleCalibrations[0].referenceSensor != scc.referenceSensor ) 485 | cout << "[WARNING!] Reference sensor in " << scc.referenceScaleCalibrationFile 486 | << " doesn't match reference sensor label set in the configuration file" << endl; 487 | 488 | if ( v_scaleCalibrations[1].referenceSensor != scc.scaledSensor ) 489 | cout << "[WARNING!] Reference sensor in " << scc.referenceScaleCalibrationFile 490 | << " doesn't match scaled sensor label set in the configuration file" << endl; 491 | 492 | if ( v_scaleCalibrations[0].resolution != v_scaleCalibrations[1].resolution ) 493 | throw logic_error("Scale calibration files have a different resolution."); 494 | 495 | if ( v_scaleCalibrations[0].lowerRange != v_scaleCalibrations[1].lowerRange ) 496 | throw logic_error("Scale calibration files have a different lowerRange."); 497 | 498 | if ( v_scaleCalibrations[0].higerRange != v_scaleCalibrations[1].higerRange ) 499 | throw logic_error("Scale calibration files have a different higerRange."); 500 | 501 | ofstream f( scc.o_scaleCalibrationFile.c_str() ); 502 | 503 | if ( !f.is_open() ) 504 | cout << " [ERROR] Opening file " << scc.o_scaleCalibrationFile << endl; 505 | 506 | cout << " [INFO] Saving depth scale calibration results to " 507 | << scc.o_scaleCalibrationFile << endl; 508 | 509 | f << "Reference_sensor " << scc.referenceSensor << endl; 510 | f << "Scaled_sensor " << scc.scaledSensor << endl; 511 | f << "Resolution " << v_scaleCalibrations[0].resolution << endl; 512 | f << "Lower_depth " << v_scaleCalibrations[0].lowerRange << endl; 513 | f << "Higher_depth " << v_scaleCalibrations[0].higerRange << endl; 514 | 515 | for ( int i = 0; i < v_scaleCalibrations[0].scaleMultipliers.size(); i++ ) 516 | { 517 | f << v_scaleCalibrations[0].scaleMultipliers[i]/v_scaleCalibrations[1].scaleMultipliers[i] << " "; 518 | } 519 | 520 | cout << " [INFO] Done! " << endl << endl; 521 | } 522 | 523 | inline Eigen::Matrix4f getPoseEigenMatrix(const mrpt::poses::CPose3D & pose) 524 | { 525 | Eigen::Matrix4f pose_mat; 526 | mrpt::math::CMatrixDouble44 pose_mat_mrpt; 527 | pose.getHomogeneousMatrix(pose_mat_mrpt); 528 | pose_mat << pose_mat_mrpt(0,0), pose_mat_mrpt(0,1), pose_mat_mrpt(0,2), pose_mat_mrpt(0,3), 529 | pose_mat_mrpt(1,0), pose_mat_mrpt(1,1), pose_mat_mrpt(1,2), pose_mat_mrpt(1,3), 530 | pose_mat_mrpt(2,0), pose_mat_mrpt(2,1), pose_mat_mrpt(2,2), pose_mat_mrpt(2,3), 531 | pose_mat_mrpt(3,0), pose_mat_mrpt(3,1), pose_mat_mrpt(3,2), pose_mat_mrpt(3,3) ; 532 | return pose_mat; 533 | } 534 | 535 | //----------------------------------------------------------- 536 | // 537 | // showPoseFromHomogeneusMatrix 538 | // 539 | //----------------------------------------------------------- 540 | 541 | void showPoseFromHomogeneusMatrix() 542 | { 543 | Eigen::Matrix4f homoMatrix; 544 | cout << " [INFO] Loading homogeneus matrix from file: " << homogeneusMatrixFile << endl; 545 | homoMatrix.loadFromTextFile( homogeneusMatrixFile ); 546 | 547 | cout << endl << " Homogeneus matrix" << endl; 548 | cout << homoMatrix << endl << endl; 549 | 550 | mrpt::math::CMatrixDouble44 homoMatrixMRPT(homoMatrix); 551 | mrpt::poses::CPose3D rel_pose(homoMatrixMRPT); 552 | 553 | cout << "Relative pos: " << rel_pose << endl; 554 | 555 | mrpt::poses::CPose3D rgbd1_pose(0.271,-0.035,1.015,DEG2RAD(-45),DEG2RAD(0),DEG2RAD(-90)); 556 | 557 | cout << "Pose rgbd1 : " << rgbd1_pose << endl; 558 | cout << "Pose final : " << rgbd1_pose + rel_pose << endl << endl; 559 | 560 | } 561 | 562 | 563 | 564 | 565 | //----------------------------------------------------------- 566 | // 567 | // main 568 | // 569 | //----------------------------------------------------------- 570 | 571 | int main(int argc, char* argv[]) 572 | { 573 | try 574 | { 575 | // Set 3D window 576 | 577 | /*win3D.setWindowTitle("Sequential visualization"); 578 | 579 | win3D.resize(400,300); 580 | 581 | win3D.setCameraAzimuthDeg(140); 582 | win3D.setCameraElevationDeg(20); 583 | win3D.setCameraZoom(6.0); 584 | win3D.setCameraPointingToPoint(2.5,0,0); 585 | 586 | mrpt::opengl::COpenGLScenePtr scene = win3D.get3DSceneAndLock(); 587 | 588 | opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-7,7,-7,7,0,1); 589 | obj->setColor(0.7,0.7,0.7); 590 | obj->setLocation(0,0,0); 591 | scene->insert( obj ); 592 | 593 | mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create(); 594 | gl_points->setPointSize(4.5); 595 | 596 | win3D.unlockAccess3DScene();*/ 597 | 598 | cout << endl << "-----------------------------------------------------" << endl; 599 | cout << " Calibrate app. " << endl; 600 | cout << " [Object Labeling Tookit] " << endl; 601 | cout << "-----------------------------------------------------" << endl << endl; 602 | 603 | // 604 | // Load paramteres 605 | // 606 | 607 | if ( argc >= 1 ) 608 | { 609 | for ( size_t arg = 1; arg < argc; arg++ ) 610 | { 611 | if ( !strcmp(argv[arg],"-h") ) 612 | { 613 | showUsageInformation(); 614 | return 0; 615 | } 616 | else if ( !strcmp(argv[arg],"-fromHomogeneusMatrixToPose") ) 617 | { 618 | fromHomogeneusMatrixToPose = true; 619 | homogeneusMatrixFile = argv[arg+1]; 620 | 621 | arg++; 622 | cout << " [INFO] Showing 3D pose from homogeneus matrix within a file." << endl; 623 | } 624 | else if ( !strcmp(argv[arg],"-config") ) 625 | { 626 | configFileName = homogeneusMatrixFile = argv[arg]; 627 | } 628 | else 629 | { 630 | cout << " [ERROR] Unknown option " << argv[arg] << endl; 631 | showUsageInformation(); 632 | return -1; 633 | } 634 | } 635 | 636 | } 637 | else 638 | { 639 | showUsageInformation(); 640 | 641 | return 0; 642 | } 643 | 644 | 645 | // 646 | // What to do? 647 | 648 | if ( fromHomogeneusMatrixToPose ) 649 | showPoseFromHomogeneusMatrix(); 650 | else 651 | { 652 | // 653 | // Load config information 654 | 655 | loadConfig(); 656 | 657 | if ( computeScaleBetweenRGBDSensors && !scaleCalibrationConfig.computeTransitiveScale ) 658 | computeScale(); 659 | 660 | else if ( computeScaleBetweenRGBDSensors && scaleCalibrationConfig.computeTransitiveScale ) 661 | computeTransitiveScale(); 662 | } 663 | 664 | return 0; 665 | 666 | } catch (exception &e) 667 | { 668 | cout << "Exception caught: " << e.what() << endl; 669 | return -1; 670 | } 671 | catch (...) 672 | { 673 | printf("Another exception!!"); 674 | return -1; 675 | } 676 | } 677 | 678 | 679 | 680 | //! Return the rotation vector of the input pose 681 | /*inline Eigen::Matrix4f getPoseEigenMatrix(const mrpt::poses::CPose3D & pose) 682 | { 683 | Eigen::Matrix4f pose_mat; 684 | mrpt::math::CMatrixDouble44 pose_mat_mrpt; 685 | pose.getHomogeneousMatrix(pose_mat_mrpt); 686 | pose_mat << pose_mat_mrpt(0,0), pose_mat_mrpt(0,1), pose_mat_mrpt(0,2), pose_mat_mrpt(0,3), 687 | pose_mat_mrpt(1,0), pose_mat_mrpt(1,1), pose_mat_mrpt(1,2), pose_mat_mrpt(1,3), 688 | pose_mat_mrpt(2,0), pose_mat_mrpt(2,1), pose_mat_mrpt(2,2), pose_mat_mrpt(2,3), 689 | pose_mat_mrpt(3,0), pose_mat_mrpt(3,1), pose_mat_mrpt(3,2), pose_mat_mrpt(3,3) ; 690 | return pose_mat; 691 | } 692 | int main (int argc, char ** argv) 693 | { 694 | 695 | cout << "Change the reference system of the calibrattion of RGBD360 multisensor\n"; 696 | float angle_offset = 22.5; //45 697 | Eigen::Matrix4f rot_offset = Eigen::Matrix4f::Identity(); rot_offset(1,1) = rot_offset(2,2) = cos(DEG2RAD(angle_offset)); rot_offset(1,2) = -sin(DEG2RAD(angle_offset)); rot_offset(2,1) = -rot_offset(1,2); 698 | // Load initial calibration 699 | cout << "Load initial calibration\n"; 700 | mrpt::poses::CPose3D pose[NUM_ASUS_SENSORS]; 701 | pose[0] = mrpt::poses::CPose3D(0.285, 0, 1.015, DEG2RAD(0), DEG2RAD(1.3), DEG2RAD(-90)); 702 | pose[1] = mrpt::poses::CPose3D(0.271, -0.031, 1.015, DEG2RAD(-45), DEG2RAD(0), DEG2RAD(-90)); 703 | pose[2] = mrpt::poses::CPose3D(0.271, 0.031, 1.125, DEG2RAD(45), DEG2RAD(2), DEG2RAD(-89)); 704 | pose[3] = mrpt::poses::CPose3D(0.24, -0.045, 0.975, DEG2RAD(-90), DEG2RAD(1.5), DEG2RAD(-90)); 705 | // int rgbd180_arrangement[4] = {1,8,2,7}; 706 | 707 | Eigen::Matrix4f Rt_[4]; 708 | Eigen::Matrix4f Rt_raul[4]; 709 | Eigen::Matrix4f relative_edu[4]; 710 | Eigen::Matrix4f relative_raul[4]; 711 | Eigen::Matrix4f Rt_raul_new[4]; 712 | 713 | Eigen::Matrix4f change_ref = Eigen::Matrix4f::Zero(); 714 | // change_ref(0,1) = 1.f; 715 | // change_ref(1,2) = 1.f; 716 | // change_ref(2,0) = 1.f; 717 | // change_ref(3,3) = 1.f; 718 | 719 | // change_ref(0,2) = 1.f; 720 | // change_ref(1,1) = -1.f; 721 | // change_ref(2,0) = 1.f; 722 | // change_ref(3,3) = 1.f; 723 | 724 | change_ref(0,2) = 1.f; 725 | change_ref(1,0) = -1.f; 726 | change_ref(2,1) = -1.f; 727 | change_ref(3,3) = 1.f; 728 | 729 | 730 | for(size_t sensor_id=0; sensor_id < NUM_ASUS_SENSORS; sensor_id++) 731 | { 732 | Rt_[sensor_id].loadFromTextFile( mrpt::format("Rt_0%i.txt",sensor_id+1) ); 733 | Rt_raul[sensor_id] = getPoseEigenMatrix( pose[sensor_id] ); //.inverse(); 734 | //cout << " Rt_raul \n" << pose[sensor_id].getHomogeneousMatrixVal() << endl << Rt_raul[sensor_id] << endl; 735 | 736 | if(sensor_id > 0) 737 | { 738 | relative_edu[sensor_id] = Rt_[0].inverse() * Rt_[sensor_id]; 739 | //relative_raul[sensor_id] = change_ref.transpose() * relative_edu[sensor_id] * change_ref; 740 | relative_raul[sensor_id] = change_ref * relative_edu[sensor_id] * change_ref.inverse(); 741 | 742 | //relative_edu[sensor_id] = Rt_[0] * Rt_[sensor_id]; 743 | //relative_raul[sensor_id] = relative_edu[sensor_id] * change_ref; 744 | 745 | Rt_raul_new[sensor_id] = Rt_raul[0] * relative_raul[sensor_id]; 746 | 747 | cout << " rel edu \n" << relative_edu[sensor_id] << endl; 748 | cout << " rel edu to raul \n" << relative_raul[sensor_id] << endl; 749 | cout << " rel raul \n" << Rt_raul[0].inverse() * Rt_raul[sensor_id] << endl; 750 | cout << " ref sensor \n" < | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | /** This application permits to create a video file showing the RBG or depth 24 | * images of RGB-D observations into a rawlog. If the batch mode is on, then 25 | * the video will contain all the images in all the rawlogs of the dataset. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include // std::accumulate 41 | 42 | using namespace mrpt; 43 | using namespace mrpt::utils; 44 | using namespace mrpt::vision; 45 | using namespace mrpt::obs; 46 | using namespace mrpt::system; 47 | using namespace mrpt::math; 48 | 49 | using namespace std; 50 | 51 | 52 | vector v_sensorsToUse; 53 | bool useDepthImg = false; 54 | 55 | 56 | //----------------------------------------------------------- 57 | // getImages 58 | //----------------------------------------------------------- 59 | 60 | void getImages( CVideoFileWriter &vid, string &i_rawlogFileName, 61 | size_t imgWidth, size_t width, size_t height, size_t N_channels, 62 | string textToAdd, size_t N_sensors ) 63 | { 64 | if ( !mrpt::system::fileExists(i_rawlogFileName) ) 65 | return; 66 | 67 | cout << " Working with " << i_rawlogFileName; 68 | cout.flush(); 69 | 70 | CImage lastImg; 71 | vector v_obs(4); 72 | vector v_imagesLoaded(4,false); 73 | 74 | // Insert a few frames to show the 75 | CImage preImage(width,height,N_channels); 76 | preImage.textOut(10,20,textToAdd,TColor(255,0,255)); 77 | 78 | for ( size_t i=0; i < 10; i++ ) 79 | vid << preImage; 80 | 81 | CFileGZInputStream i_rawlog(i_rawlogFileName); 82 | 83 | CActionCollectionPtr action; 84 | CSensoryFramePtr observations; 85 | CObservationPtr obs; 86 | size_t obsIndex = 0; 87 | 88 | while ( CRawlog::getActionObservationPairOrObservation(i_rawlog,action,observations,obs,obsIndex) ) 89 | { 90 | if ( IS_CLASS(obs, CObservation3DRangeScan) ) 91 | { 92 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 93 | obs3D->load(); 94 | 95 | size_t inserted; 96 | 97 | if ( obs3D->sensorLabel == "RGBD_1" ) 98 | { 99 | v_obs[0] = obs3D; 100 | v_imagesLoaded[0] = true; 101 | inserted = 0; 102 | } 103 | else if ( obs3D->sensorLabel == "RGBD_2" ) 104 | { 105 | v_obs[1] = obs3D; 106 | v_imagesLoaded[1] = true; 107 | inserted = 1; 108 | } 109 | else if ( obs3D->sensorLabel == "RGBD_3" ) 110 | { 111 | v_obs[2] = obs3D; 112 | v_imagesLoaded[2] = true; 113 | inserted = 2; 114 | } 115 | else if ( obs3D->sensorLabel == "RGBD_4" ) 116 | { 117 | v_obs[3] = obs3D; 118 | v_imagesLoaded[3] = true; 119 | inserted = 3; 120 | } 121 | 122 | double sum = v_imagesLoaded[0] + v_imagesLoaded[1] 123 | + v_imagesLoaded[2] + v_imagesLoaded[3]; 124 | 125 | for ( size_t i = 0; i < 4; i++ ) 126 | { 127 | if (( i == inserted ) || (v_obs[i].null())) 128 | continue; 129 | 130 | if ( timeDifference( v_obs[i]->timestamp, v_obs[inserted]->timestamp ) 131 | > 0.5 ) 132 | 133 | v_imagesLoaded[i] = 0; 134 | 135 | 136 | } 137 | 138 | if ( sum == 4 ) 139 | { 140 | 141 | CImage img(height,width,N_channels); 142 | 143 | size_t imagesDrawn = 0; 144 | 145 | vector v_imgsOrder(4); 146 | /*v_imgsOrder[0] = 2; 147 | v_imgsOrder[1] = 0; 148 | v_imgsOrder[2] = 1; 149 | v_imgsOrder[3] = 3;*/ 150 | 151 | v_imgsOrder[0] = 2; 152 | v_imgsOrder[1] = 3; 153 | v_imgsOrder[2] = 0; 154 | v_imgsOrder[3] = 1; 155 | 156 | for ( size_t i = 0; i < 4; i++ ) 157 | { 158 | if ( v_sensorsToUse[i] ) 159 | { 160 | if ( useDepthImg ) 161 | { 162 | CImage imgDepth; 163 | imgDepth.setFromMatrix(v_obs[v_imgsOrder[i]]->rangeImage); 164 | 165 | img.drawImage(0,imgWidth*imagesDrawn,imgDepth); 166 | } 167 | else 168 | img.drawImage(0,imgWidth*imagesDrawn,v_obs[v_imgsOrder[i]]->intensityImage); 169 | imagesDrawn++; 170 | } 171 | } 172 | 173 | 174 | CImage rotatedImg(width,height,N_channels); 175 | 176 | for ( size_t row = 0; row < 320; row++ ) 177 | for ( size_t col = 0; col < imgWidth*N_sensors; col++ ) 178 | { 179 | u_int8_t r, g,b; 180 | 181 | if ( useDepthImg ) 182 | { 183 | r = g = b = *(img.get_unsafe(row,col,0)); 184 | } 185 | else 186 | { 187 | r = *(img.get_unsafe(row,col,2)); 188 | g = *(img.get_unsafe(row,col,1)); 189 | b = *(img.get_unsafe(row,col,0)); 190 | } 191 | 192 | TColor color(r,g,b); 193 | 194 | rotatedImg.setPixel(col,320-row,color); 195 | 196 | } 197 | 198 | rotatedImg.textOut(10,20,textToAdd,TColor(255,0,255)); 199 | vid << rotatedImg; 200 | lastImg = rotatedImg; 201 | 202 | v_imagesLoaded.clear(); 203 | v_imagesLoaded.resize(4,0); 204 | 205 | } 206 | } 207 | } 208 | 209 | for ( size_t i = 0; i < 8; i++ ) 210 | vid << lastImg; 211 | 212 | cout << " DONE!" << endl; 213 | 214 | } 215 | 216 | //----------------------------------------------------------- 217 | // main 218 | //----------------------------------------------------------- 219 | 220 | int main(int argc, char **argv) 221 | { 222 | 223 | cout << endl << "-----------------------------------------------------" << endl; 224 | cout << " Create video app. " << endl; 225 | cout << " [Object Labeling Tookit] " << endl; 226 | cout << "-----------------------------------------------------" << endl << endl; 227 | 228 | try 229 | { 230 | 231 | string i_rawlogFile; 232 | string o_videoFile; 233 | 234 | int fps = 10; 235 | 236 | bool batchMode; 237 | 238 | string datasetPath; 239 | vector v_sessions; 240 | v_sessions.push_back(""); 241 | // v_sessions.push_back("session1"); 242 | // v_sessions.push_back("session2"); 243 | // v_sessions.push_back("session3"); 244 | 245 | vector v_typesOfRooms; 246 | v_typesOfRooms.push_back("bathroom"); 247 | v_typesOfRooms.push_back("bedroom"); 248 | v_typesOfRooms.push_back("corridor"); 249 | v_typesOfRooms.push_back("fullhouse"); 250 | v_typesOfRooms.push_back("hall"); 251 | v_typesOfRooms.push_back("kitchen"); 252 | v_typesOfRooms.push_back("livingroom"); 253 | v_typesOfRooms.push_back("masterroom"); 254 | v_typesOfRooms.push_back("livingroomkitchen"); 255 | 256 | vector v_sequences; 257 | 258 | v_sensorsToUse.resize(4,true); 259 | 260 | // 261 | // Load parameters 262 | // 263 | 264 | bool sensorsReset = false; 265 | 266 | if ( argc >= 3 ) 267 | { 268 | i_rawlogFile = argv[1]; 269 | o_videoFile = argv[2]; 270 | 271 | size_t arg = 3; 272 | 273 | while ( arg < argc ) 274 | { 275 | if ( !strcmp(argv[arg],"-useDepthImg") ) 276 | { 277 | useDepthImg = true; 278 | } 279 | else if ( !strcmp(argv[arg],"-fps") ) 280 | { 281 | fps = atoi(argv[arg+1]); 282 | arg++; 283 | } 284 | else if ( !strcmp(argv[arg],"-batchMode") ) 285 | { 286 | batchMode = true; 287 | datasetPath = i_rawlogFile; 288 | } 289 | else if ( !strcmp(argv[arg],"-sensor") ) 290 | { 291 | if ( !sensorsReset ) 292 | { 293 | v_sensorsToUse.clear(); 294 | v_sensorsToUse.resize(4,false); 295 | } 296 | 297 | sensorsReset = true; 298 | 299 | string sensorLabel(argv[arg+1]); 300 | 301 | if ( sensorLabel == "RGBD_1" ) 302 | v_sensorsToUse[0] = true; 303 | else if ( sensorLabel == "RGBD_2" ) 304 | v_sensorsToUse[1] = true; 305 | else if ( sensorLabel == "RGBD_3" ) 306 | v_sensorsToUse[2] = true; 307 | else if ( sensorLabel == "RGBD_4" ) 308 | v_sensorsToUse[3] = true; 309 | else 310 | { 311 | cout << "[Error] " << argv[arg+1] << "unknown sensor label" << endl; 312 | return -1; 313 | } 314 | 315 | arg++; 316 | } 317 | else 318 | { 319 | cout << "[Error] " << argv[arg] << " unknown paramter" << endl; 320 | return -1; 321 | } 322 | 323 | arg++; 324 | } 325 | } 326 | else 327 | { 328 | cout << "Usage information. Two expected arguments: " << endl << 329 | " \t (1) Input rawlog file / path of the dataset in the system." << endl << 330 | " \t (2) Output video file." << endl; 331 | cout << "Then, optional parameters:" << endl << 332 | " \t -fps : Frames per second of the output video." << endl << 333 | " \t -sensor : Use information from this sensor." << endl << 334 | " \t -useDepthImg : Use depth images instead of RGB." << endl << 335 | " \t -batchMode : Create a video with all the sequences into the dataset." << endl << 336 | " \t If set, then the first app parameter is the dataset path." << endl; 337 | return 0; 338 | } 339 | 340 | if ( useDepthImg ) 341 | cout << " [INFO] Using depth image." << endl; 342 | if ( batchMode ) 343 | { 344 | cout << " [INFO] Using batch mode." << endl; 345 | 346 | for ( size_t i = 0; i < v_typesOfRooms.size(); i++ ) 347 | { 348 | std::stringstream ss; 349 | ss << datasetPath << v_typesOfRooms[i]; 350 | 351 | bool dirExists = true; 352 | size_t j = 1; 353 | 354 | while (dirExists) 355 | { 356 | std::stringstream ss2; 357 | ss2 << ss.str() << j; 358 | 359 | string dir = ss2.str(); 360 | 361 | if ( mrpt::system::directoryExists(dir) ) 362 | { 363 | std::stringstream s3; 364 | s3 << v_typesOfRooms[i] << j; 365 | v_sequences.push_back(s3.str()); 366 | cout << " " << dir << " exists" << endl; 367 | } 368 | else 369 | { 370 | dirExists = false; 371 | cout << " " << dir << " doesn't exist" << endl; 372 | } 373 | 374 | j++; 375 | } 376 | } 377 | } 378 | 379 | size_t N_sensors = v_sensorsToUse[0] + v_sensorsToUse[1] 380 | + v_sensorsToUse[2] + v_sensorsToUse[3]; 381 | 382 | CVideoFileWriter vid; 383 | 384 | size_t imgWidth = ( useDepthImg ) ? 244 : 240; 385 | 386 | size_t width = imgWidth*N_sensors; 387 | size_t height = 320; 388 | 389 | size_t N_channels = ( useDepthImg ) ? 1 : 3; 390 | 391 | vid.open( o_videoFile, 392 | fps, 393 | TPixelCoord(width,height), 394 | "MJPG", 395 | (useDepthImg) ? false : true 396 | ); 397 | 398 | if ( !vid.isOpen() ) 399 | { 400 | cout << " [ERROR] Opening video file." << endl; 401 | return -1; 402 | } 403 | 404 | 405 | 406 | // 407 | // Batch mode 408 | // 409 | 410 | if ( !batchMode ) 411 | { 412 | cout << "Working with " << i_rawlogFile << endl; 413 | 414 | getImages( vid, i_rawlogFile, imgWidth, width, height, N_channels, string(""),N_sensors ); 415 | 416 | } 417 | else 418 | { 419 | for ( size_t session_index = 0; session_index < v_sessions.size(); session_index++ ) 420 | for ( size_t sequece_index = 0; sequece_index < v_sequences.size(); sequece_index++ ) 421 | for ( size_t rawlog_index = 1; rawlog_index < 4; rawlog_index++ ) 422 | { 423 | 424 | std::stringstream ss; 425 | ss << rawlog_index; 426 | string i_rawlogFileName = datasetPath+"/"+v_sessions[session_index]+"/"+v_sequences[sequece_index]+"/"+ss.str()+".rawlog"; 427 | string textToAdd = v_sessions[session_index]+" / "+v_sequences[sequece_index] + " / " + ss.str(); 428 | 429 | getImages( vid, i_rawlogFileName, 430 | imgWidth, width, 431 | height, N_channels, 432 | textToAdd, N_sensors ); 433 | } 434 | } 435 | 436 | vid.close(); 437 | 438 | cout << " [INFO] Video completed!" << endl; 439 | 440 | return 0; 441 | 442 | } catch (exception &e) 443 | { 444 | cout << "HOMe exception caught: " << e.what() << endl; 445 | return -1; 446 | } 447 | catch (...) 448 | { 449 | printf("Another exception!!"); 450 | return -1; 451 | } 452 | } 453 | -------------------------------------------------------------------------------- /apps/dataset_statistics.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include "CAnalyzer.hpp" 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | using namespace mrpt::utils; 32 | using namespace mrpt::math; 33 | using namespace mrpt::obs; 34 | using namespace mrpt::poses; 35 | using namespace mrpt; 36 | using namespace std; 37 | 38 | // 39 | // Data types 40 | // 41 | 42 | vector sensors_to_use; 43 | CRawlog rawlog; 44 | bool analyzeDepthInfo = false; 45 | 46 | struct TConfiguration 47 | { 48 | vector labelsToConsider; // Objects' labels to be considered 49 | vector rawlogFiles; // Rawlog files to be considered 50 | bool instancesLabeled; // Are we working with instances? e.g. knife_1 51 | bool saveStatisticsToFile; // Save compute statistics to a file 52 | string statisticsOutputFileName; // Where to store statistics 53 | }; 54 | 55 | struct TStatistics 56 | { 57 | map labelOccurrences; // Map with 58 | map labelNumOfPixels; // Map with 59 | size_t N_observations; // Total number of observations processed 60 | 61 | TStatistics() : N_observations(0) 62 | {} 63 | }; 64 | 65 | // 66 | // Global variables 67 | // 68 | 69 | TConfiguration conf; 70 | TStatistics stats; 71 | 72 | //----------------------------------------------------------- 73 | // 74 | // showUsageInformation 75 | // 76 | //----------------------------------------------------------- 77 | 78 | void showUsageInformation() 79 | { 80 | cout << "Usage information. At least one expected argument: " << endl << 81 | " \t : Configuration file." << endl; 82 | cout << "Then, optional parameters:" << endl << 83 | " \t -h : This help." << endl << 84 | " \t -sensor : Use obs from this sensor (all by default)." << endl << 85 | " \t -analyzeDepthInfo : Analyze depth info." << endl << endl; 86 | 87 | } 88 | 89 | 90 | //----------------------------------------------------------- 91 | // 92 | // getInstanceLabel 93 | // 94 | //----------------------------------------------------------- 95 | 96 | string getInstanceLabel(const string &instaceLabel ) 97 | { 98 | vector::iterator it; 99 | 100 | for ( it = conf.labelsToConsider.begin(); 101 | it != conf.labelsToConsider.end(); 102 | it++ ) 103 | if ( instaceLabel.find(*it)!=std::string::npos ) 104 | return *it; 105 | 106 | string empty; 107 | return empty; 108 | } 109 | 110 | 111 | //----------------------------------------------------------- 112 | // 113 | // loadConfig 114 | // 115 | //----------------------------------------------------------- 116 | 117 | void loadConfig( string const configFile ) 118 | { 119 | CConfigFile config( configFile ); 120 | 121 | // Load general configuration 122 | 123 | config.read_vector("GENERAL","labelsToConsider",vector(0),conf.labelsToConsider,true); 124 | conf.instancesLabeled = config.read_bool("GENERAL","instancesLabeled",0,true); 125 | conf.saveStatisticsToFile = config.read_bool("GENERAL","saveStatisticsToFile",0,true); 126 | conf.statisticsOutputFileName = config.read_string("GENERAL","statisticsOutputFileName","",conf.saveStatisticsToFile); 127 | 128 | vector keys; 129 | config.getAllKeys("RAWLOGS",keys); 130 | 131 | for ( size_t i = 0; i < keys.size(); i++ ) 132 | { 133 | string rawlogName = config.read_string("RAWLOGS",keys[i],"",true); 134 | conf.rawlogFiles.push_back(rawlogName); 135 | } 136 | 137 | cout << "[INFO] Configuration successfully loaded." << endl; 138 | 139 | } 140 | 141 | 142 | //----------------------------------------------------------- 143 | // 144 | // initializeStats 145 | // 146 | //----------------------------------------------------------- 147 | 148 | void initializeStats() 149 | { 150 | // Initialize stats 151 | vector::iterator it; 152 | 153 | for ( it = conf.labelsToConsider.begin(); it != conf.labelsToConsider.end(); it++ ) 154 | { 155 | stats.labelNumOfPixels[*it] = 0; 156 | stats.labelOccurrences[*it] = 0; 157 | } 158 | 159 | cout << "[INFO] Stats initialized." << endl; 160 | } 161 | 162 | 163 | //----------------------------------------------------------- 164 | // 165 | // updateStatsFromObs 166 | // 167 | //----------------------------------------------------------- 168 | 169 | void updateStatsFromObs(CObservation3DRangeScanPtr obs, 170 | size_t N_rows, size_t N_cols ) 171 | { 172 | // Increment the number of processed observations 173 | stats.N_observations++; 174 | 175 | std::map::iterator it; 176 | 177 | for ( it = obs->pixelLabels->pixelLabelNames.begin(); 178 | it != obs->pixelLabels->pixelLabelNames.end(); 179 | it++ ) 180 | { 181 | string label = ( conf.instancesLabeled ) ? 182 | getInstanceLabel(it->second) : 183 | label = it->second; 184 | 185 | //cout << "Cheking label " << label << endl; 186 | 187 | // Update occurrences 188 | if ( !label.empty() ) 189 | stats.labelOccurrences[label] = stats.labelOccurrences[label] + 1; 190 | 191 | // Update num of pixels 192 | for ( size_t row = 0; row < N_rows; row++ ) 193 | for ( size_t col = 0; col < N_cols; col++ ) 194 | if ( obs->pixelLabels->checkLabel(row,col,it->first) ) 195 | stats.labelNumOfPixels[label] = stats.labelNumOfPixels[label] + 1; 196 | 197 | } 198 | 199 | } 200 | 201 | bool compareLabels( pair l1, pair l2 ) 202 | { 203 | return ( l1.second > l2.second ); 204 | } 205 | 206 | 207 | //----------------------------------------------------------- 208 | // 209 | // saveStatsToFile 210 | // 211 | //----------------------------------------------------------- 212 | 213 | void saveStatsToFile(ostream &statsFile, vector &v_sensorsUsed ) 214 | { 215 | // 216 | // General information 217 | // 218 | 219 | statsFile << "Rawlogs processed: " << conf.rawlogFiles.size() << endl; 220 | for ( size_t rawlog_index = 0; rawlog_index < conf.rawlogFiles.size(); rawlog_index++ ) 221 | statsFile << "- " << conf.rawlogFiles[rawlog_index] << endl; 222 | 223 | if ( v_sensorsUsed.empty() ) 224 | statsFile << "Sensors used: all" << endl; 225 | else 226 | { 227 | statsFile << "Sensors used: " << v_sensorsUsed.size() << endl; 228 | for ( size_t sensor_index = 0; sensor_index < v_sensorsUsed.size(); sensor_index++ ) 229 | statsFile << "- " << v_sensorsUsed[sensor_index] << endl; 230 | } 231 | 232 | statsFile << "Number of observations processed: "; 233 | statsFile << stats.N_observations << endl; 234 | 235 | // 236 | // Occurrences 237 | // 238 | 239 | std::vector > sorted_labels; 240 | 241 | //fill items 242 | map< string, size_t>::iterator it; 243 | 244 | for ( it = stats.labelOccurrences.begin(); 245 | it != stats.labelOccurrences.end(); 246 | it++ ) 247 | sorted_labels.push_back( pair(it->first,it->second) ); 248 | 249 | //sort by value using std::sort 250 | std::sort(sorted_labels.begin(), sorted_labels.end(), compareLabels ); 251 | 252 | statsFile << "Occurrence of object labels:" << endl; 253 | 254 | for ( size_t i = 0; i < sorted_labels.size(); i++ ) 255 | statsFile << sorted_labels[i].first << " " << sorted_labels[i].second << endl; 256 | 257 | // 258 | // Pixels 259 | // 260 | 261 | sorted_labels.clear(); 262 | 263 | for ( it = stats.labelNumOfPixels.begin(); 264 | it != stats.labelNumOfPixels.end(); 265 | it++ ) 266 | sorted_labels.push_back( pair(it->first,it->second) ); 267 | 268 | //sort by value using std::sort 269 | std::sort(sorted_labels.begin(), sorted_labels.end(), compareLabels ); 270 | 271 | statsFile << "Number of pixels per object labels:" << endl; 272 | 273 | for ( size_t i = 0; i < sorted_labels.size(); i++ ) 274 | statsFile << sorted_labels[i].first << " " << sorted_labels[i].second << endl; 275 | 276 | } 277 | 278 | int loadParamters(int argc, char* argv[]) 279 | { 280 | if ( argc > 1 ) 281 | { 282 | // Get configuration file name 283 | 284 | string configFile = argv[1]; 285 | 286 | // Load configuration and initialize stats 287 | 288 | loadConfig(configFile); 289 | initializeStats(); 290 | 291 | // Get optional paramteres 292 | if ( argc > 2 ) 293 | { 294 | size_t arg = 2; 295 | 296 | while ( arg < argc ) 297 | { 298 | if ( !strcmp(argv[arg],"-h") ) 299 | { 300 | showUsageInformation(); 301 | arg++; 302 | } 303 | else if ( !strcmp(argv[arg],"-sensor") ) 304 | { 305 | string sensor = argv[arg+1]; 306 | sensors_to_use.push_back( sensor ); 307 | arg += 2; 308 | } 309 | else if ( !strcmp(argv[arg],"-analyzeDepthInfo") ) 310 | { 311 | analyzeDepthInfo = true; 312 | arg ++; 313 | } 314 | 315 | else 316 | { 317 | cout << "[Error] " << argv[arg] << " unknown paramter" << endl; 318 | return -1; 319 | } 320 | } 321 | } 322 | } 323 | else 324 | { 325 | showUsageInformation(); 326 | 327 | return -1; 328 | } 329 | 330 | } 331 | 332 | void generateStats() 333 | { 334 | const size_t N_rawlogs = conf.rawlogFiles.size(); 335 | cout << "[INFO] a total of " << N_rawlogs << " to process." << endl; 336 | 337 | for ( size_t rawlog_index = 0; rawlog_index < N_rawlogs; rawlog_index++ ) 338 | { 339 | rawlog.loadFromRawLogFile( conf.rawlogFiles[rawlog_index] ); 340 | 341 | cout << "[INFO] Processing rawlog file : " << conf.rawlogFiles[rawlog_index]; 342 | cout << " with " << rawlog.size() << " obs and index " << rawlog_index << endl; 343 | 344 | // 345 | // Iterate over the obs into the rawlog updating the stats 346 | // 347 | 348 | for ( size_t obs_index = 0; obs_index < rawlog.size(); obs_index++ ) 349 | { 350 | CObservationPtr obs = rawlog.getAsObservation(obs_index); 351 | 352 | // Check that it is a 3D observation 353 | if ( !IS_CLASS(obs, CObservation3DRangeScan) ) 354 | continue; 355 | 356 | // Check if the sensor is being used 357 | if ( !sensors_to_use.empty() 358 | && find(sensors_to_use.begin(), sensors_to_use.end(),obs->sensorLabel) 359 | == sensors_to_use.end() ) 360 | continue; 361 | 362 | // Get obs pose 363 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 364 | obs3D->load(); 365 | 366 | size_t rows = obs3D->cameraParams.nrows; 367 | size_t cols = obs3D->cameraParams.ncols; 368 | 369 | // Update statistics with information from this observations 370 | updateStatsFromObs( obs3D, rows, cols ); 371 | 372 | } 373 | } 374 | 375 | // 376 | // Save stats to file 377 | // 378 | 379 | if ( conf.saveStatisticsToFile ) 380 | { 381 | cout << "[INFO] Saving statistics to " << conf.statisticsOutputFileName << endl; 382 | ofstream statsFile(conf.statisticsOutputFileName.c_str(),ios::trunc); 383 | 384 | if ( statsFile.is_open() ) 385 | { 386 | saveStatsToFile(statsFile,sensors_to_use); 387 | statsFile.close(); 388 | } 389 | else 390 | cout << "[ERROR] Error when opening the file to save the statistics."; 391 | } 392 | 393 | saveStatsToFile(cout,sensors_to_use); 394 | 395 | cout << "[INFO] Done!" << endl; 396 | } 397 | 398 | void analyzeDepths() 399 | { 400 | const size_t N_rawlogs = conf.rawlogFiles.size(); 401 | cout << "[INFO] a total of " << N_rawlogs << " to process." << endl; 402 | 403 | double maxDepth = 0; 404 | double minDepth = std::numeric_limits::max(); 405 | double meanDepth = 0; 406 | 407 | for ( size_t rawlog_index = 0; rawlog_index < N_rawlogs; rawlog_index++ ) 408 | { 409 | OLT::CDepthInfoAnalyzer analyzer; 410 | 411 | analyzer.setInputRawlog( conf.rawlogFiles[rawlog_index] ); 412 | 413 | vector results; 414 | 415 | analyzer.process(results); 416 | 417 | if ( results[0] > maxDepth ) 418 | maxDepth = results[0]; 419 | 420 | if ( results[1] < minDepth ) 421 | minDepth = results[1]; 422 | 423 | meanDepth = results[2]; 424 | 425 | } 426 | 427 | cout << " [INFO] Results: " << endl; 428 | cout << " Max depth = " << maxDepth << endl; 429 | cout << " Min depth = " << minDepth << endl; 430 | cout << " Mean depth = " << meanDepth / static_cast(N_rawlogs) << endl; 431 | 432 | cout << "[INFO] Done!" << endl; 433 | } 434 | 435 | //----------------------------------------------------------- 436 | // 437 | // main 438 | // 439 | //----------------------------------------------------------- 440 | 441 | int main(int argc, char* argv[]) 442 | { 443 | 444 | try 445 | { 446 | // Useful variables 447 | 448 | loadParamters(argc, argv); 449 | 450 | if ( sensors_to_use.empty() ) 451 | cout << "[INFO] Considering observations from any sensor." << endl; 452 | else 453 | { 454 | cout << "[INFO] Considering observations from: "; 455 | for ( size_t i_sensor = 0; i_sensor < sensors_to_use.size(); i_sensor++ ) 456 | cout << sensors_to_use[i_sensor] << " "; 457 | cout << endl; 458 | } 459 | 460 | if ( analyzeDepthInfo ) 461 | analyzeDepths(); 462 | else 463 | generateStats(); 464 | 465 | return 0; 466 | 467 | } catch (exception &e) 468 | { 469 | cout << "Exception caught: " << e.what() << endl; 470 | return -1; 471 | } 472 | catch (...) 473 | { 474 | printf("Another exception!!"); 475 | return -1; 476 | } 477 | } 478 | -------------------------------------------------------------------------------- /apps/label_rawlog.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | #include 55 | 56 | #include 57 | 58 | #include 59 | #include 60 | 61 | #include 62 | 63 | using namespace mrpt::utils; 64 | using namespace mrpt::math; 65 | using namespace mrpt::opengl; 66 | using namespace mrpt::maps; 67 | using namespace mrpt::obs; 68 | using namespace mrpt::poses; 69 | using namespace mrpt; 70 | using namespace std; 71 | 72 | // 73 | // Data types 74 | // 75 | 76 | struct TLabelledBox 77 | { 78 | // Loaded from .scene file 79 | CBoxPtr box; // object containing the corners of the object's bounding box 80 | string label; // e.g. scourer, bowl, or scourer_1, bowl_3 if working with instances 81 | // Computed 82 | pcl::PointCloud::Ptr convexHullCloud; 83 | vector polygons; 84 | 85 | TLabelledBox() : convexHullCloud(new pcl::PointCloud()) 86 | {} 87 | }; 88 | 89 | struct TConfiguration 90 | { 91 | bool visualizeLabels;// Enable visualization of individual observations 92 | string rawlogFile; // Rawlog file to be labeled 93 | string labelledScene; // Scene already labeled by "Label_scene" 94 | bool instancesLabeled; // Are we working with instances? e.g. knife_1 95 | bool saveLabeledImgsToFile; // Save label masks to a .jpg file 96 | }; 97 | 98 | // 99 | // Global variables 100 | // 101 | 102 | TConfiguration configuration; 103 | 104 | vector sensors_to_use; 105 | CFileGZInputStream i_rawlog; 106 | CFileGZOutputStream o_rawlog; 107 | 108 | vector v_labelled_boxes; 109 | map m_consideredLabels; // Map 110 | vector v_appearingLabels; 111 | 112 | typedef boost::shared_ptr pclWindow; 113 | mrpt::gui::CDisplayWindowPtr window; 114 | 115 | 116 | //----------------------------------------------------------- 117 | // 118 | // showUsageInformation 119 | // 120 | //----------------------------------------------------------- 121 | 122 | void showUsageInformation() 123 | { 124 | cout << "Usage information. At least one expected argument: " << endl << 125 | " \t : Configuration file." << endl; 126 | cout << "Then, optional parameters:" << endl << 127 | " \t -h : This help." << endl << 128 | " \t -config : Configuration file to be loaded." << endl << 129 | " \t -i : Rawlog file to process." << endl << 130 | " \t -sensor : Use obs. from this sensor (all used by default)." << endl << 131 | " \t -step : Enable step by step execution." << endl; 132 | } 133 | 134 | 135 | //----------------------------------------------------------- 136 | // 137 | // getInstanceLabel 138 | // 139 | //----------------------------------------------------------- 140 | 141 | string getInstanceLabel(const string &instaceLabel ) 142 | { 143 | map::iterator it; 144 | 145 | for ( it = m_consideredLabels.begin(); it != m_consideredLabels.end(); it++ ) 146 | if ( instaceLabel.find(it->first)!=std::string::npos ) 147 | return it->first; 148 | 149 | string empty; 150 | return empty; 151 | } 152 | 153 | 154 | //----------------------------------------------------------- 155 | // 156 | // loadConfig 157 | // 158 | //----------------------------------------------------------- 159 | 160 | void loadConfig( string const configFile ) 161 | { 162 | CConfigFile config( configFile ); 163 | 164 | // Load general configuration 165 | 166 | configuration.visualizeLabels = config.read_bool("GENERAL","visualizeLabels",0,true); 167 | configuration.rawlogFile = config.read_string("GENERAL","rawlogFile","",true); 168 | configuration.labelledScene = config.read_string("GENERAL","labelledScene","",true); 169 | configuration.instancesLabeled= config.read_bool("GENERAL","instancesLabeled","",true); 170 | configuration.saveLabeledImgsToFile= config.read_bool("GENERAL","saveLabeledImgsToFile","",true); 171 | 172 | 173 | // Load object labels (classes) to be considered 174 | 175 | vector v_labelNames; 176 | config.read_vector("LABELS","labelNames",vector(0),v_labelNames,true); 177 | 178 | size_t magicNumber = ceil(pow(v_labelNames.size(),1.0/3.0)); 179 | 180 | vector v_colors; 181 | 182 | for ( double r = 0; r < magicNumber; r+= 1 ) 183 | for ( double g = 0; g < magicNumber; g+= 1 ) 184 | for ( double b = 0; b < magicNumber; b+= 1 ) 185 | v_colors.push_back(TPoint3D(1.0*(1-r/(magicNumber-1)), 186 | 1.0*(1-g/(magicNumber-1)), 187 | 1.0*(1-b/(magicNumber-1)))); 188 | 189 | 190 | for ( size_t i_label = 0; i_label < v_labelNames.size(); i_label++ ) 191 | m_consideredLabels[v_labelNames[i_label]] = v_colors[i_label]; 192 | 193 | /*cout << "[INFO] Loaded labels: " << endl; 194 | 195 | map::iterator it; 196 | 197 | for ( it = m_consideredLabels.begin(); it != m_consideredLabels.end(); it++ ) 198 | cout << " - " << it->first << ", with color " << it->second << endl;*/ 199 | 200 | cout << " [INFO] Configuration successfully loaded." << endl; 201 | 202 | } 203 | 204 | 205 | //----------------------------------------------------------- 206 | // 207 | // loadLabelledScene 208 | // 209 | //----------------------------------------------------------- 210 | 211 | void loadLabelledScene() 212 | { 213 | 214 | mrpt::opengl::COpenGLScenePtr labelledScene; 215 | 216 | labelledScene = mrpt::opengl::COpenGLScene::Create(); 217 | if ( labelledScene->loadFromFile( configuration.labelledScene ) ) 218 | { 219 | // Load previously inserted boxes 220 | bool keepLoading = true; 221 | size_t boxes_inserted = 0; 222 | 223 | while ( keepLoading ) 224 | { 225 | CBoxPtr box = labelledScene->getByClass(boxes_inserted); 226 | 227 | if ( box.null() ) 228 | keepLoading = false; 229 | else 230 | { 231 | TLabelledBox labelled_box; 232 | 233 | labelled_box.box = box; 234 | labelled_box.label = box->getName(); 235 | 236 | TPose3D pose = box->getPose(); 237 | 238 | TPoint3D c1,c2; 239 | box->getBoxCorners(c1,c2); 240 | 241 | TPoint3D C111 ( CPose3D(pose) + TPose3D(TPoint3D(c1.x,c1.y,c1.z)) ); 242 | TPoint3D C112 ( CPose3D(pose) + TPose3D(TPoint3D(c1.x,c1.y,c2.z)) ); 243 | TPoint3D C121 ( CPose3D(pose) + TPose3D(TPoint3D(c1.x,c2.y,c1.z)) ); 244 | TPoint3D C122 ( CPose3D(pose) + TPose3D(TPoint3D(c1.x,c2.y,c2.z)) ); 245 | TPoint3D C211 ( CPose3D(pose) + TPose3D(TPoint3D(c2.x,c1.y,c1.z)) ); 246 | TPoint3D C212 ( CPose3D(pose) + TPose3D(TPoint3D(c2.x,c1.y,c2.z)) ); 247 | TPoint3D C221 ( CPose3D(pose) + TPose3D(TPoint3D(c2.x,c2.y,c1.z)) ); 248 | TPoint3D C222 ( CPose3D(pose) + TPose3D(TPoint3D(c2.x,c2.y,c2.z)) ); 249 | 250 | pcl::PointCloud::Ptr pointCloud ( new pcl::PointCloud()); 251 | pointCloud->push_back( pcl::PointXYZ( C111.x, C111.y, C111.z )); 252 | pointCloud->push_back( pcl::PointXYZ( C112.x, C112.y, C112.z )); 253 | pointCloud->push_back( pcl::PointXYZ( C121.x, C121.y, C121.z )); 254 | pointCloud->push_back( pcl::PointXYZ( C122.x, C122.y, C122.z )); 255 | pointCloud->push_back( pcl::PointXYZ( C211.x, C211.y, C211.z )); 256 | pointCloud->push_back( pcl::PointXYZ( C212.x, C212.y, C212.z )); 257 | pointCloud->push_back( pcl::PointXYZ( C221.x, C221.y, C221.z )); 258 | pointCloud->push_back( pcl::PointXYZ( C222.x, C222.y, C222.z )); 259 | 260 | pcl::ConvexHull convex_hull; 261 | convex_hull.setInputCloud(pointCloud); 262 | convex_hull.setDimension(3); 263 | convex_hull.reconstruct(*labelled_box.convexHullCloud, 264 | labelled_box.polygons); 265 | 266 | Eigen::Matrix4f transMat; 267 | 268 | transMat(0,0)=0; transMat(0,1)=-1; transMat(0,2)=0; transMat(0,3)=0; 269 | transMat(1,0)=0; transMat(1,1)=0; transMat(1,2)=+1; transMat(1,3)=0; 270 | transMat(2,0)=1; transMat(2,1)=0; transMat(2,2)=0; transMat(2,3)=0; 271 | transMat(3,0)=0; transMat(3,1)=0; transMat(3,2)=0; transMat(3,3)=1; 272 | 273 | pcl::transformPointCloud( *labelled_box.convexHullCloud, 274 | *labelled_box.convexHullCloud, 275 | transMat ); 276 | 277 | v_labelled_boxes.push_back( labelled_box ); 278 | 279 | if ( !configuration.instancesLabeled ) 280 | { 281 | if ( !m_consideredLabels.count(labelled_box.label) ) 282 | cout << " [CAUTION] label " << labelled_box.label << " does not appear in the label list." << endl; 283 | } 284 | else 285 | { 286 | string label = getInstanceLabel(labelled_box.label); 287 | if ( label.empty() ) 288 | cout << " [CAUTION] label of instance " << labelled_box.label << " does not appear in the label list." << endl; 289 | } 290 | 291 | 292 | // Check if the label has been already inserted 293 | if ( find(v_appearingLabels.begin(), 294 | v_appearingLabels.end(), 295 | labelled_box.label) == v_appearingLabels.end() ) 296 | v_appearingLabels.push_back(labelled_box.label); 297 | } 298 | 299 | boxes_inserted++; 300 | } 301 | 302 | cout << " [INFO] " << v_labelled_boxes.size() << " labelled boxes loaded." << endl; 303 | 304 | } 305 | else 306 | cout << " [ERROR] While loading the labelled scene file." << endl; 307 | 308 | } 309 | 310 | 311 | //----------------------------------------------------------- 312 | // 313 | // labelObs 314 | // 315 | //----------------------------------------------------------- 316 | 317 | void labelObs(CObservation3DRangeScanPtr obs, 318 | pcl::PointCloud::Ptr cloud, 319 | size_t N_rows, size_t N_cols ) 320 | { 321 | map::iterator it; 322 | 323 | size_t N_boxes = v_labelled_boxes.size(); 324 | 325 | pclWindow viewer; 326 | 327 | CImage img(N_rows,N_cols); 328 | 329 | img.setOriginTopLeft(false); 330 | 331 | if ( configuration.visualizeLabels ) 332 | { 333 | window = mrpt::gui::CDisplayWindowPtr( new mrpt::gui::CDisplayWindow("Labeled depth img")); 334 | 335 | viewer = pclWindow(new pcl::visualization::PCLVisualizer ("3D Viewer")); 336 | viewer->initCameraParameters (); 337 | } 338 | 339 | if ( configuration.visualizeLabels || configuration.saveLabeledImgsToFile ) 340 | { 341 | for ( size_t row = 0; row < N_rows; row++ ) 342 | for ( size_t col = 0; col < N_cols; col++ ) 343 | img.setPixel(row, col, 0); 344 | } 345 | 346 | for ( size_t box_index = 0; box_index < v_labelled_boxes.size(); box_index++ ) 347 | { 348 | 349 | TLabelledBox &box = v_labelled_boxes[box_index]; 350 | 351 | //cout << "Evaluating " << box.label; 352 | 353 | pcl::PointCloud::Ptr outputCloud(new pcl::PointCloud()); 354 | pcl::CropHull cropHull; 355 | cropHull.setInputCloud( cloud ); 356 | //cropHull.setIndices( boost::make_shared (indices) ); 357 | cropHull.setHullIndices(box.polygons); 358 | cropHull.setHullCloud(box.convexHullCloud); 359 | cropHull.setDim(3); 360 | 361 | 362 | // viewer->removeAllPointClouds(); 363 | // viewer->addPointCloud( cloud ); 364 | // viewer->resetStoppedFlag(); 365 | // while (!viewer->wasStopped()) 366 | // viewer->spinOnce(100); 367 | 368 | vector v_indices; 369 | 370 | cropHull.filter(v_indices); 371 | //cout << "Size of indices: " << v_indices[box_index].size() << endl; 372 | 373 | if ( !v_indices.empty() ) 374 | { 375 | 376 | // 377 | // Give color to the point cloud excerpt 378 | // 379 | 380 | if ( configuration.visualizeLabels || configuration.saveLabeledImgsToFile ) 381 | { 382 | cropHull.filter(*outputCloud); 383 | 384 | pcl::PointCloud::Ptr coloredOutputCloud(new pcl::PointCloud()); 385 | 386 | TPoint3D color; 387 | 388 | if ( configuration.instancesLabeled ) 389 | { 390 | string label = getInstanceLabel(box.label); 391 | if ( label.size() ) 392 | color = m_consideredLabels[label]; 393 | else 394 | color = TPoint3D(1,1,1); 395 | } 396 | else 397 | color = m_consideredLabels[box.label]; 398 | 399 | uint8_t color_r = color.x*255; 400 | uint8_t color_g = color.y*255; 401 | uint8_t color_b = color.z*255; 402 | 403 | for ( size_t point = 0; point < outputCloud->size(); point++ ) 404 | { 405 | // Get and set pixel color for the depth img 406 | 407 | size_t indice = v_indices[point]; 408 | 409 | size_t pixelRow = floor(indice/double(N_cols)); 410 | size_t pixelCol = indice % N_cols; 411 | TColor pixelColor(color_r,color_g,color_b); 412 | img.setPixel(pixelRow,pixelCol,pixelColor); 413 | 414 | 415 | // Now, for the point cloud 416 | 417 | pcl::PointXYZRGB coloredPoint(color_r,color_g,color_b); 418 | coloredPoint.x = outputCloud->points[point].x; 419 | coloredPoint.y = outputCloud->points[point].y; 420 | coloredPoint.z = outputCloud->points[point].z; 421 | 422 | 423 | coloredOutputCloud->points.push_back(coloredPoint); 424 | } 425 | 426 | //viewer->removeAllPointClouds(); 427 | //viewer->adsetSizedPointCloud( box.convexHullCloud ); 428 | stringstream ss; 429 | ss << "Outputcloud_" << box_index; 430 | if ( configuration.visualizeLabels ) 431 | viewer->addPointCloud( coloredOutputCloud,string(ss.str()) ); 432 | } 433 | 434 | // Label the observation itself 435 | CObservation3DRangeScan::TPixelLabelInfoBase::TMapLabelID2Name &m_insertedLabels 436 | = obs->pixelLabels->pixelLabelNames; 437 | 438 | // Check if the label has been already inserted, so it has an index 439 | std::map::iterator it; 440 | int labelIndex = -1; 441 | 442 | for ( it = m_insertedLabels.begin(); it != m_insertedLabels.end(); it++ ) 443 | if ( it->second == box.label ) 444 | labelIndex = it->first; 445 | 446 | // If not, add it to the labels map with a new index 447 | if ( labelIndex == -1 ) 448 | { 449 | size_t N_labels = m_insertedLabels.size(); 450 | labelIndex = N_labels+1; 451 | obs->pixelLabels->setLabelName(labelIndex,box.label); 452 | //cout << "Label " << box.label << " does not exist in the obs"; 453 | //cout << ", now inserted with id: " << labelIndex << endl; 454 | 455 | } 456 | 457 | if ( labelIndex == -1 ) 458 | continue; 459 | 460 | // iterate over the points indices, and set the pixels into the observation 461 | // to the label index 462 | 463 | vector::iterator itIndices; 464 | for ( itIndices = v_indices.begin(); itIndices != v_indices.end(); itIndices++ ) 465 | { 466 | int &indice = *itIndices; 467 | size_t pixelRow = floor(indice/double(N_cols)); 468 | size_t pixelCol = indice % N_cols; 469 | //cout << "Setting pixel " << pixelRow << "," << pixelCol; 470 | //cout << " with label index " << labelIndex << " class " << box.label << endl; 471 | obs->pixelLabels->setLabel(pixelRow,pixelCol,labelIndex); 472 | } 473 | } 474 | } 475 | 476 | // Visualize labeling results 477 | 478 | if ( configuration.visualizeLabels ) 479 | { 480 | window->showImage(img); 481 | 482 | viewer->resetStoppedFlag(); 483 | 484 | while (!viewer->wasStopped()) 485 | viewer->spinOnce(100); 486 | 487 | } 488 | 489 | if ( configuration.saveLabeledImgsToFile ) 490 | { 491 | static int count = 0; 492 | std::stringstream ss; 493 | ss << "img_" << count++ << ".jpg"; 494 | img.saveToFile(ss.str()); 495 | } 496 | 497 | //cout << "Let's see.... " << endl; 498 | //cout << *(obs->pixelLabels) << endl; 499 | //mrpt::system::pause(); 500 | 501 | } 502 | 503 | 504 | //----------------------------------------------------------- 505 | // 506 | // loadParameters 507 | // 508 | //----------------------------------------------------------- 509 | 510 | int loadParameters(int argc, char* argv[]) 511 | { 512 | bool stepByStepExecution = false; 513 | 514 | if ( argc > 1 ) 515 | { 516 | size_t arg = 1; 517 | 518 | while ( arg < argc ) 519 | { 520 | if ( !strcmp(argv[arg],"-h") ) 521 | { 522 | showUsageInformation(); 523 | arg++; 524 | } 525 | else if ( !strcmp(argv[arg],"-i") ) 526 | { 527 | configuration.rawlogFile = argv[arg+1]; 528 | arg += 2; 529 | } 530 | else if ( !strcmp(argv[arg],"-config") ) 531 | { 532 | string configFile = argv[arg+1]; 533 | loadConfig(configFile); 534 | arg += 2; 535 | } 536 | else if ( !strcmp(argv[arg],"-l") ) 537 | { 538 | configuration.labelledScene = argv[arg+1]; 539 | arg += 2; 540 | } 541 | else if ( !strcmp(argv[arg],"-sensor") ) 542 | { 543 | string sensor = argv[arg+1]; 544 | sensors_to_use.push_back( sensor ); 545 | arg += 2; 546 | } 547 | else if ( !strcmp(argv[arg], "-step") ) 548 | { 549 | stepByStepExecution = true; 550 | arg++; 551 | } 552 | else 553 | { 554 | cout << " [Error] " << argv[arg] << " unknown paramter" << endl; 555 | showUsageInformation(); 556 | return -1; 557 | } 558 | 559 | } 560 | 561 | } 562 | else 563 | { 564 | showUsageInformation(); 565 | 566 | return -1; 567 | } 568 | 569 | return 0; 570 | } 571 | 572 | 573 | //----------------------------------------------------------- 574 | // 575 | // labelRawlog 576 | // 577 | //----------------------------------------------------------- 578 | 579 | void labelRawlog() 580 | { 581 | if ( sensors_to_use.empty() ) 582 | cout << " [INFO] Considering observations from any sensor." << endl; 583 | else 584 | { 585 | cout << " [INFO] Considering observations from: "; 586 | for ( size_t i_sensor = 0; i_sensor < sensors_to_use.size(); i_sensor++ ) 587 | cout << sensors_to_use[i_sensor] << " "; 588 | cout << endl; 589 | } 590 | 591 | // 592 | // Check input rawlog file 593 | 594 | if (!mrpt::system::fileExists(configuration.rawlogFile)) 595 | { 596 | cerr << " [ERROR] A rawlog file with name " << configuration.rawlogFile; 597 | cerr << " doesn't exist." << endl; 598 | // return; 599 | } 600 | 601 | i_rawlog.open(configuration.rawlogFile); 602 | 603 | cout << " [INFO] Rawlog file : " << configuration.rawlogFile << endl; 604 | cout << " [INFO] Labeled scene : " << configuration.labelledScene << endl; 605 | loadLabelledScene(); 606 | 607 | // 608 | // Set output rawlog file 609 | 610 | string o_rawlogFile; 611 | 612 | o_rawlogFile = configuration.rawlogFile.substr(0,configuration.rawlogFile.size()-7); 613 | o_rawlogFile += "_labelled.rawlog"; 614 | 615 | o_rawlog.open(o_rawlogFile); 616 | 617 | if ( configuration.visualizeLabels ) 618 | window = mrpt::gui::CDisplayWindowPtr( new mrpt::gui::CDisplayWindow("Labeled depth img")); 619 | 620 | // 621 | // Process rawlog 622 | 623 | CActionCollectionPtr action; 624 | CSensoryFramePtr observations; 625 | CObservationPtr obs; 626 | size_t obsIndex = 0; 627 | 628 | cout.flush(); 629 | 630 | while ( CRawlog::getActionObservationPairOrObservation(i_rawlog,action,observations,obs,obsIndex) ) 631 | { 632 | // Check that it is a 3D observation 633 | if ( !IS_CLASS(obs, CObservation3DRangeScan) ) 634 | continue; 635 | 636 | cout << "\r" << " Process: " << obsIndex; 637 | cout.flush(); 638 | 639 | // Check if the sensor is being used 640 | if ( !sensors_to_use.empty() 641 | && find(sensors_to_use.begin(), sensors_to_use.end(),obs->sensorLabel) 642 | == sensors_to_use.end() ) 643 | continue; 644 | 645 | // Get obs pose 646 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 647 | obs3D->load(); 648 | 649 | CPose3D pose; 650 | obs3D->getSensorPose( pose ); 651 | //cout << "Pose [" << obs_index << "]: " << pose << endl; 652 | 653 | size_t rows = obs3D->cameraParams.nrows; 654 | size_t cols = obs3D->cameraParams.ncols; 655 | 656 | // Create per pixel labeling 657 | // Label size (0=8 bits, 1=16 bits, 2=32 bits, 3=32 bits, 8=64 bits 658 | const unsigned int LABEL_SIZE = 2; // 32 bits 659 | obs3D->pixelLabels = CObservation3DRangeScan::TPixelLabelInfoPtr( new CObservation3DRangeScan::TPixelLabelInfo< LABEL_SIZE >() ); 660 | obs3D->pixelLabels->setSize(rows,cols); 661 | 662 | pcl::PointCloud::Ptr pcl_cloud( new pcl::PointCloud() ); 663 | 664 | obs3D->project3DPointsFromDepthImageInto( *pcl_cloud, true ); 665 | 666 | Eigen::Matrix4f transMat; 667 | 668 | transMat(0,0)=0; transMat(0,1)=-1; transMat(0,2)=0; transMat(0,3)=0; 669 | transMat(1,0)=0; transMat(1,1)=0; transMat(1,2)=+1; transMat(1,3)=0; 670 | transMat(2,0)=1; transMat(2,1)=0; transMat(2,2)=0; transMat(2,3)=0; 671 | transMat(3,0)=0; transMat(3,1)=0; transMat(3,2)=0; transMat(3,3)=1; 672 | 673 | pcl::transformPointCloud( *pcl_cloud, *pcl_cloud, transMat ); 674 | 675 | pcl_cloud->height = 240; 676 | pcl_cloud->width = 320; 677 | 678 | // 679 | // Label observation 680 | 681 | labelObs( obs3D, pcl_cloud, rows, cols ); 682 | 683 | // 684 | // Save to output file 685 | 686 | o_rawlog << obs3D; 687 | 688 | } 689 | 690 | i_rawlog.close(); 691 | o_rawlog.close(); 692 | 693 | cout << endl << " [INFO] Done!" << endl << endl; 694 | } 695 | 696 | //----------------------------------------------------------- 697 | // 698 | // main 699 | // 700 | //----------------------------------------------------------- 701 | 702 | int main(int argc, char* argv[]) 703 | { 704 | 705 | cout << endl << "-----------------------------------------------------" << endl; 706 | cout << " Label rawlog app. " << endl; 707 | cout << " [Object Labeling Tookit] " << endl; 708 | cout << "-----------------------------------------------------" << endl << endl; 709 | 710 | try 711 | { 712 | //mrpt::utils::registerClass(CLASS_ID(CObservation3DRangeScan)); 713 | 714 | // 715 | // Load parameters 716 | 717 | int res = loadParameters(argc,argv); 718 | 719 | if ( res < 0 ) 720 | return -1; 721 | 722 | // 723 | // Label rawlog 724 | 725 | labelRawlog(); 726 | 727 | return 0; 728 | 729 | } catch (exception &e) 730 | { 731 | cout << " Exception caught: " << e.what() << endl; 732 | return -1; 733 | } 734 | catch (...) 735 | { 736 | printf("Another exception!!"); 737 | return -1; 738 | } 739 | } 740 | -------------------------------------------------------------------------------- /apps/tests.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jotaraul/Object-Labeling-Toolkit/4fc11deea6150cd8f068e7f18e2cda2386008ae9/apps/tests.cpp -------------------------------------------------------------------------------- /apps/visualize_reconstruction.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include "processing.hpp" 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | 49 | using namespace mrpt::utils; 50 | using namespace mrpt::opengl; 51 | using namespace mrpt::obs; 52 | using namespace mrpt::maps; 53 | using namespace mrpt::poses; 54 | using namespace mrpt::math; 55 | using namespace mrpt; 56 | using namespace std; 57 | 58 | // Visualization vbles 59 | mrpt::opengl::COpenGLScenePtr scene; 60 | mrpt::gui::CDisplayWindow3DPtr win3D; 61 | gui::CDisplayWindowPlotsPtr win; 62 | 63 | // Configuration vbles 64 | string i_rawlogFileName; // Rawlog file name 65 | string i_sceneFilename; // Rawlog file name 66 | CFileGZInputStream i_rawlog; // Rawlog file stream 67 | bool stepByStepExecution = false; // Enables step by step execution 68 | bool clearAfterStep = false; 69 | bool showPoses = false; 70 | double zUpperLimit = std::numeric_limits::max(); 71 | vector sensors_to_use; // Visualize data from this sensors. Empty=all 72 | bool setNumberOfObs = false; 73 | size_t N_limitOfObs = std::numeric_limits::max(); 74 | size_t N_lowerLimitOfObs = 0; 75 | size_t decimate = 0; 76 | float distBetweenPoints = 0.02; 77 | float pointSize = 3; 78 | bool equalizeRGBDHist = false; 79 | bool visualize2Dposes = false; 80 | bool saveAsPlainText = false; 81 | 82 | //----------------------------------------------------------- 83 | // 84 | // showUsageInformation 85 | // 86 | //----------------------------------------------------------- 87 | 88 | void showUsageInformation() 89 | { 90 | cout << " Usage information. At least one expected argument: " << endl << 91 | " (1) Rawlog or scene file." << endl; 92 | 93 | cout << " Then, optional parameters:" << endl << 94 | " -sensor : Use obs. from this sensor (all used by default)." << endl << 95 | " -visualize2Dposes : Visualize sensors' poses in a 2D plot." << endl << 96 | " -decimate : Visualize one of each RGBD observations." << endl << 97 | " -pointSize : Size of the points for visualization purposes (default 3)." << endl << 98 | " -distBetweenPoints : Min distance between two points to insert the second one." << endl << 99 | " -step : Enable step by step execution." << endl << 100 | " -equalizeRGBDHist : Enable the equalization of the RGB images." << endl << 101 | " -clear : Clear the scene after a step." << endl << 102 | " -poses : Show spheres in the scene representing observation poses." << endl << 103 | " -zUpperLimit : Remove points with a height higher than this paramter." << endl << 104 | " -limit : Sets a limit to the number of obs to process." << endl << 105 | " -lowerLimit : Sets a lower limit to the number of obs to process." << endl << 106 | " -saveAsPlainText : If a scene is loaded, save it as plain text." << endl << endl; 107 | 108 | } 109 | 110 | 111 | //----------------------------------------------------------- 112 | // 113 | // loadParameters 114 | // 115 | //----------------------------------------------------------- 116 | 117 | int loadParameters(int argc, char* argv[]) 118 | { 119 | if ( argc > 1 ) 120 | { 121 | // Get rawlog file name 122 | string fileName = argv[1]; 123 | 124 | if ( !fileName.compare(fileName.size()-7,7,".rawlog") ) 125 | i_rawlogFileName = fileName; 126 | else if ( !fileName.compare(fileName.size()-6,6,".scene") ) 127 | i_sceneFilename = fileName; 128 | else 129 | return -1; 130 | 131 | // Get optional paramteres 132 | if ( argc > 2 ) 133 | { 134 | size_t arg = 2; 135 | 136 | while ( arg < argc ) 137 | { 138 | if ( !strcmp(argv[arg], "-h") ) 139 | { 140 | showUsageInformation(); 141 | arg++; 142 | } 143 | else if ( !strcmp(argv[arg],"-sensor") ) 144 | { 145 | string sensor = argv[arg+1]; 146 | arg += 2; 147 | 148 | sensors_to_use.push_back( sensor ); 149 | 150 | } 151 | else if ( !strcmp(argv[arg],"-saveAsPlainText") ) 152 | { 153 | saveAsPlainText = true; 154 | 155 | cout << " [INFO] Saving as plain text" << endl; 156 | 157 | arg++; 158 | } 159 | else if ( !strcmp(argv[arg],"-decimate") ) 160 | { 161 | decimate = atoi(argv[arg+1]); 162 | arg += 2; 163 | 164 | cout << " [INFO] Employing only 1 of each " << decimate << " obs" << endl; 165 | } 166 | else if ( !strcmp(argv[arg],"-equalizeRGBDHist") ) 167 | { 168 | equalizeRGBDHist = true; 169 | 170 | cout << " [INFO] Equalizing RGB images" << endl; 171 | arg++; 172 | } 173 | else if ( !strcmp(argv[arg],"-visualize2Dposes") ) 174 | { 175 | visualize2Dposes = true; 176 | 177 | cout << " [INFO] Visualizing poses in a 2D plot." << endl; 178 | arg++; 179 | } 180 | else if ( !strcmp(argv[arg],"-pointSize") ) 181 | { 182 | pointSize = atof(argv[arg+1]); 183 | arg += 2; 184 | 185 | cout << " [INFO] Point size set to: " << pointSize << endl; 186 | } 187 | else if ( !strcmp(argv[arg],"-distBetweenPoints") ) 188 | { 189 | distBetweenPoints = atof(argv[arg+1]); 190 | arg += 2; 191 | 192 | cout << " [INFO] Distance between points set to: " << distBetweenPoints << endl; 193 | } 194 | else if ( !strcmp(argv[arg], "-step") ) 195 | { 196 | stepByStepExecution = true; 197 | arg++; 198 | } 199 | else if ( !strcmp(argv[arg], "-clear") ) 200 | { 201 | clearAfterStep = true; 202 | arg++; 203 | 204 | cout << " [INFO] Clearing scene after each step" << endl; 205 | } 206 | else if ( !strcmp(argv[arg], "-poses") ) 207 | { 208 | showPoses = true; 209 | arg++; 210 | } 211 | else if ( !strcmp(argv[arg],"-limit") ) 212 | { 213 | N_limitOfObs = atoi(argv[arg+1]); 214 | setNumberOfObs = true; 215 | arg += 2; 216 | } 217 | else if ( !strcmp(argv[arg],"-lowerLimit") ) 218 | { 219 | N_lowerLimitOfObs = atoi(argv[arg+1]); 220 | arg += 2; 221 | } 222 | else if ( !strcmp(argv[arg],"-zUpperLimit") ) 223 | { 224 | zUpperLimit = atof(argv[arg+1]); 225 | arg += 2; 226 | cout << " [INFO] Upper limit set to: " << zUpperLimit << endl; 227 | } 228 | else 229 | { 230 | cout << " [Error] " << argv[arg] << " unknown paramter." << endl; 231 | showUsageInformation(); 232 | return -1; 233 | } 234 | } 235 | } 236 | } 237 | else 238 | { 239 | showUsageInformation(); 240 | 241 | return -1; 242 | } 243 | } 244 | 245 | 246 | //----------------------------------------------------------- 247 | // 248 | // visualizeScene 249 | // 250 | //----------------------------------------------------------- 251 | 252 | void visualizeScene() 253 | { 254 | if (!mrpt::system::fileExists(i_sceneFilename)) 255 | { 256 | cout << " [ERROR] Couldn't open scene dataset file " << 257 | i_sceneFilename << endl; 258 | return; 259 | } 260 | 261 | // Get scene name 262 | vector tokens; 263 | mrpt::system::tokenize(i_sceneFilename,"/",tokens); 264 | 265 | win3D = gui::CDisplayWindow3DPtr( new gui::CDisplayWindow3D() ); 266 | win3D->setWindowTitle(format("Visualizing %s",tokens[tokens.size()-1].c_str())); 267 | 268 | scene = win3D->get3DSceneAndLock(); 269 | 270 | if (scene->loadFromFile(i_sceneFilename)) 271 | cout << " [INFO] Scene " << i_sceneFilename << " loaded" << endl; 272 | else 273 | cout << " [INFO] Error while loading scene " << i_sceneFilename << endl; 274 | 275 | win3D->unlockAccess3DScene(); 276 | win3D->forceRepaint(); 277 | } 278 | 279 | 280 | //----------------------------------------------------------- 281 | // 282 | // buildScene 283 | // 284 | //----------------------------------------------------------- 285 | 286 | void buildScene() 287 | { 288 | // 289 | // Check the input rawlog file 290 | 291 | if (!mrpt::system::fileExists(i_rawlogFileName)) 292 | { 293 | cout << " [ERROR] Couldn't open rawlog dataset file " << 294 | i_rawlogFileName << endl; 295 | return; 296 | } 297 | 298 | i_rawlog.open(i_rawlogFileName); 299 | 300 | cout << " [INFO] Working with " << i_rawlogFileName << endl; 301 | 302 | if ( sensors_to_use.empty() ) 303 | cout << " [INFO] Visualizing observations from any sensor." << endl; 304 | else 305 | { 306 | cout << " [INFO] Visualizing observations from: "; 307 | for ( size_t i_sensor = 0; i_sensor < sensors_to_use.size(); i_sensor++ ) 308 | cout << sensors_to_use[i_sensor] << " "; 309 | cout << endl; 310 | } 311 | 312 | // 313 | // Set 3D window and visualization objects 314 | 315 | // Get rawlog name 316 | vector tokens; 317 | mrpt::system::tokenize(i_rawlogFileName,"/",tokens); 318 | 319 | win3D = gui::CDisplayWindow3DPtr( new gui::CDisplayWindow3D() ); 320 | win3D->setWindowTitle(format("Building reconstruction visualization of %s",tokens[tokens.size()-1].c_str())); 321 | 322 | win3D->resize(400,300); 323 | 324 | win3D->setCameraAzimuthDeg(140); 325 | win3D->setCameraElevationDeg(20); 326 | win3D->setCameraZoom(6.0); 327 | win3D->setCameraPointingToPoint(2.5,0,0); 328 | 329 | scene = win3D->get3DSceneAndLock(); 330 | 331 | opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-7,7,-7,7,0,1); 332 | obj->setColor(0.7,0.7,0.7); 333 | obj->setLocation(0,0,0); 334 | scene->insert( obj ); 335 | 336 | mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create(); 337 | gl_points->setPointSize(pointSize); 338 | CColouredPointsMap colouredMap; 339 | colouredMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage; 340 | colouredMap.insertionOptions.minDistBetweenLaserPoints = distBetweenPoints; 341 | //colouredMap.insertionOptions.fuseWithExisting = true; 342 | 343 | scene->insert( gl_points ); 344 | 345 | win3D->unlockAccess3DScene(); 346 | 347 | // 348 | // Set 2D window 349 | 350 | if ( visualize2Dposes ) 351 | { 352 | win = gui::CDisplayWindowPlotsPtr( new gui::CDisplayWindowPlots("2D poses localization") ); 353 | win->hold_on(); 354 | } 355 | 356 | // 357 | // Let's go! 358 | 359 | mrpt::system::sleep(3000); 360 | 361 | size_t N_inserted_point_clouds = 0; 362 | 363 | // 364 | // Iterate over the obs into the rawlog and show them in the 3D/2D windows 365 | 366 | cout << " [INFO] Showing observations from " << N_lowerLimitOfObs << " up to "; 367 | 368 | if ( N_limitOfObs == std::numeric_limits::max() ) 369 | cout << "the end"<< endl; 370 | else 371 | cout << N_limitOfObs << endl; 372 | 373 | 374 | CActionCollectionPtr action; 375 | CSensoryFramePtr observations; 376 | CObservationPtr obs; 377 | size_t obsIndex = N_lowerLimitOfObs; 378 | 379 | while (( obsIndex < N_limitOfObs ) && 380 | ( CRawlog::getActionObservationPairOrObservation(i_rawlog,action, 381 | observations,obs,obsIndex) )) 382 | { 383 | // Check that it is a 3D observation 384 | if ( !IS_CLASS(obs, CObservation3DRangeScan) ) 385 | continue; 386 | 387 | // Using information from this sensor? 388 | if ( !sensors_to_use.empty() 389 | && find(sensors_to_use.begin(), sensors_to_use.end(),obs->sensorLabel) 390 | == sensors_to_use.end() ) 391 | continue; 392 | 393 | if ( decimate && ( obsIndex%decimate) ) 394 | continue; 395 | 396 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 397 | obs3D->load(); 398 | 399 | if ( !obs3D->hasPoints3D ) 400 | obs3D->project3DPointsFromDepthImage(); 401 | 402 | if (equalizeRGBDHist) 403 | obs3D->intensityImage.equalizeHistInPlace(); 404 | 405 | CPose3D pose; 406 | obs3D->getSensorPose( pose ); 407 | cout << " Sensor " << obs3D->sensorLabel << " index " 408 | << obsIndex << " pose: " << pose << endl; 409 | 410 | // Clear previous point clouds? 411 | if ( clearAfterStep ) 412 | { 413 | size_t index = std::distance(sensors_to_use.begin(), 414 | find(sensors_to_use.begin(), 415 | sensors_to_use.end(),obs->sensorLabel)); 416 | if ( !index ) 417 | { 418 | colouredMap.clear(); 419 | colouredMap.loadFromRangeScan( *obs3D ); 420 | } 421 | } 422 | else 423 | { 424 | CColouredPointsMap localColouredMap; 425 | localColouredMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage; 426 | localColouredMap.insertionOptions.minDistBetweenLaserPoints = distBetweenPoints; 427 | localColouredMap.loadFromRangeScan( *obs3D ); 428 | 429 | colouredMap.addFrom( localColouredMap ); 430 | } 431 | 432 | size_t N_points = colouredMap.size(); 433 | cout << " Points in the map: " << N_points << endl; 434 | 435 | CVectorDouble coords,x,y; 436 | pose.getAsVector( coords ); 437 | x.push_back( coords[0] ); 438 | y.push_back( coords[1] ); 439 | CPoint3D point((double)coords[0], (double)coords[1], (double)coords[2]); 440 | 441 | if ( visualize2Dposes ) 442 | { 443 | // Plot sensor pose into the 2D window 444 | win->plot(x,y,"b.4"); 445 | } 446 | 447 | // Plot point cloud into the 3D window 448 | scene = win3D->get3DSceneAndLock(); 449 | 450 | gl_points->loadFromPointsMap( &colouredMap ); 451 | 452 | // Remove points with a z higher than a given one 453 | for ( size_t i = 0; i < N_points; i++ ) 454 | if ( gl_points->getPointf(i).z > zUpperLimit ) 455 | gl_points->setPoint_fast(i,0,0,0); 456 | 457 | // Show spheres representing the observation poses? 458 | if ( showPoses ) 459 | { 460 | mrpt::opengl::CSpherePtr sphere = mrpt::opengl::CSphere::Create(0.02); 461 | sphere->setPose(point); 462 | scene->insert( sphere ); 463 | } 464 | 465 | N_inserted_point_clouds++; 466 | 467 | win3D->unlockAccess3DScene(); 468 | win3D->repaint(); 469 | 470 | // Step by step execution? 471 | if ( stepByStepExecution ) 472 | win3D->waitForKey(); 473 | } 474 | 475 | cout << " [INFO] Number of points clouds in the scene: " << N_inserted_point_clouds << endl; 476 | } 477 | 478 | 479 | //----------------------------------------------------------- 480 | // 481 | // saveSceneToFile 482 | // 483 | //----------------------------------------------------------- 484 | 485 | void saveSceneToFile() 486 | { 487 | string sceneFile; 488 | sceneFile.assign( i_rawlogFileName.begin(), i_rawlogFileName.end()-7 ); 489 | sceneFile += ".scene"; 490 | 491 | cout << " [INFO] Saving to scene file " << sceneFile; 492 | cout.flush(); 493 | 494 | scene->saveToFile( sceneFile ); 495 | cout << " ... done" << endl; 496 | 497 | } 498 | 499 | 500 | //----------------------------------------------------------- 501 | // 502 | // main 503 | // 504 | //----------------------------------------------------------- 505 | 506 | int main(int argc, char* argv[]) 507 | { 508 | cout << endl << "-----------------------------------------------------" << endl; 509 | cout << " Visualize reconstruction app. " << endl; 510 | cout << " [Object Labeling Tookit] " << endl; 511 | cout << "-----------------------------------------------------" << endl << endl; 512 | 513 | try 514 | { 515 | // 516 | // Load configuration params 517 | 518 | int res = loadParameters(argc,argv); 519 | 520 | if ( res < 0 ) 521 | return -1; 522 | if ( !res ) 523 | return 0; 524 | 525 | // 526 | // Visualize scene 527 | 528 | if ( !i_rawlogFileName.empty() ) 529 | { 530 | buildScene(); 531 | 532 | // 533 | // Decide if save the scene or not 534 | 535 | if ( !win3D.null() ) 536 | { 537 | cout << " [INFO] Press 's' to save the scene or other key to end the program." << endl; 538 | while ( win3D->isOpen() && !win3D->keyHit() ) 539 | mrpt::system::sleep(10); 540 | 541 | int key = win3D->getPushedKey(); 542 | 543 | // 544 | // Save the built scene to file 545 | if (( key == 's' ) || ( key == 'S')) 546 | saveSceneToFile(); 547 | } 548 | } 549 | else if ( !i_sceneFilename.empty() ) 550 | { 551 | 552 | if ( saveAsPlainText ) 553 | { 554 | string withoutExtension = i_sceneFilename.substr(0,i_sceneFilename.size()-6); 555 | 556 | OLT::CSaveAsPlainText editor; 557 | 558 | editor.setOption("output_file",withoutExtension+"_scene.txt"); 559 | 560 | editor.setInputScene(i_sceneFilename); 561 | editor.process(); 562 | } 563 | else 564 | { 565 | visualizeScene(); 566 | 567 | while ( win3D->isOpen() && !win3D->keyHit() ) 568 | mrpt::system::sleep(10); 569 | } 570 | } 571 | 572 | return 0; 573 | 574 | } catch (exception &e) 575 | { 576 | cout << "Exception caught: " << e.what() << endl; 577 | return -1; 578 | } 579 | catch (...) 580 | { 581 | printf("Another exception!!"); 582 | return -1; 583 | } 584 | } 585 | -------------------------------------------------------------------------------- /libs/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | #---------------------------------------------------------------------------# 3 | # Object Labeling Toolkit # 4 | # A set of software components for the management and # 5 | # labeling of RGB-D datasets # 6 | # # 7 | # Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento # 8 | # University of Malaga # 9 | # MAPIR Group: # 10 | # # 11 | # This program is free software: you can redistribute it and/or modify # 12 | # it under the terms of the GNU General Public License as published by # 13 | # the Free Software Foundation, either version 3 of the License, or # 14 | # (at your option) any later version. # 15 | # # 16 | # This program is distributed in the hope that it will be useful, # 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of # 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # 19 | # GNU General Public License for more details. # 20 | # # 21 | # # 22 | #---------------------------------------------------------------------------# 23 | 24 | 25 | SET ( CORE_LIB_NAME core ) 26 | 27 | # TARGET: 28 | FILE(GLOB aux_srcs1 ${CMAKE_SOURCE_DIR}/libs/${CORE_LIB_NAME}/*.cpp) 29 | FILE(GLOB aux_srcs2 ${CMAKE_SOURCE_DIR}/libs/${CORE_LIB_NAME}/*.hpp) 30 | 31 | SET(SRCS ${aux_srcs1} ${aux_srcs2}) 32 | 33 | ADD_LIBRARY(${CORE_LIB_NAME} SHARED ${SRCS}) 34 | 35 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs) 36 | 37 | install(TARGETS ${CORE_LIB_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) 38 | install(FILES ${aux_srcs2} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${CORE_LIB_NAME} ) 39 | 40 | -------------------------------------------------------------------------------- /libs/core/core.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jotaraul/Object-Labeling-Toolkit/4fc11deea6150cd8f068e7f18e2cda2386008ae9/libs/core/core.cpp -------------------------------------------------------------------------------- /libs/core/core.hpp: -------------------------------------------------------------------------------- 1 | #include "iostream" 2 | -------------------------------------------------------------------------------- /libs/labeling/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | #---------------------------------------------------------------------------# 3 | # Object Labeling Toolkit # 4 | # A set of software components for the management and # 5 | # labeling of RGB-D datasets # 6 | # # 7 | # Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento # 8 | # University of Malaga # 9 | # MAPIR Group: # 10 | # # 11 | # This program is free software: you can redistribute it and/or modify # 12 | # it under the terms of the GNU General Public License as published by # 13 | # the Free Software Foundation, either version 3 of the License, or # 14 | # (at your option) any later version. # 15 | # # 16 | # This program is distributed in the hope that it will be useful, # 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of # 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # 19 | # GNU General Public License for more details. # 20 | # # 21 | # # 22 | #---------------------------------------------------------------------------# 23 | 24 | 25 | SET ( LABELING_LIB_NAME labeling ) 26 | 27 | # TARGET: 28 | FILE(GLOB aux_srcs1 ${CMAKE_SOURCE_DIR}/libs/${LABELING_LIB_NAME}/*.cpp) 29 | FILE(GLOB aux_srcs2 ${CMAKE_SOURCE_DIR}/libs/${LABELING_LIB_NAME}/*.hpp) 30 | 31 | SET(SRCS ${aux_srcs1} ${aux_srcs2}) 32 | 33 | ADD_LIBRARY(${LABELING_LIB_NAME} SHARED ${SRCS}) 34 | 35 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs) 36 | 37 | TARGET_LINK_LIBRARIES(${LABELING_LIB_NAME} core) 38 | 39 | install(TARGETS ${LABELING_LIB_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) 40 | install(FILES ${aux_srcs2} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${LABELING_LIB_NAME} ) 41 | 42 | -------------------------------------------------------------------------------- /libs/labeling/todo.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jotaraul/Object-Labeling-Toolkit/4fc11deea6150cd8f068e7f18e2cda2386008ae9/libs/labeling/todo.cpp -------------------------------------------------------------------------------- /libs/mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | #---------------------------------------------------------------------------# 3 | # Object Labeling Toolkit # 4 | # A set of software components for the management and # 5 | # labeling of RGB-D datasets # 6 | # # 7 | # Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento # 8 | # University of Malaga # 9 | # MAPIR Group: # 10 | # # 11 | # This program is free software: you can redistribute it and/or modify # 12 | # it under the terms of the GNU General Public License as published by # 13 | # the Free Software Foundation, either version 3 of the License, or # 14 | # (at your option) any later version. # 15 | # # 16 | # This program is distributed in the hope that it will be useful, # 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of # 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # 19 | # GNU General Public License for more details. # 20 | # # 21 | # # 22 | #---------------------------------------------------------------------------# 23 | 24 | 25 | SET ( MAPPING_LIB_NAME mapping ) 26 | 27 | # TARGET: 28 | FILE(GLOB aux_srcs1 ${CMAKE_SOURCE_DIR}/libs/${MAPPING_LIB_NAME}/*.cpp) 29 | FILE(GLOB aux_srcs2 ${CMAKE_SOURCE_DIR}/libs/${MAPPING_LIB_NAME}/*.hpp) 30 | 31 | SET(SRCS ${aux_srcs1} ${aux_srcs2}) 32 | 33 | ADD_LIBRARY(${MAPPING_LIB_NAME} SHARED ${SRCS}) 34 | 35 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs) 36 | 37 | TARGET_LINK_LIBRARIES(${MAPPING_LIB_NAME} core) 38 | 39 | install(TARGETS ${MAPPING_LIB_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) 40 | install(FILES ${aux_srcs2} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${MAPPING_LIB_NAME} ) 41 | 42 | -------------------------------------------------------------------------------- /libs/mapping/todo.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jotaraul/Object-Labeling-Toolkit/4fc11deea6150cd8f068e7f18e2cda2386008ae9/libs/mapping/todo.cpp -------------------------------------------------------------------------------- /libs/processing/CAnalyzer.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include "CAnalyzer.hpp" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | 40 | using namespace OLT; 41 | using namespace std; 42 | 43 | using namespace mrpt; 44 | using namespace mrpt::obs; 45 | using namespace mrpt::math; 46 | using namespace mrpt::utils; 47 | using namespace mrpt::opengl; 48 | using namespace mrpt::poses; 49 | 50 | 51 | int CDepthInfoAnalyzer::process(vector &results) 52 | { 53 | if ( m_iRawlog.is_open() ) 54 | return processRawlog(results); 55 | else 56 | return processScene(results); 57 | } 58 | 59 | int CDepthInfoAnalyzer::processRawlog(vector &results) 60 | { 61 | cout << " [INFO] Analyzing depth information." << endl; 62 | 63 | double maxValue = 0; 64 | double minValue = std::numeric_limits::max(); 65 | double meanValue=0; 66 | double N_obs = 0; 67 | 68 | // 69 | // Process rawlog 70 | // 71 | 72 | CActionCollectionPtr action; 73 | CSensoryFramePtr observations; 74 | CObservationPtr obs; 75 | size_t obsIndex = 0; 76 | 77 | cout << " Process: "; 78 | cout.flush(); 79 | 80 | while ( CRawlog::getActionObservationPairOrObservation(m_iRawlog,action,observations,obs,obsIndex) ) 81 | { 82 | // Show progress as dots 83 | 84 | if ( !(obsIndex % 200) ) 85 | { 86 | if ( !(obsIndex % 1000) ) cout << "+ "; else cout << ". "; 87 | cout.flush(); 88 | } 89 | 90 | if (IS_CLASS(obs, CObservation3DRangeScan)) 91 | { 92 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 93 | obs3D->load(); 94 | 95 | if ( obs3D->hasRangeImage ) 96 | { 97 | double max = obs3D->rangeImage.maxCoeff(); 98 | double min = obs3D->rangeImage.minCoeff(); 99 | double mean = obs3D->rangeImage.mean(); 100 | 101 | if ( max > maxValue ) 102 | maxValue = max; 103 | if ( minValue < min ) 104 | min = minValue; 105 | 106 | meanValue += mean; 107 | N_obs++; 108 | } 109 | } 110 | } 111 | 112 | results.push_back( maxValue ); 113 | results.push_back( minValue ); 114 | results.push_back( meanValue / N_obs ); 115 | 116 | cout << endl << " [INFO] Done!" << endl << endl; 117 | 118 | return 1; 119 | } 120 | 121 | int CDepthInfoAnalyzer::processScene(vector &results) 122 | { 123 | // TODO (under request) 124 | return 1; 125 | } 126 | 127 | -------------------------------------------------------------------------------- /libs/processing/CAnalyzer.hpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #ifndef _OLT_ANALYZER_ 24 | #define _OLT_ANALYZER_ 25 | 26 | #include "core.hpp" 27 | 28 | #include "map" 29 | #include 30 | #include 31 | #include 32 | 33 | 34 | namespace OLT 35 | { 36 | class CAnalyzer 37 | { 38 | 39 | protected: 40 | 41 | mrpt::utils::CFileGZInputStream m_iRawlog; 42 | mrpt::opengl::COpenGLScene m_scene; 43 | std::map m_optionsD; 44 | std::map m_optionsS; 45 | 46 | public: 47 | 48 | int setInputRawlog(const std::string &i_rawlogName) 49 | { 50 | if (!mrpt::system::fileExists(i_rawlogName)) 51 | { 52 | std::cerr << " [ERROR] A rawlog file with name " << i_rawlogName; 53 | std::cerr << " doesn't exist." << std::endl; 54 | return 0; 55 | } 56 | else 57 | std::cout << " [INFO] Processing rawlog " << i_rawlogName << std::endl; 58 | 59 | m_iRawlog.open(i_rawlogName); 60 | 61 | return 1; 62 | } 63 | 64 | int setInputScene(const std::string &i_sceneName) 65 | { 66 | if (!mrpt::system::fileExists(i_sceneName)) 67 | { 68 | std::cerr << " [ERROR] A scene file with name " << i_sceneName; 69 | std::cerr << " doesn't exist." << std::endl; 70 | return 0; 71 | } 72 | else 73 | std::cout << " [INFO] Processing scene " << i_sceneName << std::endl; 74 | 75 | if (!m_scene.loadFromFile(i_sceneName)) 76 | { 77 | std::cerr << " [ERROR] Can't open scene file " << i_sceneName << std::endl; 78 | return 0; 79 | } 80 | 81 | return 1; 82 | } 83 | 84 | void setOption( const std::string option, const double &value){ 85 | m_optionsD[option] = value; 86 | } 87 | 88 | void setOption( const std::string option, const std::string value){ 89 | m_optionsS[option] = value; 90 | } 91 | 92 | virtual int process(std::vector &results) = 0; 93 | }; 94 | 95 | 96 | class CDepthInfoAnalyzer : public CAnalyzer 97 | { 98 | public: 99 | int process(std::vector &results); 100 | private: 101 | int processRawlog(std::vector &results); 102 | int processScene(std::vector &results); 103 | }; 104 | } 105 | 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /libs/processing/CEditor.cpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #include "CEditor.hpp" 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #ifdef USING_OPENCV 38 | #include 39 | #endif 40 | 41 | 42 | using namespace OLT; 43 | using namespace std; 44 | 45 | using namespace mrpt; 46 | using namespace mrpt::obs; 47 | using namespace mrpt::math; 48 | using namespace mrpt::utils; 49 | using namespace mrpt::opengl; 50 | using namespace mrpt::poses; 51 | 52 | 53 | int CSaveAsPlainText::process() 54 | { 55 | if ( m_iRawlog.is_open() ) 56 | return processRawlog(); 57 | else 58 | return processScene(); 59 | } 60 | 61 | int CSaveAsPlainText::processRawlog() 62 | { 63 | cout << " [INFO] Saving as human readable " << endl; 64 | string outputFile = "."; 65 | string outputObsDir = "observations"; 66 | 67 | if (m_optionsS.count("output_file")) 68 | outputFile = m_optionsS["output_file"]; 69 | 70 | 71 | if (m_optionsS.count("output_obs_dir")) 72 | outputObsDir = m_optionsS["output_obs_dir"]; 73 | 74 | cout << " [INFO] Creating directory " << outputObsDir << endl; 75 | mrpt::system::createDirectory(outputObsDir); 76 | 77 | // 78 | // Process rawlog 79 | // 80 | 81 | CActionCollectionPtr action; 82 | CSensoryFramePtr observations; 83 | CObservationPtr obs; 84 | size_t obsIndex = 0; 85 | 86 | cout << " [INFO] Opening sequence file " << outputFile << endl; 87 | 88 | cout << " Process: "; 89 | cout.flush(); 90 | 91 | ofstream sequenceFile(outputFile.c_str()); 92 | 93 | if (!sequenceFile.is_open()) 94 | { 95 | cerr << " [ERROR] Can't open file " << outputFile << endl; 96 | return 0; 97 | } 98 | 99 | sequenceFile << "# This file contains information about the (sequence of) observations in the directory with the same name. \n" 100 | "# Its format is: \n" 101 | "# [Observation_id] [sensor_label] [sensor_pose_x] [sensor_pose_y] [sensor_pose_z] [sensor_pose_yaw] [sensor_pose_pitch] [sensor_pose_roll] [time-stamp] \n" 102 | "# Units for the sensor pose are meters and radians. The tiem-stamp holds the the number of 100-nanosecond intervals since January 1, 1601 (UTC). \n"; 103 | 104 | while ( CRawlog::getActionObservationPairOrObservation(m_iRawlog,action,observations,obs,obsIndex) ) 105 | { 106 | // Show progress as dots 107 | 108 | if ( !(obsIndex % 200) ) 109 | { 110 | if ( !(obsIndex % 1000) ) cout << "+ "; else cout << ". "; 111 | cout.flush(); 112 | } 113 | 114 | // Get info about the obs to be stored in the sequence file 115 | 116 | TPose3D pose; 117 | obs->getSensorPose(pose); 118 | vector v_pose; 119 | pose.getAsVector(v_pose); 120 | 121 | // Store obs info in the sequence file 122 | 123 | sequenceFile << obsIndex << " " << obs->sensorLabel << " "; 124 | sequenceFile << v_pose[0] << " " << v_pose[1] << " " << v_pose[2] << " " 125 | << v_pose[3] << " " << v_pose[4] << " " << v_pose[5] << " "; 126 | sequenceFile << obs->timestamp << endl; 127 | 128 | string fileName; 129 | 130 | if (IS_CLASS(obs, CObservation2DRangeScan)) 131 | { 132 | 133 | CObservation2DRangeScanPtr obs2D = CObservation2DRangeScanPtr(obs); 134 | 135 | ofstream file; 136 | 137 | fileName = format("%s%lu_scan.txt",outputObsDir.c_str(),obsIndex); 138 | file.open(fileName.c_str()); 139 | 140 | 141 | file << "# This file contains a 2D laser scan observation. \n" 142 | "# Its format is: \n" 143 | "# [aperture] [max_range] [number_of_scans] \n" 144 | "# [vector_of_scans] \n" 145 | "# [vector_of_valid_scans] \n" 146 | "# The aperture of the sensor is in radians, the maximum range \n" 147 | "# and measurements are in meters. '1' means a valid scan, '0' otherwise.\n"; 148 | 149 | 150 | file << obs2D->aperture << endl; 151 | file << obs2D->maxRange << endl; 152 | file << obs2D->scan.size() << endl; 153 | 154 | for ( size_t i = 0; i < obs2D->scan.size(); i++ ) 155 | file << obs2D->scan[i] << " "; 156 | 157 | file << endl; 158 | 159 | for ( size_t i = 0; i < obs2D->validRange.size(); i++ ) 160 | ( !obs2D->validRange[i] ) ? file << "0 " : file << "1 "; 161 | 162 | file << endl; 163 | 164 | file.close(); 165 | } 166 | else if (IS_CLASS(obs, CObservation3DRangeScan)) 167 | { 168 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); 169 | obs3D->load(); 170 | 171 | ofstream file; 172 | 173 | if ( m_optionsD["generate_point_clouds"] ) 174 | { 175 | if ( !obs3D->hasPoints3D ) 176 | obs3D->project3DPointsFromDepthImage(); 177 | 178 | fileName = format("%s%lu_points.txt",outputObsDir.c_str(),obsIndex); 179 | file.open(fileName.c_str()); 180 | 181 | for (size_t i = 0; i < obs3D->points3D_x.size(); i++ ) 182 | { 183 | file << trunc(1000 * obs3D->points3D_x[i]) / 1000 << 184 | " " << trunc(1000 * obs3D->points3D_y[i]) / 1000 << 185 | " " << trunc(1000 * obs3D->points3D_z[i]) / 1000 << endl; 186 | } 187 | 188 | file.close(); 189 | } 190 | 191 | if ( obs3D->hasPixelLabels() ) 192 | { 193 | int N_cols, N_rows; 194 | obs3D->pixelLabels->getSize(N_rows,N_cols); 195 | 196 | file << N_rows << endl; 197 | file << N_cols << endl; 198 | 199 | fileName = format("%s%lu_labels.txt",outputObsDir.c_str(),obsIndex); 200 | file.open(fileName.c_str()); 201 | 202 | file << "#This file contains a labelled matrix/mask, associated with the RGB-D images with the same id. \n" 203 | "#Its format is: \n" 204 | "#[number_of_labels/categories] \n" 205 | "#[label_id_0] [label_string_0] \n" 206 | "# ... \n" 207 | "#[label_id_n] [label_string_n] \n" 208 | "#[number_of_rows_of_the_matrix] \n" 209 | "#[number_of_cols_of_the_matrix] \n" 210 | "#[labelled_matrix/mask] \n" 211 | "#The elements/pixels of the matrix must be interpreted as string of bites, \n" 212 | "#so a bit with value 1 means that the element belongs to the label \n" 213 | "#which label_id matchs the bit position. For example, if an element has \n" 214 | "#the value 1010, this means that is belongs to the labels with id 1 and 3. \n" 215 | "#In this way an element can belong to more than one label. \n"; 216 | 217 | 218 | std::map::iterator it; 219 | 220 | file << obs3D->pixelLabels->pixelLabelNames.size() << endl; 221 | 222 | for ( it = obs3D->pixelLabels->pixelLabelNames.begin(); 223 | it != obs3D->pixelLabels->pixelLabelNames.end(); it++ ) 224 | { 225 | file << it->first << " " << it->second << endl; 226 | } 227 | 228 | 229 | 230 | for ( int row = 0; row < N_rows; row++ ) 231 | { 232 | for ( int col = 0; col < N_cols; col++ ) 233 | { 234 | uint64_t labels; 235 | obs3D->pixelLabels->getLabels(row,col,labels); 236 | file << static_cast(labels) << " "; 237 | } 238 | 239 | file << endl; 240 | } 241 | 242 | file.close(); 243 | } 244 | 245 | 246 | if ( obs3D->hasRangeImage ) 247 | { 248 | 249 | int N_rows = obs3D->rangeImage.rows(); 250 | int N_cols = obs3D->rangeImage.cols(); 251 | 252 | #ifdef USING_OPENCV 253 | { 254 | uint16_t max = numeric_limits::max(); 255 | 256 | double maxValue = 10; 257 | double factor = static_cast(max)/maxValue; 258 | 259 | cv::Mat img(N_rows,N_cols, CV_16UC1); 260 | 261 | for ( int row = 0; row < N_rows; row++ ) 262 | for ( int col = 0; col < N_cols; col++ ) 263 | { 264 | double value = obs3D->rangeImage(row,col); 265 | uint16_t finalValue = static_cast(value*factor); 266 | 267 | img.at(row,col) = finalValue; 268 | } 269 | 270 | fileName = format("%s%lu_depth.png",outputObsDir.c_str(),obsIndex); 271 | vector compression_params; 272 | compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION); 273 | compression_params.push_back(0); 274 | 275 | cv::imwrite(fileName,img,compression_params); 276 | 277 | // Load to check that it's ok 278 | //cv::Mat image = cv::imread(fileName,CV_LOAD_IMAGE_UNCHANGED); 279 | } 280 | #else 281 | { 282 | 283 | 284 | fileName = format("%s%lu_depth.txt",outputObsDir.c_str(),obsIndex); 285 | file.open(fileName.c_str()); 286 | 287 | file << "#This file contains the depth image associated with the RGB image with the same id. \n" 288 | "#Its format is: \n" 289 | "#[number_of_rows] \n" 290 | "#[number_of_cols] \n" 291 | "#[matrix_of_depths] \n" 292 | "#Units are meters \n"; 293 | 294 | for ( int row = 0; row < N_rows; row++ ) 295 | { 296 | for ( int col = 0; col < N_cols; col++ ) 297 | { 298 | file << trunc(1000 * obs3D->rangeImage(row,col) )/ 1000 << " "; 299 | } 300 | 301 | file << endl; 302 | } 303 | 304 | file.close(); 305 | } 306 | #endif 307 | 308 | //obs3D->rangeImage.saveToTextFile(format("./external/%d_depth.txt",obsIndex),mrpt::math::MATRIX_FORMAT_FIXED);//_convertToExternalStorage(format("./external/%d_depth.txt",obsIndex),"./"); 309 | if ( obs3D->hasIntensityImage ) 310 | { 311 | fileName = format("%s%lu_intensity.png",outputObsDir.c_str(),obsIndex); 312 | obs3D->intensityImage.saveToFile(fileName,100); 313 | } 314 | 315 | } 316 | } 317 | 318 | } 319 | 320 | sequenceFile.close(); 321 | 322 | cout << endl << " [INFO] Done!" << endl << endl; 323 | 324 | return 1; 325 | 326 | } 327 | 328 | int CSaveAsPlainText::processScene() 329 | { 330 | string outputFile = "."; 331 | 332 | if (m_optionsS.count("output_file")) 333 | outputFile = m_optionsS["output_file"]; 334 | 335 | ofstream sceneFile(outputFile.c_str()); 336 | 337 | cout << " [INFO] Saving as human readable in " << outputFile << endl; 338 | 339 | vector v_labels; 340 | vector > v_corners; 341 | vector v_poses; 342 | 343 | // Load previously inserted boxes 344 | bool keepLoading = true; 345 | size_t boxes_inserted = 0; 346 | 347 | while ( keepLoading ) 348 | { 349 | CText3DPtr text = m_scene.getByClass(boxes_inserted); 350 | 351 | if ( !text.null() ) 352 | v_labels.push_back(text->getString()); 353 | else 354 | v_labels.push_back("none"); 355 | 356 | CBoxPtr box = m_scene.getByClass(boxes_inserted); 357 | 358 | if ( box.null() ) 359 | keepLoading = false; 360 | else 361 | { 362 | CPose3D pose = box->getPose(); 363 | 364 | TPoint3D c1,c2; 365 | box->getBoxCorners(c1,c2); 366 | 367 | TPoint3D C111 ( pose + static_cast(TPoint3D(c1.x,c1.y,c1.z)) ); 368 | TPoint3D C222 ( pose + static_cast(TPoint3D(c2.x,c2.y,c2.z)) ); 369 | 370 | vector v; 371 | v.push_back(C111); 372 | v.push_back(C222); 373 | 374 | v_corners.push_back(v); 375 | 376 | v_poses.push_back(pose); 377 | } 378 | 379 | boxes_inserted++; 380 | } 381 | 382 | size_t N_corners = v_corners.size(); 383 | 384 | // Is it a labelled scene? 385 | 386 | if ( N_corners > 0 ) 387 | { 388 | sceneFile << "#This file contains a labelled 3D point cloud of a reconstructed scene. \n" 389 | "#Its format is: \n" 390 | "#[number_of_bounding_boxes/labels] \n" 391 | "#[bounding_box_1_label] \n" 392 | "#[bb1_pose_x] [bb1_pose_y] [bb1_pose_z] [bb1_pose_yaw] [bb1_pose_pitch] [bb1_pose_roll] \n" 393 | "#[bb1_corner1_x] [bb1_corner1_y] [bb1_corner1_z] \n" 394 | "#[bb1_corner2_x] [bb1_corner2_y] [bb1_corner2_z] \n" 395 | "# ..." 396 | "#[bbn_pose_x] [bbn_pose_y] [bbn_pose_z] [bbn_pose_yaw] [bbn_pose_pitch] [bbn_pose_roll] \n" 397 | "#[bbn_corner1_x] [bbn_corner1_y] [bbn_corner1_z] \n" 398 | "#[bbn_corner2_x] [bbn_corner2_y] [bbn_corner2_z] \n" 399 | "#[number_of_points] \n" 400 | "#[point1_x] [point1_y] [point1_z] [point1_color_R] [point1_color_G] [point1_color_B]\n" 401 | "# ..." 402 | "#[pointn_x] [pointn_y] [pointn_z] [pointn_color_R] [pointn_color_G] [pointn_color_B]\n"; 403 | 404 | sceneFile << N_corners << endl; 405 | 406 | for ( size_t i = 0; i < N_corners; i++ ) 407 | { 408 | sceneFile << v_labels[i] << endl; 409 | sceneFile << v_poses[i].x() << " " << v_poses[i].y() << " " << v_poses[i].z() << " " 410 | << v_poses[i].yaw() << " " << v_poses[i].pitch() << " " << v_poses[i].roll() << endl; 411 | sceneFile << v_corners[i][0].x << " " << v_corners[i][0].y << " " << v_corners[i][0].z << endl; 412 | sceneFile << v_corners[i][1].x << " " << v_corners[i][1].y << " " << v_corners[i][1].z << endl; 413 | } 414 | 415 | } 416 | else 417 | { 418 | sceneFile << "#This file contains a 3D point cloud of a reconstructed scene. \n" 419 | "#Its format is: \n" 420 | "#[number_of_points] \n" 421 | "#[point1_x] [point1_y] [point1_z] [point1_color_R] [point1_color_G] [point1_color_B]\n" 422 | "# ... \n" 423 | "#[pointn_x] [pointn_y] [pointn_z] [pointn_color_R] [pointn_color_G] [pointn_color_B]\n"; 424 | } 425 | 426 | CPointCloudColouredPtr gl_points = m_scene.getByClass(0); 427 | size_t N_points = gl_points->size(); 428 | sceneFile << N_points << endl; 429 | 430 | for ( size_t i = 0; i < N_points; i++ ) 431 | { 432 | CPointCloudColoured::TPointColour point = gl_points->getPoint(i); 433 | 434 | sceneFile << point.x << " " << point.y << " " << point.z << " " 435 | << point.R << " " << point.G << " " << point.B << endl; 436 | } 437 | 438 | cout << " [INFO] Done! " << endl << endl; 439 | 440 | return 1; 441 | } 442 | 443 | -------------------------------------------------------------------------------- /libs/processing/CEditor.hpp: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------------------------* 2 | | Object Labeling Toolkit | 3 | | A set of software components for the management and | 4 | | labeling of RGB-D datasets | 5 | | | 6 | | Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento | 7 | | University of Malaga | 8 | | MAPIR Group: | 9 | | | 10 | | This program is free software: you can redistribute it and/or modify | 11 | | it under the terms of the GNU General Public License as published by | 12 | | the Free Software Foundation, either version 3 of the License, or | 13 | | (at your option) any later version. | 14 | | | 15 | | This program is distributed in the hope that it will be useful, | 16 | | but WITHOUT ANY WARRANTY; without even the implied warranty of | 17 | | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 18 | | GNU General Public License for more details. | 19 | | | 20 | | | 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #ifndef _OLT_EDITOR_ 24 | #define _OLT_EDITOR_ 25 | 26 | #include "core.hpp" 27 | 28 | #include "map" 29 | #include 30 | #include 31 | #include 32 | 33 | 34 | namespace OLT 35 | { 36 | class CEditor 37 | { 38 | 39 | protected: 40 | 41 | mrpt::utils::CFileGZInputStream m_iRawlog; 42 | mrpt::opengl::COpenGLScene m_scene; 43 | std::map m_optionsD; 44 | std::map m_optionsS; 45 | 46 | public: 47 | 48 | int setInputRawlog(const std::string &i_rawlogName) 49 | { 50 | if (!mrpt::system::fileExists(i_rawlogName)) 51 | { 52 | std::cerr << " [ERROR] A rawlog file with name " << i_rawlogName; 53 | std::cerr << " doesn't exist." << std::endl; 54 | return 0; 55 | } 56 | else 57 | std::cout << " [INFO] Processing rawlog " << i_rawlogName << std::endl; 58 | 59 | m_iRawlog.open(i_rawlogName); 60 | 61 | return 1; 62 | } 63 | 64 | int setInputScene(const std::string &i_sceneName) 65 | { 66 | if (!mrpt::system::fileExists(i_sceneName)) 67 | { 68 | std::cerr << " [ERROR] A scene file with name " << i_sceneName; 69 | std::cerr << " doesn't exist." << std::endl; 70 | return 0; 71 | } 72 | else 73 | std::cout << " [INFO] Processing scene " << i_sceneName << std::endl; 74 | 75 | if (!m_scene.loadFromFile(i_sceneName)) 76 | { 77 | std::cerr << " [ERROR] Can't open scene file " << i_sceneName << std::endl; 78 | return 0; 79 | } 80 | 81 | return 1; 82 | } 83 | 84 | void setOption( const std::string option, const double &value){ 85 | m_optionsD[option] = value; 86 | } 87 | 88 | void setOption( const std::string option, const std::string value){ 89 | m_optionsS[option] = value; 90 | } 91 | 92 | virtual int process() = 0; 93 | }; 94 | 95 | 96 | class CSaveAsPlainText : public CEditor 97 | { 98 | /** SaveAsPlainText options: 99 | * 'output_file' : directory to store the sequence information file 100 | * 'output_obs_dir': directory where observations have to be stored 101 | * 'generate_point_clouds': generate files with point clouds 102 | */ 103 | public: 104 | int process(); 105 | private: 106 | int processRawlog(); 107 | int processScene(); 108 | }; 109 | } 110 | 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /libs/processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | #---------------------------------------------------------------------------# 3 | # Object Labeling Toolkit # 4 | # A set of software components for the management and # 5 | # labeling of RGB-D datasets # 6 | # # 7 | # Copyright (C) 2015-2016 Jose Raul Ruiz Sarmiento # 8 | # University of Malaga # 9 | # MAPIR Group: # 10 | # # 11 | # This program is free software: you can redistribute it and/or modify # 12 | # it under the terms of the GNU General Public License as published by # 13 | # the Free Software Foundation, either version 3 of the License, or # 14 | # (at your option) any later version. # 15 | # # 16 | # This program is distributed in the hope that it will be useful, # 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of # 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # 19 | # GNU General Public License for more details. # 20 | # # 21 | # # 22 | #---------------------------------------------------------------------------# 23 | 24 | 25 | SET ( PROCESSING_LIB_NAME processing ) 26 | 27 | # TARGET: 28 | FILE(GLOB aux_srcs1 ${CMAKE_SOURCE_DIR}/libs/${PROCESSING_LIB_NAME}/*.cpp) 29 | FILE(GLOB aux_srcs2 ${CMAKE_SOURCE_DIR}/libs/${PROCESSING_LIB_NAME}/*.hpp) 30 | 31 | SET(SRCS ${aux_srcs1} ${aux_srcs2}) 32 | 33 | ADD_LIBRARY(${PROCESSING_LIB_NAME} SHARED ${SRCS}) 34 | 35 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/libs) 36 | 37 | 38 | IF (OLT_USING_OPENCV) 39 | TARGET_LINK_LIBRARIES(${PROCESSING_LIB_NAME} core ${MRPT_LIBS} ${OpenCV_LIBS}) 40 | ELSE (OLT_USING_OPENCV) 41 | TARGET_LINK_LIBRARIES(${PROCESSING_LIB_NAME} core ${MRPT_LIBS} ) 42 | ENDIF (OLT_USING_OPENCV) 43 | 44 | 45 | install(TARGETS ${PROCESSING_LIB_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) 46 | install(FILES ${aux_srcs2} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/${PROCESSING_LIB_NAME} ) 47 | 48 | -------------------------------------------------------------------------------- /libs/processing/processing.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _OLT_PROCESSING_ 3 | #define _OLT_PROCESSING_ 4 | 5 | #include "CEditor.hpp" 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /share/olt/config_files/Dataset_statistics.conf: -------------------------------------------------------------------------------- 1 | [GENERAL] 2 | instancesLabeled = true // Set if the labeled rawlogs has information about object instances, e.g. "table_1" 3 | saveStatisticsToFile = true 4 | statisticsOutputFileName = stats.txt 5 | 6 | labelsToConsider = bass_speaker,bed,bin,book,bowl,cabinet,can,ceiling_lamp,ceiling,cereal_box,chair,clock_alarm,closet,clothes_box,clutter, computer_screen,computer,couch,counter,curtain,cushion,doorframe, door,game_controller,video_console,faucet,floor,freezer,fridge,keyboard,knife,lamp,microwave,milk_bottle,mouse, night_stand,oven,paper_roll,pc,pen_holder,pen_case,picture,pillow,plate,pot,remote_control,scourer,serviette,sink,shelf,shelves,shoes,speaker,spoon,stove, table,telephone,trash_bin,tray,tv_set,tv,wall,window 7 | 8 | [RAWLOGS] // Put here the rawlog files to consider 9 | rawlog1 = example1.rawlog 10 | rawlog2 = example2.rawlog -------------------------------------------------------------------------------- /share/olt/config_files/Label_rawlog.conf: -------------------------------------------------------------------------------- 1 | // Sample configuration file, to be used with the 'Label_rawlog' application 2 | // within the Object Labeling Toolkit (OLT). See https://github.com/jotaraul/Object-Labeling-Toolkit 3 | // for more details 4 | 5 | [GENERAL] 6 | rawlogFile = // Put here the rawlog file to label 7 | labelledScene = // Here, the labeled scene 8 | visualizeLabels = false // true if you want to visually check the assigned labels 9 | instancesLabeled = false // has the scene been labeled with objects' instances? e.g. cup_1, towel_3, etc. 10 | 11 | [LABELS] 12 | labelNames = floor,ceiling,bed,lamp,table,chair,night_stand,pillow,wall,computer_screen,pc,keyboard,door,shelf,shelves,book,mouse,window,curtain,clutter,closet,clock_alarm, lamp,picture,computer,shoes,fridge,oven,cabinet,counter,paper_roll,pot,microwave,bowl,milk_bottle,cereal_box,scourer,faucet,sink,stove,trash_bin,door 13 | -------------------------------------------------------------------------------- /share/olt/config_files/Label_scene.conf: -------------------------------------------------------------------------------- 1 | // Sample configuration file, to be used with the 'Label_scene' application 2 | // within the Object Labeling Toolkit (OLT). See https://github.com/jotaraul/Object-Labeling-Toolkit 3 | // for more details 4 | 5 | [GENERAL] 6 | sceneFile = // Put here the scene file to label 7 | showOnlyLabels = false // if you want to visualize only the boxes inserted in the scene 8 | OFFSET = 0.02; 9 | OFFSET_ANGLES = 0.02; 10 | 11 | [LABELS] 12 | labelNames = floor,ceiling,bed,lamp,table,chair,night_stand,pillow,wall,computer_screen,pc,keyboard,door,shelf,shelves,book,mouse,window,curtain,clutter,closet,clock_alarm, lamp,picture,computer,shoes,fridge,oven,cabinet,counter,paper_roll,pot,microwave,bowl,milk_bottle,cereal_box,scourer,faucet,sink,stove,trash_bin,door 13 | -------------------------------------------------------------------------------- /share/olt/config_files/Process_rawlog.conf: -------------------------------------------------------------------------------- 1 | // Sample configuration file, to be used with the 'Process_rawlog' application 2 | // within the Object Labeling Toolkit (OLT). See https://github.com/jotaraul/Object-Labeling-Toolkit 3 | // for more details 4 | 5 | [GENERAL] 6 | use_default_intrinsics = false 7 | equalize_RGB_histograms = true // equalize the histograms of the RGB images from the RGB-D devices 8 | 9 | [HOKUYO1] 10 | x = 0.205 11 | y = 0 12 | z = 0.31 13 | yaw = 0 14 | pitch = 0 15 | roll = 0 16 | 17 | [RGBD_1] 18 | x = 0.285 19 | y = 0 20 | z = 1.015 21 | yaw = 0 22 | pitch = 1.3 23 | roll = -90 24 | loadIntrinsic = false 25 | 26 | // If you want to include your own intrinsic parameters, set loadIntrinsic to true 27 | // and introduce the parameters in their respective sections. This has to be done for 28 | // each RGB_D device within the dataset, and example for RGBD_1 is shown below. 29 | 30 | [RGBD_2] 31 | x = 0.271 32 | y = -0.031 33 | z = 1.015 34 | yaw = -45 35 | pitch = 0 36 | roll = -90 37 | loadIntrinsic = false 38 | 39 | [RGBD_3] 40 | x = 0.271 41 | y = 0.031 42 | z = 1.125 43 | yaw = 45 44 | pitch = 2 45 | roll = -89 46 | loadIntrinsic = false 47 | 48 | [RGBD_4] 49 | x = 0.24 50 | y = -0.045 51 | z = 0.975 52 | yaw = -90 53 | pitch = 1.5 54 | roll = -90 55 | loadIntrinsic = false 56 | 57 | //[RGBD_1_depth] 58 | //resolution = [NCOLS NROWS] 59 | //cx = CX 60 | //cy = CY 61 | //fx = FX 62 | //fy = FY 63 | //dist = [K1 K2 T1 T2 K3] # Distortion 64 | //focal_length = FOCAL_LENGTH # [optional field] 65 | //[RGBD_1_intensity] 66 | //resolution = [NCOLS NROWS] 67 | //cx = CX 68 | //cy = CY 69 | //fx = FX 70 | //fy = FY 71 | //dist = [K1 K2 T1 T2 K3] # Distortion 72 | //focal_length = FOCAL_LENGTH # [optional field] 73 | -------------------------------------------------------------------------------- /third_party/difodo_multi/CDifodo_multi.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jotaraul/Object-Labeling-Toolkit/4fc11deea6150cd8f068e7f18e2cda2386008ae9/third_party/difodo_multi/CDifodo_multi.cpp -------------------------------------------------------------------------------- /third_party/difodo_multi/CDifodo_multi.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | //#include // Eigen 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define NC 4 // Number of cameras 17 | #define NL 6 // Number of coarse-to-fine levels 18 | 19 | 20 | /** This abstract class implements a method called "Difodo" to perform Visual odometry with range cameras. 21 | * It is based on the range flow equation and assumes that the scene is rigid. 22 | * It can work with different image resolutions (640 x 480, 320 x 240 or 160 x 120) and a different number of 23 | * coarse-to-fine levels which can be adjusted with the member variables (rows,cols,ctf_levels). 24 | * 25 | * How to use: 26 | * - A derived class must be created which defines the method "loadFrame(...)" according to the user application. 27 | * This method has to load the depth image into the variable "depth_wf". 28 | * - Call loadFrame(); 29 | * - Call odometryCalculation(); 30 | * 31 | * For further information have a look at the apps: 32 | * - [DifOdometry-Camera](http://www.mrpt.org/list-of-mrpt-apps/application-difodometry-camera/) 33 | * - [DifOdometry-Datasets](http://www.mrpt.org/list-of-mrpt-apps/application-difodometry-datasets/) 34 | * 35 | * Please refer to the respective publication when using this method: 36 | * title = {Fast Visual Odometry for {3-D} Range Sensors}, 37 | * author = {Jaimez, Mariano and Gonzalez-Jimenez, Javier}, 38 | * journal = {IEEE Transactions on Robotics}, 39 | * volume = {31}, 40 | * number = {4}, 41 | * pages = {809 - 822}, 42 | * year = {2015} 43 | * 44 | * - JUN/2013: First design. 45 | * - JAN/2014: Integrated into MRPT library. 46 | * - DIC/2014: Reformulated and improved. The class now needs Eigen version 3.1.0 or above. 47 | * 48 | * \sa CDifodoCamera, CDifodoDatasets 49 | * \ingroup mrpt_vision_grp 50 | */ 51 | 52 | class CDifodo { 53 | protected: 54 | 55 | /** Matrix that stores the original depth frames with the image resolution */ 56 | Eigen::MatrixXf depth_wf[NC]; 57 | 58 | /** Matrices that store the point coordinates after downsampling. */ 59 | Eigen::MatrixXf depth[NC][NL]; 60 | Eigen::MatrixXf depth_old[NC][NL]; 61 | Eigen::MatrixXf depth_inter[NC][NL]; 62 | Eigen::MatrixXf depth_warped[NC][NL]; 63 | Eigen::MatrixXf zz_global[NC][NL]; 64 | Eigen::MatrixXf xx[NC][NL]; 65 | Eigen::MatrixXf xx_inter[NC][NL]; 66 | Eigen::MatrixXf xx_old[NC][NL]; 67 | Eigen::MatrixXf xx_warped[NC][NL]; 68 | Eigen::MatrixXf xx_global[NC][NL]; 69 | Eigen::MatrixXf yy[NC][NL]; 70 | Eigen::MatrixXf yy_inter[NC][NL]; 71 | Eigen::MatrixXf yy_old[NC][NL]; 72 | Eigen::MatrixXf yy_warped[NC][NL]; 73 | Eigen::MatrixXf yy_global[NC][NL]; 74 | 75 | /** Matrices that store the depth derivatives */ 76 | Eigen::MatrixXf du[NC]; 77 | Eigen::MatrixXf dv[NC]; 78 | Eigen::MatrixXf dt[NC]; 79 | 80 | /** Weights for the range flow constraint equations in the least square solution */ 81 | Eigen::MatrixXf weights[NC]; 82 | 83 | /** Matrix which indicates whether the depth of a pixel is zero (null = 1) or not (null = 00).*/ 84 | Eigen::Matrix null[NC]; 85 | 86 | /** Least squares covariance matrix */ 87 | Eigen::Matrix est_cov; 88 | 89 | /** Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid*/ 90 | bool fast_pyramid; 91 | Eigen::Matrix4f f_mask; 92 | float g_mask[5][5]; 93 | 94 | /** Camera properties: */ 95 | float fovh; //! kai_loc_level; 135 | 136 | /** Last filtered velocity in absolute coordinates */ 137 | Eigen::Matrix kai_abs; 138 | 139 | /** Filtered velocity in local coordinates */ 140 | Eigen::Matrix kai_loc; 141 | Eigen::Matrix kai_loc_old; 142 | 143 | /** Create the gaussian image pyramid according to the number of coarse-to-fine levels */ 144 | void buildCoordinatesPyramid(); 145 | void buildCoordinatesPyramidFast(); 146 | 147 | /** Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level */ 148 | void performWarping(); 149 | 150 | /** Calculate the "average" coordinates of the points observed by the camera between two consecutive frames and find the Null measurements */ 151 | void calculateCoord(); 152 | 153 | /** Calculates the depth derivatives respect to u,v (rows and cols) and t (time) */ 154 | void calculateDepthDerivatives(); 155 | 156 | /** This method computes the weighting fuction associated to measurement and linearization errors */ 157 | void computeWeights(); 158 | 159 | /** The Solver. It buils the overdetermined system and gets the least-square solution. 160 | * It also calculates the least-square covariance matrix */ 161 | void solveOneLevel(); 162 | 163 | /** Method to filter the velocity at each level of the pyramid. */ 164 | void filterLevelSolution(); 165 | 166 | /** Update camera pose and the velocities for the filter */ 167 | void poseUpdate(); 168 | 169 | 170 | public: 171 | 172 | /** Resolution of the images taken by the range camera */ 173 | unsigned int cam_mode; // (1 - 640 x 480, 2 - 320 x 240, 4 - 160 x 120) 174 | 175 | /** Downsample the image taken by the range camera. Useful to reduce the computational burden, 176 | * as this downsampling is applied before the gaussian pyramid is built. It must be used when 177 | the virtual method "loadFrame()" is implemented */ 178 | unsigned int downsample; // (1 - original size, 2 - res/2, 4 - res/4) 179 | 180 | /** Num of valid points after removing null pixels*/ 181 | unsigned int num_valid_points; 182 | 183 | /** Execution time (ms) */ 184 | float execution_time; 185 | 186 | /** Camera poses */ 187 | mrpt::poses::CPose3D cam_pose[NC]; //!< Last camera pose 188 | mrpt::poses::CPose3D cam_oldpose[NC]; //!< Previous camera pose 189 | mrpt::poses::CPose3D global_pose; 190 | mrpt::poses::CPose3D global_oldpose; 191 | 192 | /** This method performs all the necessary steps to estimate the camera velocity once the new image is read, 193 | and updates the camera pose */ 194 | void odometryCalculation(); 195 | 196 | /** Get the rows and cols of the depth image that are considered by the visual odometry method. */ 197 | inline void getRowsAndCols(unsigned int &num_rows, unsigned int &num_cols) const {num_rows = rows; num_cols = cols;} 198 | 199 | /** Get the number of coarse-to-fine levels that are considered by the visual odometry method. */ 200 | inline void getCTFLevels(unsigned int &levels) const {levels = ctf_levels;} 201 | 202 | /** Set the horizontal and vertical field of vision (in degrees) */ 203 | inline void setFOV(float new_fovh, float new_fovv); 204 | 205 | /** Get the horizontal and vertical field of vision (in degrees) */ 206 | inline void getFOV(float &cur_fovh, float &cur_fovv) const {cur_fovh = 57.296*fovh; cur_fovv = 57.296*fovv;} 207 | 208 | /** Get the filter constant-weight of the velocity filter. */ 209 | inline float getSpeedFilterConstWeight() const {return previous_speed_const_weight;} 210 | 211 | /** Get the filter eigen-weight of the velocity filter. */ 212 | inline float getSpeedFilterEigWeight() const {return previous_speed_eig_weight;} 213 | 214 | /** Set the filter constant-weight of the velocity filter. */ 215 | inline void setSpeedFilterConstWeight(float new_cweight) { previous_speed_const_weight = new_cweight;} 216 | 217 | /** Set the filter eigen-weight of the velocity filter. */ 218 | inline void setSpeedFilterEigWeight(float new_eweight) { previous_speed_eig_weight = new_eweight;} 219 | 220 | /** Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local reference frame estimated by the solver (before filtering) */ 221 | inline mrpt::math::CMatrixFloat61 getSolverSolution() const {return kai_loc_level;} 222 | 223 | /** Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame, obtained after filtering */ 224 | inline mrpt::math::CMatrixFloat61 getLastSpeedAbs() const {return kai_abs;} 225 | 226 | /** Get the least-square covariance matrix */ 227 | inline mrpt::math::CMatrixFloat66 getCovariance() const {return est_cov;} 228 | 229 | /** Virtual method to be implemented in derived classes. 230 | * It should be used to load a new depth image into the variable depth_wf */ 231 | virtual void loadFrame() = 0; 232 | 233 | //Constructor. Initialize variables and matrix sizes 234 | CDifodo(); 235 | 236 | }; 237 | 238 | -------------------------------------------------------------------------------- /third_party/difodo_multi/Difodo_multi_datasets.cpp: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #include "Difodo_multi_datasets.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "legend.xpm" 21 | 22 | using namespace Eigen; 23 | using namespace std; 24 | using namespace mrpt; 25 | using namespace mrpt::opengl; 26 | 27 | using namespace mrpt::gui; 28 | using namespace mrpt::obs; 29 | using namespace mrpt::maps; 30 | using namespace mrpt::math; 31 | using namespace mrpt::utils; 32 | using namespace mrpt::poses; 33 | 34 | 35 | 36 | void CDifodoDatasets::loadConfiguration(unsigned int &i_rows, unsigned int &i_cols, 37 | vector &v_poses, 38 | const string &rawlogFileName, 39 | vector &cameras_order, 40 | bool visualizeResults ) 41 | { 42 | visualize_results = visualizeResults; 43 | 44 | fovh = M_PI*62.5/180.0; //Larger FOV because depth is registered with color 45 | fovv = M_PI*48.5/180.0; 46 | 47 | 48 | rows = i_rows; 49 | cols = i_cols; 50 | 51 | cam_mode = 2; 52 | 53 | fast_pyramid = false; 54 | 55 | downsample = 1; // 1 - 640x480, 2 - 320x240, 4 - 160x120 56 | 57 | ctf_levels = 5; 58 | 59 | fast_pyramid = true; 60 | 61 | // Load cameras' extrinsic calibrations 62 | //================================================================== 63 | 64 | cams_oder = cameras_order; 65 | 66 | for ( int c = 0; c < NC; c++ ) 67 | { 68 | cam_pose[c] = v_poses[c]; 69 | CMatrixDouble44 homoMatrix; 70 | cam_pose[c].getHomogeneousMatrix(homoMatrix); 71 | calib_mat[c] = (CMatrixFloat44)homoMatrix.inverse(); 72 | } 73 | 74 | // Open Rawlog File 75 | //================================================================== 76 | if (!dataset.loadFromRawLogFile(rawlogFileName)) 77 | throw std::runtime_error("\nCouldn't open rawlog dataset file for input..."); 78 | 79 | rawlog_count = 0; 80 | 81 | // Set external images directory: 82 | const string imgsPath = CRawlog::detectImagesDirectory(rawlogFileName); 83 | CImage::IMAGES_PATH_BASE = imgsPath; 84 | 85 | // Resize matrices and adjust parameters 86 | //========================================================= 87 | width = 640/(cam_mode*downsample); 88 | height = 480/(cam_mode*downsample); 89 | repr_level = utils::round(log(float(width/cols))/log(2.f)); 90 | 91 | //Resize pyramid 92 | const unsigned int pyr_levels = round(log(float(width/cols))/log(2.f)) + ctf_levels; 93 | 94 | for (unsigned int c=0; cresize(1000,900); 180 | window->setPos(900,0); 181 | window->setCameraZoom(16); 182 | window->setCameraAzimuthDeg(0); 183 | window->setCameraElevationDeg(90); 184 | window->setCameraPointingToPoint(0,0,0); 185 | window->setCameraPointingToPoint(0,0,1); 186 | 187 | scene = window->get3DSceneAndLock(); 188 | 189 | // Lights: 190 | scene->getViewport()->setNumberOfLights(1); 191 | CLight & light0 = scene->getViewport()->getLight(0); 192 | light0.light_ID = 0; 193 | light0.setPosition(0,0,1,1); 194 | 195 | //Grid (ground) 196 | CGridPlaneXYPtr ground = CGridPlaneXY::Create(); 197 | scene->insert( ground ); 198 | 199 | //Reference 200 | CSetOfObjectsPtr reference = stock_objects::CornerXYZ(); 201 | scene->insert( reference ); 202 | 203 | // Cameras and points 204 | //------------------------------------------------------ 205 | 206 | //Cameras 207 | for (unsigned int c=0; csetPose(cam_pose[c] + rel_lenspose); 211 | camera_odo->setColor(0,1,0); 212 | scene->insert( camera_odo ); 213 | 214 | //Frustum 215 | opengl::CFrustumPtr FOV = opengl::CFrustum::Create(0.3, 2, 57.3*fovh, 57.3*fovv, 1.f, true, false); 216 | FOV->setColor(0.7,0.7,0.7); 217 | FOV->setPose(cam_pose[c]); 218 | scene->insert( FOV ); 219 | 220 | //Camera points 221 | CPointCloudColouredPtr cam_points = CPointCloudColoured::Create(); 222 | cam_points->setColor(1,0,0); 223 | cam_points->setPointSize(2); 224 | cam_points->enablePointSmooth(1); 225 | cam_points->setPose(cam_pose[c]); 226 | scene->insert( cam_points ); 227 | } 228 | 229 | 230 | // Trajectories and covariance 231 | //------------------------------------------------------------- 232 | 233 | //Trajectory of the global reference frame 234 | CSetOfLinesPtr traj_lines_odo = CSetOfLines::Create(); 235 | traj_lines_odo->setLocation(0,0,0); 236 | traj_lines_odo->setColor(0,0.6,0); 237 | traj_lines_odo->setLineWidth(6); 238 | scene->insert( traj_lines_odo ); 239 | CPointCloudPtr traj_points_odo = CPointCloud::Create(); 240 | traj_points_odo->setColor(0,0.6,0); 241 | traj_points_odo->setPointSize(4); 242 | traj_points_odo->enablePointSmooth(1); 243 | scene->insert( traj_points_odo ); 244 | 245 | //Trajectory of one camera 246 | CSetOfLinesPtr traj_lines_cam = CSetOfLines::Create(); 247 | traj_lines_cam->setLocation(0,0,0); 248 | traj_lines_cam->setColor(0.6,0,0); 249 | traj_lines_cam->setLineWidth(6); 250 | scene->insert( traj_lines_cam ); 251 | CPointCloudPtr traj_points_cam = CPointCloud::Create(); 252 | traj_points_cam->setColor(0.6,0,0); 253 | traj_points_cam->setPointSize(4); 254 | traj_points_cam->enablePointSmooth(1); 255 | scene->insert( traj_points_cam ); 256 | 257 | //Ellipsoid showing covariance 258 | math::CMatrixFloat33 cov3d = 20.f*est_cov.topLeftCorner(3,3); 259 | CEllipsoidPtr ellip = CEllipsoid::Create(); 260 | ellip->setCovMatrix(cov3d); 261 | ellip->setQuantiles(2.0); 262 | ellip->setColor(1.0, 1.0, 1.0, 0.5); 263 | ellip->enableDrawSolid3D(true); 264 | ellip->setPose(global_pose); 265 | scene->insert( ellip ); 266 | 267 | //User-interface information 268 | utils::CImage img_legend; 269 | img_legend.loadFromXPM(legend_xpm); 270 | COpenGLViewportPtr legend = scene->createViewport("legend"); 271 | legend->setViewportPosition(20, 20, 332, 164); 272 | legend->setImageView(img_legend); 273 | 274 | window->unlockAccess3DScene(); 275 | window->repaint(); 276 | } 277 | 278 | void CDifodoDatasets::updateScene() 279 | { 280 | if ( !visualize_results ) 281 | return; 282 | 283 | CPose3D rel_lenspose(0,-0.022,0,0,0,0); 284 | 285 | scene = window->get3DSceneAndLock(); 286 | 287 | //Reference gt 288 | CSetOfObjectsPtr reference_gt = scene->getByClass(0); 289 | reference_gt->setPose(global_pose); 290 | 291 | //Camera points 292 | for (unsigned int c=0; cgetByClass(c); 295 | cam_points->clear(); 296 | cam_points->setPose(global_pose + cam_pose[c]); 297 | 298 | for (unsigned int y=0; ypush_back(depth[c][repr_level](z,y), xx[c][repr_level](z,y), yy[c][repr_level](z,y), 301 | // 0.f, 0.f, 1.f); 302 | 1.f-sqrt(weights[c](z,y)), sqrt(weights[c](z,y)), 0); 303 | 304 | //DifOdo camera 305 | CBoxPtr camera_odo = scene->getByClass(c); 306 | camera_odo->setPose(global_pose + cam_pose[c] + rel_lenspose); 307 | 308 | //Frustum 309 | CFrustumPtr FOV = scene->getByClass(c); 310 | FOV->setPose(global_pose + cam_pose[c]); 311 | } 312 | 313 | //Global traj lines 314 | CSetOfLinesPtr traj_lines_odo = scene->getByClass(0); 315 | traj_lines_odo->appendLine(global_oldpose.x(), global_oldpose.y(), global_oldpose.z(), global_pose.x(), global_pose.y(), global_pose.z()); 316 | 317 | //Global traj points 318 | CPointCloudPtr traj_points_odo = scene->getByClass(0); 319 | traj_points_odo->insertPoint(global_pose.x(), global_pose.y(), global_pose.z()); 320 | 321 | ////Cam traj lines 322 | //CPose3D new_cam_pose = global_pose + cam_pose[0], old_cam_pose = global_oldpose + cam_pose[0]; 323 | // CSetOfLinesPtr traj_lines_cam = scene->getByClass(1); 324 | // traj_lines_cam->appendLine(old_cam_pose.x(), old_cam_pose.y(), old_cam_pose.z(), new_cam_pose.x(), new_cam_pose.y(), new_cam_pose.z()); 325 | 326 | // //Cam traj points 327 | // CPointCloudPtr traj_points_cam = scene->getByClass(1); 328 | // traj_points_cam->insertPoint(new_cam_pose.x(), new_cam_pose.y(), new_cam_pose.z()); 329 | 330 | //Ellipsoid showing covariance 331 | math::CMatrixFloat33 cov3d = 20.f*est_cov.topLeftCorner(3,3); 332 | CEllipsoidPtr ellip = scene->getByClass(0); 333 | ellip->setCovMatrix(cov3d); 334 | ellip->setPose(global_pose + rel_lenspose); 335 | 336 | window->unlockAccess3DScene(); 337 | window->repaint(); 338 | } 339 | 340 | int CDifodoDatasets::getCameraIndex( int camera ) 341 | { 342 | for ( size_t i = 0; i < NC; i++ ) 343 | if ( camera == cams_oder[i] ) 344 | return i; 345 | } 346 | 347 | void CDifodoDatasets::loadFrame() 348 | { 349 | v_processedObs.clear(); 350 | vector v_obs(NC); // set of obs 351 | vector v_obs_loaded(NC,false); // Track the camera with an obs loaded 352 | 353 | bool loadedFrame = false; 354 | 355 | while ( !loadedFrame && !dataset_finished ) 356 | { 357 | CObservationPtr alfa = dataset.getAsObservation(rawlog_count); 358 | 359 | while (!IS_CLASS(alfa, CObservation3DRangeScan)) 360 | { 361 | rawlog_count++; 362 | if (dataset.size() <= rawlog_count) 363 | { 364 | dataset_finished = true; 365 | return; 366 | } 367 | alfa = dataset.getAsObservation(rawlog_count); 368 | } 369 | 370 | CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(alfa); 371 | 372 | string label = obs3D->sensorLabel; 373 | 374 | size_t cameraIndex = getCameraIndex(atoi(label.substr(label.size()-1,label.size()).c_str())); 375 | 376 | v_obs[cameraIndex] = obs3D; 377 | v_obs_loaded[cameraIndex] = true; 378 | 379 | unsigned int sum = 0; 380 | 381 | for ( size_t i_sensor = 0; i_sensor < NC; i_sensor++ ) 382 | sum += v_obs_loaded[i_sensor]; 383 | 384 | if ( sum == NC ) 385 | { 386 | v_obs_loaded.clear(); 387 | v_obs_loaded.resize(NC,false); 388 | 389 | for ( size_t c = 0; c < NC; c++ ) 390 | { 391 | v_obs[c]->load(); 392 | 393 | const CMatrix range = v_obs[c]->rangeImage; 394 | const unsigned int height = range.getRowCount(); 395 | const unsigned int width = range.getColCount(); 396 | 397 | for (unsigned int j = 0; junload(); 406 | v_processedObs.push_back(v_obs[c]); 407 | } 408 | 409 | loadedFrame = true; 410 | } 411 | 412 | rawlog_count++; 413 | 414 | if (dataset.size() <= rawlog_count) 415 | dataset_finished = true; 416 | } 417 | } 418 | 419 | void CDifodoDatasets::reset() 420 | { 421 | loadFrame(); 422 | if (fast_pyramid) buildCoordinatesPyramidFast(); 423 | else buildCoordinatesPyramid(); 424 | 425 | global_oldpose = global_pose; 426 | } 427 | 428 | void CDifodoDatasets::writeTrajectoryFile() 429 | { 430 | ////Don't take into account those iterations with consecutive equal depth images 431 | //if (abs(dt.sumAll()) > 0) 432 | //{ 433 | // mrpt::math::CQuaternionDouble quat; 434 | // CPose3D auxpose, transf; 435 | // transf.setFromValues(0,0,0,0.5*M_PI, -0.5*M_PI, 0); 436 | 437 | // auxpose = cam_pose - transf; 438 | // auxpose.getAsQuaternion(quat); 439 | // 440 | // char aux[24]; 441 | // sprintf(aux,"%.04f", timestamp_obs); 442 | // f_res << aux << " "; 443 | // f_res << cam_pose[0] << " "; 444 | // f_res << cam_pose[1] << " "; 445 | // f_res << cam_pose[2] << " "; 446 | // f_res << quat(2) << " "; 447 | // f_res << quat(3) << " "; 448 | // f_res << -quat(1) << " "; 449 | // f_res << -quat(0) << endl; 450 | //} 451 | } 452 | 453 | 454 | -------------------------------------------------------------------------------- /third_party/difodo_multi/Difodo_multi_datasets.h: -------------------------------------------------------------------------------- 1 | /* +---------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | http://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file | 6 | | See: http://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See details in http://www.mrpt.org/License | 8 | +---------------------------------------------------------------------------+ */ 9 | 10 | #include "CDifodo_multi.h" 11 | #include // Eigen (with MRPT "plugin" in BaseMatrix<>) 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | class CDifodoDatasets : public CDifodo { 22 | public: 23 | 24 | std::vector v_processedObs; 25 | mrpt::opengl::COpenGLScenePtr scene; //!< Opengl scene 26 | bool visualize_results; 27 | mrpt::gui::CDisplayWindow3DPtr window; 28 | mrpt::obs::CRawlog dataset; 29 | std::ifstream f_gt; 30 | std::ofstream f_res; 31 | std::vector cams_oder; 32 | 33 | 34 | unsigned int repr_level; 35 | unsigned int rawlog_count; 36 | bool first_pose; 37 | bool save_results; 38 | bool dataset_finished; 39 | 40 | /** Constructor. */ 41 | CDifodoDatasets() : CDifodo() 42 | { 43 | visualize_results = false; 44 | save_results = 0; 45 | first_pose = false; 46 | dataset_finished = false; 47 | } 48 | 49 | /** Initialize the visual odometry method and loads the rawlog file */ 50 | void loadConfiguration(unsigned int &i_rows, unsigned int &i_cols, 51 | std::vector &v_poses, 52 | const std::string &rawlogFileName, 53 | std::vector &cameras_order, 54 | bool visualizeResults); 55 | 56 | /** Load the depth image and the corresponding groundtruth pose */ 57 | void loadFrame(); 58 | 59 | /** Create a file to save the trajectory estimates */ 60 | void CreateResultsFile(); 61 | 62 | /** Initialize the opengl scene */ 63 | void initializeScene(); 64 | 65 | /** Update the opengl scene */ 66 | void updateScene(); 67 | 68 | /** A pre-step that should be performed before starting to estimate the camera speed 69 | * As a couple of frames are necessary to estimate the camera motion, this methods loads the first frame 70 | * before any motion can be estimated.*/ 71 | void reset(); 72 | 73 | /** Save the pose estimation following the format of the TUM datasets: 74 | * 75 | * 'timestamp tx ty tz qx qy qz qw' 76 | * 77 | * Please visit http://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats for further details.*/ 78 | void writeTrajectoryFile(); 79 | 80 | /** Returns the index of a camera in the cams_order vector */ 81 | int getCameraIndex( int camera ); 82 | }; 83 | -------------------------------------------------------------------------------- /version_prefix.txt: -------------------------------------------------------------------------------- 1 | 0.2.0 --------------------------------------------------------------------------------