diff --git a/.svn/pristine/05/05b9b05125a32df2c413114fa1e0ea92fda0d134.svn-base b/.svn/pristine/05/05b9b05125a32df2c413114fa1e0ea92fda0d134.svn-base new file mode 100644 index 0000000..629585c --- /dev/null +++ b/.svn/pristine/05/05b9b05125a32df2c413114fa1e0ea92fda0d134.svn-base @@ -0,0 +1,88 @@ +#pragma once + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include + +#include "cvpmd.h" + +#include "slam6d/icp6Dminimizer.h" + +struct PMDFiles { + FILE *i; // intens + FILE *a; // ampl + FILE *p; // 3d pts + FILE *h; // pmd image headers +}; + +struct TrackingSettings { + int winSz; + double quality; + int minFeatures; + int maxFeatures; + int minDist; + IplImage *pyr; + IplImage *pyrPrev; + IplImage *iCamPrev; + CvPoint2D32f *pts[2]; + char *ptsStatus; + int trackingFlags; +}; + +struct PMDCam { + PMD *_pmd; + CvCapture *_capture; + PMDFiles _f; + CvMat *intrinsicPMD; + CvMat *intrinsicCam; + CvMat *distortionPMD; + CvMat *distortionCam; + CvMat *rotation; + CvMat *translation; + // Undistortion maps + IplImage *_mapXCam; + IplImage *_mapYCam; + IplImage *_mapXPMD; + IplImage *_mapYPMD; + // Data + IplImage *_iCamColorUBuffer; + IplImage *iCamColor; + IplImage *iCam; + IplImage *_iCamColorU; + IplImage *iPMDI; + IplImage *_iPMDIU; + IplImage *iPMDA; + IplImage *_iPMDAU; + CvPoint3D32f **pts; + unsigned int timestamp; + unsigned int timestampUsec; + // Pose Estimation + icp6Dminimizer *icp; + int minPts4Pose; + double maxError; + int minConsensusPts; + int savePoses; + // Settings + ImageHeaderInformation *header; + int synchronous; + int hybrid; + int _offlineMode; + int historyLen; + TrackingSettings _track; + double sigmaDepth; + double sigmaColor; + double dpThreshold; + +}; + +PMDCam *initPMDCam(const char *confPath); +int grabData(PMDCam *pmdc); + + diff --git a/.svn/pristine/0d/0d4746413b0fb378be8c4f744fc697b0c70c08d8.svn-base b/.svn/pristine/0d/0d4746413b0fb378be8c4f744fc697b0c70c08d8.svn-base new file mode 100644 index 0000000..9c7bfac --- /dev/null +++ b/.svn/pristine/0d/0d4746413b0fb378be8c4f744fc697b0c70c08d8.svn-base @@ -0,0 +1,491 @@ +cmake_minimum_required (VERSION 2.8.2) +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH}) +project (3DTK) + + + + +#include_directories(OPENGL_INCLUDE_DIR) +IF(WIN32) + set(Boost_USE_STATIC_LIBS TRUE) +ELSE(WIN32) + set(Boost_USE_STATIC_LIBS FALSE) +ENDIF(WIN32) + +SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49") +IF(WIN32) + # for some unknown reason no one variant works on all windows platforms + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED) +ELSE(WIN32) + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED) +ENDIF(WIN32) + +if(Boost_FOUND) + link_directories(${BOOST_LIBRARY_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS}) +endif() + +################################################# +# Declare Options and modify build accordingly ## +################################################# + + +FUNCTION(ENFORCE_OPTION_DEP_3DTK option VALUE) + SET (${option} "${VALUE}" CACHE BOOL "${${option}_DESCRIPTION}" FORCE) # this option set to VALUE as advised + + #now make sure other dependencies are also true + FOREACH(d ${${option}_DEPENDENCIES}) # look through all my dependencies + STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") + + # check for a not in front + STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP) + STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT) + #STRING(REPLACE "NOT " "" CMAKE_DEPENDENT_OPTION_DEP "${d}") + IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT + STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}") + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF) + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON) + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT) + ENDFOREACH(d) + +ENDFUNCTION(ENFORCE_OPTION_DEP_3DTK) + +MACRO(OPT_DEP option doc default depends) + OPTION(${option} "${doc}" "${default}") + SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) + SET(${option}_DEPENDENCIES "${depends}" CACHE INTERNAL "" FORCE) + SET(${option}_DESCRIPTION "${doc}" CACHE INTERNAL "" FORCE) + + IF (${option}) + #MESSAGE(STATUS "Yes ${option} is true") + # MESSAGE("FOREACH d in ${depends}") + + FOREACH(d ${depends}) + STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") + + # check for a not in front + STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP) + STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT + STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}") + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF) + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON) + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT) + ENDFOREACH(d) + + ENDIF(${option}) +ENDMACRO(OPT_DEP) + + + +## FreeGLUT +OPT_DEP(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON "") + +IF(WITH_FREEGLUT) + MESSAGE(STATUS "With freeglut") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT") +ELSE(WITH_FREEGLUT) + MESSAGE(STATUS "Without freeglut") +ENDIF(WITH_FREEGLUT) + +## Show +OPT_DEP(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON "" ) + +IF(WITH_SHOW) + FIND_PACKAGE(OpenGL REQUIRED) + FIND_PACKAGE(GLUT REQUIRED) + MESSAGE(STATUS "With show") +ELSE(WITH_SHOW) + # SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE) + MESSAGE(STATUS "Without show") +ENDIF(WITH_SHOW) + +## WXShow +OPT_DEP(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF "") + +IF(WITH_WXSHOW) + FIND_PACKAGE(OpenGL REQUIRED) + FIND_PACKAGE(GLUT REQUIRED) + find_package(wxWidgets COMPONENTS core base gl REQUIRED) + # set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration + SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE) + # wxWidgets include (this will do all the magic to configure everything) + include( ${wxWidgets_USE_FILE}) + MESSAGE(STATUS "With wxshow") +ELSE(WITH_WXSHOW) + MESSAGE(STATUS "Without wxshow") +ENDIF(WITH_WXSHOW) + +## Shapes +OPT_DEP(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF "") +IF(WITH_SHAPE_DETECTION) + MESSAGE(STATUS "With shape detection") +ELSE(WITH_SHAPE_DETECTION) + MESSAGE(STATUS "Without shape detection") +ENDIF(WITH_SHAPE_DETECTION) + +## Interior reconstruction +OPT_DEP(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF "") + +if(WITH_MODEL) + message(STATUS "With interior reconstruction") +else(WITH_MODEL) + message(STATUS "Without interior reconstruction") +endif(WITH_MODEL) + +## Thermo +OPT_DEP(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF "WITH_SHAPE_DETECTION") +IF(WITH_THERMO) + #for OpenCV 2.1 + FIND_PACKAGE(OpenCV REQUIRED) + include("3rdparty/CMakeModules/OpenCV.cmake") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") + add_subdirectory(3rdparty/cvblob) + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + MESSAGE(STATUS "With thermo") +ELSE(WITH_THERMO) + MESSAGE(STATUS "Without thermo") +ENDIF(WITH_THERMO) + + +## Octree +OPT_DEP(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF "") + +IF(WITH_COMPACT_OCTREE) + MESSAGE(STATUS "Using compact octrees") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE") +ELSE(WITH_COMPACT_OCTREE) + MESSAGE(STATUS "Not using compact octreees: Warning uses more memory") +ENDIF(WITH_COMPACT_OCTREE) + +## Glee? +OPT_DEP(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF "") + +IF(WITH_GLEE) + MESSAGE(STATUS "Using opengl extensions") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE") +ELSE(WITH_GLEE) + MESSAGE(STATUS "Not using opengl extensions") +ENDIF(WITH_GLEE) + +## Gridder +OPT_DEP(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF "") + +IF(WITH_GRIDDER) + MESSAGE(STATUS "With 2DGridder") +ELSE(WITH_GRIDDER) + MESSAGE(STATUS "Without 2DGridder") +ENDIF(WITH_GRIDDER) + +## Dynamic VELOSLAM +OPT_DEP(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF "WITH_SHOW") + +IF(WITH_VELOSLAM) + MESSAGE(STATUS "With VELOSLAM") +ELSE(WITH_VELOSLAM) + MESSAGE(STATUS "Without VELOSLAM") +ENDIF(WITH_VELOSLAM) + +## Home-made Laserscanner +OPT_DEP(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF "") + +IF(WITH_DAVID_3D_SCANNER) + MESSAGE(STATUS "With David scanner") +ELSE(WITH_DAVID_3D_SCANNER) + MESSAGE(STATUS "Without David scanner") +ENDIF(WITH_DAVID_3D_SCANNER) + +## Tools + +OPT_DEP(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF "WITH_FBR") + +IF(WITH_TOOLS) + MESSAGE(STATUS "With Tools") + find_package (Boost COMPONENTS program_options REQUIRED) +ELSE(WITH_TOOLS) + MESSAGE(STATUS "Without Tools") +ENDIF(WITH_TOOLS) + +## Segmentation + +OPT_DEP(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF "WITH_FBR") + +IF(WITH_SEGMENTATION) + MESSAGE(STATUS "With segmentation") + find_package (Boost COMPONENTS program_options REQUIRED) +ELSE(WITH_SEGMENTATION) + MESSAGE(STATUS "Without segmentation") +ENDIF(WITH_SEGMENTATION) + +## Normals + +OPT_DEP(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF "WITH_FBR") + +IF(WITH_NORMALS) + MESSAGE(STATUS "With normals") +ELSE(WITH_NORMALS) + MESSAGE(STATUS "Without normals") +ENDIF(WITH_NORMALS) + +## RivLib +OPT_DEP(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF "") + +IF(WITH_RIVLIB) + MESSAGE(STATUS "Compiling a scan IO for RXP files") + include_directories(${CMAKE_SOURCE_DIR}/3rdparty) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty) + SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/) + IF(WIN32) + SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib) + ELSE(WIN32) + SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread) + ENDIF(WIN32) + FIND_PACKAGE(LibXml2 ) + +ELSE(WITH_RIVLIB) + MESSAGE(STATUS "Do NOT compile a scan IO for RXP") +ENDIF(WITH_RIVLIB) + +## CUDA support, TODO depend on CUDA_FIND +OPT_DEP(WITH_CUDA "Compile with CUDA support" OFF "") +IF(WITH_CUDA) + MESSAGE(STATUS "Compiling WITH CUDA support") + FIND_PACKAGE(CUDA) + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA") +ELSE(WITH_CUDA) + MESSAGE(STATUS "Compiling WITHOUT CUDA support") +ENDIF(WITH_CUDA) + +## PMD +OPT_DEP(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF "") + +IF(WITH_PMD) + FIND_PACKAGE(OpenGL REQUIRED) + MESSAGE(STATUS "With Tools") +ELSE(WITH_PMD) + MESSAGE(STATUS "Without Tools") +ENDIF(WITH_PMD) + +## FBR +OPT_DEP(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF "") + +IF(WITH_FBR) + FIND_PACKAGE(OpenCV REQUIRED) + include("3rdparty/CMakeModules/OpenCV.cmake") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") + MESSAGE(STATUS "With FBR ") +ELSE(WITH_FBR) + MESSAGE(STATUS "Without FBR") +ENDIF(WITH_FBR) + +# OPEN +FIND_PACKAGE(OpenMP) +IF(OPENMP_FOUND) + OPT_DEP(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON "") +ENDIF(OPENMP_FOUND) + +IF(OPENMP_FOUND AND WITH_OPENMP) + MESSAGE(STATUS "With OpenMP ") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${NUMBER_OF_CPUS} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP") +ELSE(OPENMP_FOUND AND WITH_OPENMP) + MESSAGE(STATUS "Without OpenMP") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1") +ENDIF(OPENMP_FOUND AND WITH_OPENMP) + +## TORO +OPT_DEP(WITH_TORO "Whether to use TORO. ON/OFF" OFF "") + +IF(WITH_TORO) + IF(WIN32) + SET(Subversion_SVN_EXECUTABLE "svn.exe") + ENDIF(WIN32) + cmake_minimum_required (VERSION 2.8) + include(ExternalProject) + ExternalProject_Add(toro3d + SVN_REPOSITORY https://www.openslam.org/data/svn/toro/trunk + SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/toro" + CONFIGURE_COMMAND "" + BUILD_COMMAND make + BUILD_IN_SOURCE 1 + INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/toro/toro3d ${CMAKE_SOURCE_DIR}/bin/ + ) + MESSAGE(STATUS "With TORO ") +ELSE(WITH_TORO) + MESSAGE(STATUS "Without TORO") +ENDIF(WITH_TORO) + + +## HOGMAN +OPT_DEP(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF "") + +IF(WITH_HOGMAN) + # dependant on libqt4-devi + find_package( Qt4 REQUIRED) + # CMake of earlier versions do not have external project capabilities + cmake_minimum_required (VERSION 2.8) + include(ExternalProject) + ExternalProject_Add(hogman + SVN_REPOSITORY https://svn.openslam.org/data/svn/hog-man/trunk + SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/hogman" + CONFIGURE_COMMAND /configure --prefix= + BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make + BUILD_IN_SOURCE 1 + INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/ + ) + MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH") +ELSE(WITH_HOGMAN) + MESSAGE(STATUS "Without HOGMAN") +ENDIF(WITH_HOGMAN) + +OPT_DEP(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF "WITH_SHOW;WITH_FBR") + +IF(EXPORT_SHARED_LIBS) + ## Compile a single shared library containing all of 3DTK + add_library(slam SHARED src/slam6d/icp6D.cc) + target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s) + MESSAGE(STATUS "exporting additional libraries") +ELSE(EXPORT_SHARED_LIBS) + MESSAGE(STATUS "not exporting libraries") +ENDIF(EXPORT_SHARED_LIBS) + + +OPT_DEP(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF "") + +IF(WITH_METRICS) + MESSAGE(STATUS "With metrics in slam6d.") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS") +ELSE(WITH_METRICS) + MESSAGE(STATUS "Without metrics in slam6d.") +ENDIF(WITH_METRICS) + +################################################# +# OPERATING SYSTEM SPECIFIC BEHAVIOUR ## +################################################# + +## Special treatment for system specifics +IF(APPLE) +add_definitions(-Dfopen64=fopen) +ENDIF(APPLE) + +## Multiple Cores +IF(APPLE) + SET(PROCESSOR_COUNT 2) +ELSE(APPLE) + INCLUDE(CountProcessors) + SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" ) +ENDIF(APPLE) + +IF(WIN32) + SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING "Additional flags given to the compiler ( -O2)" ) + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64) + add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp) + SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib") +ELSE(WIN32) + SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" ) + # Add include path for OpenGL without GL/-prefix + # to avoid the include incompatibility between MACOS + # and linux + FIND_PATH(OPENGL_INC gl.h /usr/include/GL) + include_directories(${OPENGL_INC}) +ENDIF(WIN32) + +# Add OpenGL includes for MACOS if needed +# The OSX OpenGL frameworks natively supports freeglut extensions +IF(APPLE) + include_directories(/System/Library/Frameworks/GLUT.framework/Headers) + include_directories(/System/Library/Frameworks/OpenGL.framework/Headers) +ENDIF(APPLE) + +# hack to "circumvent" Debug and Release folders that are created under visual studio +# this is why the INSTALL target has to be used in visual studio +IF(MSVC) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) + + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) +ENDIF(MSVC) + + + +################################################# +# GENERAL PROJECT SETTINGS ## +################################################# + +SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}") + +# Hide CMake variables +SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE) +SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE) + + +# Set output directories for libraries and executables +SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib ) +SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj ) +SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin ) + +# Set include and link dirs ... +include_directories(${CMAKE_SOURCE_DIR}/include) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src) +link_directories(${CMAKE_SOURCE_DIR}/obj) +link_directories(${CMAKE_SOURCE_DIR}/lib) + +add_subdirectory(3rdparty) +add_subdirectory(src/slam6d) +add_subdirectory(src/scanio) +add_subdirectory(src/scanserver) +add_subdirectory(src/segmentation) +add_subdirectory(src/normals) +add_subdirectory(src/veloslam) +add_subdirectory(src/show) +add_subdirectory(src/grid) +add_subdirectory(src/pmd) +add_subdirectory(src/shapes) +add_subdirectory(src/thermo) +add_subdirectory(src/slam6d/fbr) +add_subdirectory(src/scanner) +add_subdirectory(src/model) + +MESSAGE (STATUS "Build environment is set up!") diff --git a/.svn/pristine/0e/0ea2f773ec8786b52878e4e62c48bbddb43d07fc.svn-base b/.svn/pristine/0e/0ea2f773ec8786b52878e4e62c48bbddb43d07fc.svn-base new file mode 100644 index 0000000..0254367 --- /dev/null +++ b/.svn/pristine/0e/0ea2f773ec8786b52878e4e62c48bbddb43d07fc.svn-base @@ -0,0 +1,112 @@ +/* + * grabFramesPMD implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include +#include + +#include "pmdsdk2.h" +#include "cvpmd.h" + + +/* TODO: +* flags: +* subpixel +* camera id +* flip x and y +* pmd mode? +*/ + +void usage(char *progName) { + printf("%s \n", progName); + printf("i.e.: %s 6 4 0\n", progName); + printf("press space to detect chessboard and (again) to proceed.\n"); +} + +int main(int argc, char **argv) { + + if(argc < 4) { + usage(argv[0]); + exit(1); + } + + + PMD *pmd = initPMD("../o3d.L32.pcp", "192.168.0.69"); + IplImage *img = cvCreateImage(pmdGetSize(pmd), 8, 1); + pmdUpdate(pmd->hnd); + pmdQueryImage(pmd, img); + + IplImage *imgColor = cvCreateImage(cvGetSize(img), 8, 3); + + int patx = atoi(argv[1]); + int paty = atoi(argv[2]); + CvSize patternSize = cvSize(patx, paty); + + int cornersTotal = patternSize.width * patternSize.height; + + CvPoint2D32f *corners = (CvPoint2D32f*) malloc(cornersTotal * sizeof(CvPoint2D32f)); + + cvNamedWindow("Camera", 0); + + int imageCnt = 0; + bool grabFrame = false; + char *filename = (char*) malloc(11 * sizeof(char)); + + + while(1) { + pmdUpdate(pmd->hnd); + pmdQueryImage(pmd, img); + + cvFlip(img, 0, 1); // flips image around the x-axes + + + + + if(grabFrame) { + /* ----- Chessboard detection ----- + -------------------------------- */ + int cornersCount; // should be the same for wcam and pmd and equal totalCorners + + int found = cvFindChessboardCorners(img, patternSize, corners, + &cornersCount, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + cvFindCornerSubPix(img, corners, cornersCount, cvSize(4,4), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + + if(found && (cornersCount == cornersTotal)) { + cvCvtColor(img, imgColor, CV_GRAY2BGR); + cvDrawChessboardCorners(imgColor, patternSize, corners, cornersCount, found); + cvShowImage("Camera", imgColor); + printf("grab?\n"); + int k = cvWaitKey(0); + if(k == (int)' ') { + imageCnt++; + sprintf(filename, "image%02i.jpg", imageCnt); //TODO:try png + printf("saved %s.\n", filename); + cvSaveImage(filename, img); + grabFrame = false; + continue; + } + } + } + + cvShowImage("Camera", img); + if((int)' ' == cvWaitKey(5)) grabFrame = true; + } + + return 0; +} + diff --git a/.svn/pristine/0f/0f61099f4b560a4139bd4d4db02bb24860e35981.svn-base b/.svn/pristine/0f/0f61099f4b560a4139bd4d4db02bb24860e35981.svn-base new file mode 100644 index 0000000..ad9c313 --- /dev/null +++ b/.svn/pristine/0f/0f61099f4b560a4139bd4d4db02bb24860e35981.svn-base @@ -0,0 +1,1076 @@ +/* + * veloshow implementation + * + * Copyright (C) Andreas Nuechter, Li Wei, Li Ming + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation for displaying of a matched 3D scene + * @author Andreas Nuechter. Jacobs University Bremen, Germany + * @author Li Wei, Wuhan University, China + * @author Li Ming, Wuhan University, China + */ +#ifdef WITH_GLEE +#include +#endif + +#include "show/show.h" +#include "show/show_Boctree.h" +#include "show/compacttree.h" +#include "show/NurbsPath.h" +#include "show/vertexarray.h" +#include "slam6d/scan.h" +#include "veloslam/veloscan.h" +#include "glui/glui.h" /* Header File For The glui functions */ +#include +using std::ifstream; +#include +using std::exception; + +#ifdef _MSC_VER +#include "XGetopt.h" +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + + +#ifdef _MSC_VER +#ifdef OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#include "slam6d/point_type.h" +#include "show/display.h" + +/** + * This vector contains the pointer to a vertex array for + * all colors (inner vector) and all scans (outer vector) + */ +vector< vector > vvertexArrayList; + +vector< ::SDisplay*> displays; +/** + * the octrees that store the points for each scan + */ +//Show_BOctTree **octpts; +vector octpts; +/** + * Storing the base directory + */ +string scandirectory; + +/** + * Storing the ID of the main windows + */ +int window_id; + +/** + * Size of points + */ +GLfloat pointsize = 1.0; +int anim_delay = 5; + +/** + * Select Color Buffer + */ +GLenum buffermode = GL_BACK; + +/** + * Indicator whether and how the drawing window + * has to be updated. + * + * haveToUpdate == 1 redisplay + * haveToUpdate == 2 reshape + * haveToUpdate == 3 animation scan matching + * haveToUpdate == 4 stop animation scan matching + * haveToUpdate == 6 path animation + * haveToUpdate == 7 force redisplay with all points + */ +int haveToUpdate = 0; + +/** + * Flag for invert the scene + */ +bool invert = false; + +/** + * Flag for indicating brid eyes view + */ +bool showTopView = false; + +/** + * Flag for idicating camera add mode + */ +bool addCameraView = false; //Is the view in add box mode? + +/** + * Storing the apex angle of the camera + */ +GLfloat cangle = 60.0; // Current camera opening mode +GLfloat cangle_old = cangle; + +/** + * Current rotation axis of the scene as quaternion + */ +GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; +GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; + +/** + * Current translation of the scene + */ +GLdouble X = 0.0, Y = 0.0, Z = 0.0; +GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; + +/** + * parallel zoom (similar to apex angle) for parallel projection + */ +GLfloat pzoom = 2000.0; +GLfloat pzoom_old = pzoom; + + +/** + * Mode of the fog (exp, exp2, linear) + */ +GLint fogMode = GL_EXP; + +/** + * Indicates if fog should be shown + */ +int show_fog = 1; + +/** + * Indicates if the points should be shown + */ +int show_points = 1; // Show data points in the viewer? + +/** + * Indicates if camera boxes should be shown + */ +int show_cameras = 1; // Show the camera boxes in the viewer? + +/** + * Indicates if camera path or robot path should be shown + */ +int show_path = 1; // Show the camera movement path ? + +/** + * Camera navigation by mouse or by panel + */ +int cameraNavMouseMode = 1; + +int mouseNavX, mouseNavY; +int mouseNavButton = -1; + +double mouseRotX = 0.0; +double mouseRotY = 0.0; +double mouseRotZ = 0.0; + +bool keymap[256]; + +//@@@ +//int animate_both = 0; // Animate both scan matchin and path? + +int frameNr = 0; + +/** + * Storing of all transformation (frames for animation) of all scans + */ +vector < vector > MetaMatrix; + +/** + * Storing of AlgoType for all frames + */ +vector < vector > MetaAlgoType; + +/** + * Window position + */ +int START_X = 0; +int START_Y = 0; +int START_WIDTH = 720; +int START_HEIGHT = 576; +GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio +bool advanced_controls = false; + +bool fullscreen = false; +int current_width = START_WIDTH; +int current_height = START_HEIGHT; + + +// the following values are scale dependant, i.e. all values are in m +float neardistance = 0.10; +double oldneardistance = 0.10; +float maxfardistance = 400.0;; +double fardistance = 400.0; +double oldfardistance = 40000.0; +double movementSpeed = 0.1; +double defaultZoom = 20.0; +GLfloat fogDensity = 0.1; +double voxelSize = 0.20; + + +float adaption_rate = 1.0; +float LevelOfDetail = 0.0001; + + +// Defines for Point Semantic +#define TYPE_UNKNOWN 0x0000 +#define TYPE_OBJECT 0x0001 +#define TYPE_GROUND 0x0002 +#define TYPE_CEILING 0x0003 + +unsigned int cam_choice = 0; + +static unsigned int path_iterator = 0; +static int oldcamNavMode = 0; + +/** + * Animation sould be saved to file + */ +int save_animation = 0; +/** + * If true, interpolation for camera path is based on distance, else always + * the same number of images between two cameras. + */ +int inter_by_dist = 1; + +/**some variables for the camera path**/ +vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; +vector cams; +vector lookats; +vector ups; + +NurbsPath cam_nurbs_path; +char *path_file_name; +char *pose_file_name; + +/** Factor for saved image size */ +int factor = 1; + +/** + * program tries to have this framerate + */ +float idealfps = 20.0; +/** + * value of the listBox fo Color Value and Colormap + */ +int listboxColorVal = 0; +int listboxColorMapVal = 0; +int colorScanVal = 0; +ScanColorManager *cm; +float mincolor_value = 0.0; +float maxcolor_value = 0.0; +//unsigned int types = Point::USE_HEIGHT; +PointType pointtype; + +/** + * Contains the selected points for each scan + */ +set *selected_points; +/** + * Select single points? + */ +int select_voxels = 0; +/** + * Select or unselect points ? + */ +int selectOrunselect = 1; +/** octree depth for selecting groups of points */ +int selection_depth = 1; +int brush_size = 0; +char *selection_file_name; + +int current_frame = 0; + +#include "../show/show_menu.cc" +#include "../show/show_animate.cc" +#include "../show/show_gl.cc" + +/** + * Explains the usage of this program's command line parameters + * @param prog name of the program + */ +void usage(char* prog) +{ +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl + << " will attempt to display points with a framerate of NR" << endl + << endl + << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << + "FILE" << endl + << " load objects specified in " << endl + << endl + << endl + << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl + << " neglegt all data points with a distance larger than NR 'units'" << endl + << endl + << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl + << " neglegt all data points with a distance smaller than NR 'units'" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl + << " sets the starting and reset position to: " << endl + << " 0 = the origin of the coordinate system (default)" << endl + << " 1 = the position of the first scan (default if --origin is in argument list)" << endl + << " 2 = the center of the first scan" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -S" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl + << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl + << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl + << " " << endl + << endl + + << bold << " -R, --reflectance, --reflectivity" << normal << endl + << " use reflectivity values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -a, --amplitude" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -d, --deviation" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -h, --height" << endl << normal + << " use y-values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -T, --type" << endl << normal + << " use type values for coloring point clouds" << endl + << " only works when using octree display" << endl + << bold << " -c, --color" << endl << normal + << " use color RGB values for coloring point clouds" << endl + << bold << " --saveOct" << endl << normal + << " stores all used scans as octrees in the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl + << " only works when using octree display" << endl + << bold << " --loadOct" << endl << normal + << " only reads octrees from the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are read from file." << endl + << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl + << " only works when using octree display" << endl + << endl << endl; + + exit(1); +} + + +/** + * A function that parses the command-line arguments and sets the respective flags. + * + * @param argc the number of arguments + * @param argv the arguments + * @param dir parsing result - the directory + * @param start parsing result - starting at scan number 'start' + * @param end parsing result - stopping at scan number 'end' + * @param maxDist parsing result - maximal distance + * @param minDist parsing result - minimal distance + * @param readInitial parsing result - read a file containing a initial transformation matrix + * @param type parsing result - file format to be read + * @return 0, if the parsing was successful, 1 otherwise + */ +int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, + double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, bool &loadOct, bool &saveOct, int &origin, double &scale, reader_type &type) +{ + unsigned int types = PointType::USE_NONE; + start = 0; + end = -1; // -1 indicates no limitation + maxDist = -1; // -1 indicates no limitation + int c; + // from unistd.h + extern char *optarg; + extern int optind; + + cout << endl; + static struct option longopts[] = { + { "origin", optional_argument, 0, 'o' }, + { "format", required_argument, 0, 'f' }, + { "fps", required_argument, 0, 'F' }, + { "scale", required_argument, 0, 'S' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "octree", optional_argument, 0, 'O' }, + { "reflectance", no_argument, 0, 'R' }, + { "reflectivity", no_argument, 0, 'R' }, + { "amplitude", no_argument, 0, 'a' }, + { "deviation", no_argument, 0, 'd' }, + { "height", no_argument, 0, 'h' }, + { "type", no_argument, 0, 'T' }, + { "color", no_argument, 0, 'c' }, + { "loadObj", required_argument, 0, 'l' }, + { "saveOct", no_argument, 0, '0' }, + { "loadOct", no_argument, 0, '1' }, + { "advanced", no_argument, 0, '2' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + //while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:wtRadhTcC", longopts, NULL)) != -1) + while ((c = getopt_long(argc, argv, "O:f:A:G:L:a:b:t:r:R:d:D:i:l:I:c:C:n:s:e:m:M:uqQp", longopts, NULL)) != -1) + switch (c) + { + case 's': + start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 't': + readInitial = true; + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'f': + if (!Scan::toType(optarg, type)) + abort (); + break; + case '?': + usage(argv[0]); + return 1; + case 'R': + types |= PointType::USE_REFLECTANCE; + break; + case 'a': + types |= PointType::USE_AMPLITUDE; + break; + case 'd': + types |= PointType::USE_DEVIATION; + break; + case 'h': + types |= PointType::USE_HEIGHT; + break; + case 'T': + types |= PointType::USE_TYPE; + break; + case 'c': + types |= PointType::USE_COLOR; + break; + case 'F': + fps = atof(optarg); + break; + case 'S': + scale = atof(optarg); + break; + case 'o': + if (optarg) { + origin = atoi(optarg); + } else { + origin = 1; + } + break; + case '0': + saveOct = true; + break; + case '1': + loadOct = true; + break; + case 'l': + loadObj = optarg; + break; + case '2': + advanced_controls = true; + break; + default: + abort (); + } + + if (optind != argc-1) { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + ptype = PointType(types); + return 0; +} + +void setResetView(int origin) { + if (origin == 1) { + double *transmat = MetaMatrix[0].back(); + cout << transmat << endl; + + RVX = -transmat[12]; + RVY = -transmat[13]; + RVZ = -transmat[14]; + Matrix4ToQuat(transmat, Rquat); + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + } else if (origin == 2) { + double center[3]; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[0])->getCenter(center); +#else + ((Show_BOctTree*)octpts[0])->getCenter(center); +#endif + RVX = -center[0]; + RVY = -center[1]; + RVZ = -center[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } +} + +/* + * A function that read the .frame files created by slam6D + * + * @param dir the directory + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param read a file containing a initial transformation matrix and apply it + */ +int readFrames(string dir, int start, int end, bool readInitial, reader_type &type) +{ + + double initialTransform[16]; + if (readInitial) { + cout << "Initial Transform:" << endl; + string initialTransformFileName = dir + "initital.frame"; + ifstream initial_in(initialTransformFileName.c_str()); + if (!initial_in.good()) { + cout << "Error opening " << initialTransformFileName << endl; + exit(-1); + } + initial_in >> initialTransform; + cout << initialTransform << endl; + } + + ifstream frame_in; + int fileCounter = start; + string frameFileName; + for (;;) { + if (end > -1 && fileCounter > end) break; // 'nuf read + frameFileName = dir + "scan" + to_string(fileCounter++,3) + ".frames"; + + frame_in.open(frameFileName.c_str()); + + // read 3D scan + if (!frame_in.good()) break; // no more files in the directory + + cout << "Reading Frames for 3D Scan " << frameFileName << "..."; + vector Matrices; + vector algoTypes; + int frameCounter = 0; + + while (frame_in.good()) { + frameCounter++; + double *transMatOpenGL = new double[16]; + int algoTypeInt; + Scan::AlgoType algoType; + try { + double transMat[16]; + frame_in >> transMat >> algoTypeInt; + algoType = (Scan::AlgoType)algoTypeInt; + + // convert to OpenGL coordinate system + double mirror[16]; + M4identity(mirror); + mirror[10] = -1.0; + if (readInitial) { + double tempxf[16]; + MMult(mirror, initialTransform, tempxf); + memcpy(mirror, tempxf, sizeof(tempxf)); + } + //@@@ + // memcpy(transMatOpenGL, transMat, 16*sizeof(double)); + MMult(mirror, transMat, transMatOpenGL); + } + catch (const exception &e) { + break; + } + Matrices.push_back(transMatOpenGL); + algoTypes.push_back(algoType); + } + + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + + if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && fileCounter == start+1) { + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + } + + frame_in.close(); + frame_in.clear(); + cout << MetaMatrix.back().size() << " done." << endl; + current_frame = MetaMatrix.back().size() - 1; + } + if (MetaMatrix.size() == 0) { + cerr << "*****************************************" << endl; + cerr << "** ERROR: No .frames could be found! **" << endl; + cerr << "*****************************************" << endl; + cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; + return -1; + } + return 0; +} + +void generateFrames(int start, int end, bool identity) { + if (identity) { + cout << "using Identity for frames " << endl; + } else { + cout << "using pose information for frames " << endl; + } + int fileCounter = start; + int index = 0; + for (;;) { + if (fileCounter > end) break; // 'nuf read + fileCounter++; + + vector Matrices; + vector algoTypes; + + for (int i = 0; i < 3; i++) { + double *transMat = new double[16]; + + if (identity) { + M4identity(transMat); + transMat[10] = -1.0; + } else { + EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); + } + + Matrices.push_back(transMat); + algoTypes.push_back(Scan::ICP); + + } + index++; + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + } +} + + + +/* + * create display lists + * @to do general framework for color & type definitions + */ +void createDisplayLists(bool reduced) +{ + for(unsigned int i = 0; i < Scan::allScans.size() ; i++) { + + // count points + int color1 = 0, color2 = 0; + if (!reduced) { + for (unsigned int jterator = 0; jterator < Scan::allScans[i]->get_points()->size(); jterator++) { + if (Scan::allScans[i]->get_points()->at(jterator).type & TYPE_GROUND) { + color1++; + } else { + color2++; + } + } + } else { + color2 = 3* Scan::allScans[i]->get_points_red_size(); + } + + // allocate memory + vertexArray* myvertexArray1 = new vertexArray(color1); + vertexArray* myvertexArray2 = new vertexArray(color2); + + // fill points + color1 = 0, color2 = 0; + if (reduced) { + for (int jterator = 0; jterator < Scan::allScans[i]->get_points_red_size(); jterator++) { + myvertexArray2->array[color2] = Scan::allScans[i]->get_points_red()[jterator][0]; + myvertexArray2->array[color2+1] = Scan::allScans[i]->get_points_red()[jterator][1]; + myvertexArray2->array[color2+2] = Scan::allScans[i]->get_points_red()[jterator][2]; + color2 += 3; + } + } else { + for (unsigned int jterator = 0; jterator < Scan::allScans[i]->get_points()->size(); jterator++) { + if (Scan::allScans[i]->get_points()->at(jterator).type & TYPE_GROUND) { + myvertexArray1->array[color1] = Scan::allScans[i]->get_points()->at(jterator).x; + myvertexArray1->array[color1+1] = Scan::allScans[i]->get_points()->at(jterator).y; + myvertexArray1->array[color1+2] = Scan::allScans[i]->get_points()->at(jterator).z; + color1 += 3; + } else { + myvertexArray2->array[color2] = Scan::allScans[i]->get_points()->at(jterator).x; + myvertexArray2->array[color2+1] = Scan::allScans[i]->get_points()->at(jterator).y; + myvertexArray2->array[color2+2] = Scan::allScans[i]->get_points()->at(jterator).z; + color2 += 3; + } + } + } + + glNewList(myvertexArray1->name, GL_COMPILE); + //@ + //glColor4d(0.44, 0.44, 0.44, 1.0); + //glColor4d(0.66, 0.66, 0.66, 1.0); + glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender); + glDisableClientState(GL_VERTEX_ARRAY); + glEndList(); + + glNewList(myvertexArray2->name, GL_COMPILE); + //glColor4d(1.0, 1.0, 1.0, 1.0); + //glColor4d(0.0, 0.0, 0.0, 1.0); + glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender); + glDisableClientState(GL_VERTEX_ARRAY); + glEndList(); + + // append to vector + vector vvertexArray; + vvertexArray.push_back(myvertexArray1); + vvertexArray.push_back(myvertexArray2); + vvertexArrayList.push_back(vvertexArray); + } + +} + +void cycleLOD() { + LevelOfDetail = 0.00001; + for (unsigned int i = 0; i < octpts.size(); i++) + octpts[i]->cycleLOD(); +} + + +void initShow(int argc, char **argv){ + + /***************/ + /* init OpenGL */ + /***************/ + glutInit(&argc,argv); + + cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; + + if(argc <= 1){ + usage(argv[0]); + } + + + + double red = -1.0; + int start = 0, end = -1, maxDist = -1, minDist = -1; + string dir; + bool readInitial = false; + reader_type type = UOS; + int octree = 0; + bool loadOct = false; + bool saveOct = false; + string loadObj; + int origin = 0; + double scale = 0.01; // in m + + pose_file_name = new char[1024]; + path_file_name = new char[1024]; + selection_file_name = new char[1024]; + + strncpy(pose_file_name, "pose.dat", 1024); + strncpy(path_file_name, "path.dat", 1024); + strncpy(selection_file_name, "selected.3d", 1024); + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, + octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, type); + + // modify all scale dependant variables + scale = 1.0 / scale; + movementSpeed *= scale; + neardistance *= scale; + oldneardistance *= scale; + maxfardistance *= scale; + fardistance *= scale; + fogDensity /= scale; + defaultZoom *= scale; + voxelSize *= scale; +// oldfardistance *= scale; + + //////////////////////// + SDisplay::readDisplays(loadObj, displays); + //////////////////// + + if (type == OCT) { + loadOct = true; + } + + // if we want to load display file get pointtypes from the files first + if (loadOct) { + string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; + cout << "Getting point information from " << scanFileName << endl; + cout << "Attention! All subsequent oct-files must be of the same type!" << endl; + + pointtype = BOctTree::readType(scanFileName); + } + scandirectory = dir; + + // init and create display + M4identity(view_rotate_button); + obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; + + // read frames first, to get notifyied of missing frames before all scans are read in + int r = readFrames(dir, start, end, readInitial, type); + + + // Get Scans + if (!loadOct) { +#ifndef DYNAMIC_OBJECT_REMOVAL + Scan::readScans(type, start, end, dir, maxDist, minDist, 0); +#else + VeloScan::readScans(type, start, end, dir, maxDist, minDist, 0); +#endif + } else { + cout << "Skipping files.." << endl; + } + + if (!loadOct) { + if (r) generateFrames(start, start + Scan::allScans.size() - 1, false); + } else { + if (r) generateFrames(start, start + octpts.size() - 1, true); + } + + int end_reduction = (int)Scan::allScans.size(); + #ifdef _OPENMP + #pragma omp parallel for schedule(dynamic) + #endif + for (int iterator = 0; iterator < end_reduction; iterator++) { + // reduction filter for current scan! + if (red > 0) { + cout << "Reducing Scan No. " << iterator << endl; + // TODO do another reduction so reflectance values etc are carried over + Scan::allScans[iterator]->calcReducedPoints(red, octree); + } // no copying necessary for show! + } + + cm = new ScanColorManager(4096, pointtype); + + if (loadOct) { + for (int i = start; i <= end; i++) { + string scanFileName = dir + "scan" + to_string(i,3) + ".oct"; + cout << "Reading octree " << scanFileName << endl; +#ifdef USE_COMPACT_TREE + octpts.push_back(new compactTree(scanFileName, cm)); +#else + octpts.push_back(new Show_BOctTree(scanFileName, cm)); +#endif + } + } else { +#if USE_COMPACT_TREE + cout << "Creating compact display octrees.." << endl; + for(int i = 0; i < (int)Scan::allScans.size() ; i++) { + compactTree *tree; + if (red > 0) { + tree = new compactTree(Scan::allScans[i]->get_points_red(), Scan::allScans[i]->get_points_red_size(), voxelSize, pointtype, cm); // TODO remove magic number + } else { + unsigned int nrpts = Scan::allScans[i]->get_points()->size(); + sfloat **pts = new sfloat*[nrpts]; + for (unsigned int jterator = 0; jterator < nrpts; jterator++) { + pts[jterator] = pointtype.createPoint(Scan::allScans[i]->get_points()->at(jterator)); + } + Scan::allScans[i]->clearPoints(); + tree = new compactTree(pts, nrpts , voxelSize, pointtype, cm); //TODO remove magic number + for (unsigned int jterator = 0; jterator < nrpts; jterator++) { + delete[] pts[jterator]; + } + delete[] pts; + } + if (saveOct) { + string scanFileName = dir + "scan" + to_string(i+start,3) + ".oct"; + cout << "Saving octree " << scanFileName << endl; + tree->serialize(scanFileName); + } + octpts.push_back(tree); + cout << "Scan " << i << " octree finished. Deleting original points.." << endl; + } +#else + cout << "Creating display octrees.." << endl; + for(int i = 0; i < (int)Scan::allScans.size() ; i++) { + Show_BOctTree *tree; + if (red > 0) { + tree = new Show_BOctTree(Scan::allScans[i]->get_points_red(), Scan::allScans[i]->get_points_red_size(), voxelSize, pointtype, cm); // TODO remove magic number + } else { + unsigned int nrpts = Scan::allScans[i]->get_points()->size(); + sfloat **pts = new sfloat*[nrpts]; + for (unsigned int jterator = 0; jterator < nrpts; jterator++) { + pts[jterator] = pointtype.createPoint(Scan::allScans[i]->get_points()->at(jterator)); + } + Scan::allScans[i]->clearPoints(); + tree = new Show_BOctTree(pts, nrpts , voxelSize, pointtype, cm); //TODO remove magic number + for (unsigned int jterator = 0; jterator < nrpts; jterator++) { + delete[] pts[jterator]; + } + delete[] pts; + } + octpts.push_back(tree); + if (saveOct) { + string scanFileName = dir + "scan" + to_string(i+start,3) + ".oct"; + cout << "Saving octree " << scanFileName << endl; + tree->serialize(scanFileName); + } + cout << "Scan " << i << " octree finished. Deleting original points.." << endl; + } +#endif + } + + cm->setCurrentType(PointType::USE_HEIGHT); + //ColorMap cmap; + //cm->setColorMap(cmap); + resetMinMax(0); + + selected_points = new set[octpts.size()]; + + // sets (and computes if necessary) the pose that is used for the reset button + setResetView(origin); + + for (unsigned int i = 0; i < 256; i++) { + keymap[i] = false; + } +} + +/** + * Main function. + * Reads the scan (scan000.3d, ...) and frames files (scan000.frames, ...) from the data directory. + * The frames are used for animation of the matching process. + */ + +int main(int argc, char **argv){ + + initShow(argc, argv); + initScreenWindow(); + + newMenu(); + glutMainLoop(); +} + + +void updateCamControls() { + cam_spinner->set_int_limits( 1, cams.size()); + cam_spinner->set_int_val(cam_choice); +} + +void resetRotationButton() { + rotButton->reset(); +} + +void updateTopViewControls() { + if(showTopView) { + pzoom_spinner->enable(); + cangle_spinner->disable(); + } else { + pzoom_spinner->disable(); + cangle_spinner->enable(); + } +} + + +void updateControls() { + glui1->sync_live(); + glui1->show(); + glui2->sync_live(); + glui2->show(); +} + +static bool interrupted = false; +void interruptDrawing() { + interrupted = true; +} +void checkForInterrupt() { + interrupted = false; +} +bool isInterrupted() { +#ifdef WITH_FREEGLUT +#ifndef __APPLE__ + glutMainLoopEvent(); +#endif +#endif + glutSetWindow(window_id); + return interrupted; +} + +void updatePointModeControls() { + switch(pointmode) { + case -1: + always_box->set_int_val(0); + never_box->set_int_val(1); + break; + case 0: + always_box->set_int_val(0); + never_box->set_int_val(0); + break; + case 1: + always_box->set_int_val(1); + never_box->set_int_val(0); + break; + } +} + diff --git a/.svn/pristine/12/1208ed6ab228923ef987fad7143ddd7a7c2245bd.svn-base b/.svn/pristine/12/1208ed6ab228923ef987fad7143ddd7a7c2245bd.svn-base new file mode 100644 index 0000000..6f20f70 --- /dev/null +++ b/.svn/pristine/12/1208ed6ab228923ef987fad7143ddd7a7c2245bd.svn-base @@ -0,0 +1,35 @@ +#pragma once + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + + +struct Frame { + CvMat *trn; + CvMat *rot; + IplImage *img; + CvPoint **status; + CvPoint3D32f **pts; + CvSize sz; + double alignError; +}; + +Frame *allocFrame3DData(CvSize pmdSz); +void fillFrame(Frame *f, IplImage *img, CvSize pmdSz, CvPoint3D32f **pts, CvPoint **status + , CvMat *rot, CvMat *trn, double alignError); +void releaseFrame(Frame **f); + +struct History { + Frame *frame; + History *prev; +}; + +History *createHistory(); +History *addFrame(History *h, Frame *f); +void releaseTail(History *h); +void checkHistoryLen(History *h, int maxLen); diff --git a/.svn/pristine/12/12492f5775ced91e4bbe8a6700ecf1602a462076.svn-base b/.svn/pristine/12/12492f5775ced91e4bbe8a6700ecf1602a462076.svn-base new file mode 100644 index 0000000..17c9b08 --- /dev/null +++ b/.svn/pristine/12/12492f5775ced91e4bbe8a6700ecf1602a462076.svn-base @@ -0,0 +1,648 @@ +/* + * David Scanner implementation + * + * Copyright (C) Vladislav Perelman + * + * Released under the GPL version 3. + * + */ + +/* + * david_scanner.cc + * Program takes as an input path to the config file which needs to + * have all the necessary information for the program. + * Config file has to have (each on a new line, 9 lines in total): + * + * Path to the directory where frames from the video are stored + * The first frame that has to be used + * The last frame that has to be used + * The empty frame without the laser + * Path to the file with intrinsics of the camera + * Path to the rotation of the left board + * Path to the rotation of the right board + * Path to the translation of the left board + * Path to the translation of the right board + * + * Program computes the 3 point cloud of the object and stores it in the + * file scan000.3d, each point in the cloud is represented by the line + * in the file: + * x y z r g b + * + * + * Created on: Oct 4, 2010 + * Author: Vladislav Perelman v.perelman@jacobs-university.de + */ + +#include +#include +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include +#include +#define PI 3.14159265 +using namespace std; + +int main(int argc, char** argv){ + + if (argc!=2){ + cout<<"USAGE: david_scanner config_file\nConfig file should contain path_to_frames first_valid_frame last_valid_frame empty_frame path_to_intrinsics" + "path_to_rotation_left path_to_rotation_right path_to_translation_left and path_to_translation_right each on a new line!"<imageData; + for (int row = 0; row < diff->height; row++){ + for (int col = 0; col < diff->width; col++){ + int R; + R = pixels[ row * diff->widthStep + col * 3 + 2 ]; + if (R>30) { + pixels[ row * diff->widthStep + col * 3 + 0 ] = 0; + pixels[ row * diff->widthStep + col * 3 + 1 ] = 0; + pixels[ row * diff->widthStep + col * 3 + 2 ] = 255; + } else { + pixels[ row * diff->widthStep + col * 3 + 0 ] = 0; + pixels[ row * diff->widthStep + col * 3 + 1 ] = 0; + pixels[ row * diff->widthStep + col * 3 + 2 ] = 0; + } + + } + } + + //remove pixels that don't have at least 2 red neighbors + for (int row = 1; row < diff->height-1; row++){ + for (int col = 1; col < diff->width-1; col++){ + int R = pixels[ row * diff->widthStep + col * 3 + 2 ]; + if (R == 255){ + int r1 = pixels[ (row-1)*diff->widthStep + col * 3 + 2]; + int r2 = pixels[ (row-1)*diff->widthStep + (col-1) * 3 + 2]; + int r3 = pixels[ (row-1)*diff->widthStep + (col+1) * 3 + 2]; + int r4 = pixels[ (row+1)*diff->widthStep + col * 3 + 2]; + int r5 = pixels[ (row+1)*diff->widthStep + (col-1) * 3 + 2]; + int r6 = pixels[ (row+1)*diff->widthStep + (col+1) * 3 + 2]; + int r7 = pixels[ (row)*diff->widthStep + (col-1) * 3 + 2]; + int r8 = pixels[ (row)*diff->widthStep + (col+1) * 3 + 2]; + if (r1+r2+r3+r4+r5+r6+r7+r8<=255) pixels[ row * diff->widthStep + col * 3 + 2 ]=0; + } + } + } + + //*****finding 2 lines on the image***** + + bool good = false; + int threshold = 50; //original threshold for Hough transform, incremented if too many groups of lines found + IplImage* color_dst; + IplImage* tmpImage; + int minX1, minX2, maxX1, maxX2; + CvSeq* lines = 0; + CvPoint* line1; + CvPoint* line2; + int count_groups; + + //incrementing thresholds until only 2 groups of lines can be found + while(!good){ + good = true; + count_groups = 0; //counter for number of line groups. Line group is defined by the slope + int epsilon = 1.5; //error margin for the slope + color_dst = cvCreateImage( cvGetSize(diff), 8, 3 ); + color_dst = cvCloneImage(diff); + tmpImage = cvCreateImage(cvGetSize(diff), IPL_DEPTH_8U, 1); + cvCvtColor(diff, tmpImage, CV_RGB2GRAY); + IplImage* dst = cvCreateImage( cvGetSize(diff), 8, 1 ); + cvCanny(tmpImage, dst, 20, 60, 3 ); + CvMemStorage* storage = cvCreateMemStorage(0); + //find all lines using Hough transform + lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180,threshold, 150, 100 ); + double first_group, second_group; + for(int i = 0; i < lines->total; i++ ){ + //get the slope of the line, check if it belongs to an already existing group + CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i); + double angle = atan((double)(line[1].x-line[0].x)/(double)(line[1].y-line[0].y))*180/PI; + //starting first group + if (count_groups==0){ + first_group = angle; + line1 = line; + minX1 = line[0].x; + maxX1 = line[1].x; + count_groups++; + + } else { + + if (angle-first_group(epsilon*-1)){ + //line belongs to the first group of line..that's good + if (line[0].xmaxX1)maxX1=line[1].x; + } else { + //check if belongs to the second group + if ( count_groups == 2 ){ + if (angle-second_group(epsilon*-1)){ + if (line[0].xmaxX2)maxX2=line[1].x; + }else{ + //if not then try again with a higher threshold + good = false; + threshold+=20; + cout<<"Increased threshold: "<height,ymin+10)); + points[2]=cvPoint(maxX1, min(color_dst->height,ymax+10)); + points[3]=cvPoint(maxX1, max(0,ymax-10)); + CvPoint* pts[1]; + pts[0]=points; + int npts[1]; + npts[0]=4; + cvPolyLine(color_dst, pts, npts,1,1, CV_RGB(0,0,0), 20, 8 );//removing the group + + x1 = line2[0].x; + x2 = line2[1].x; + y1 = line2[0].y; + y2 = line2[1].y; + + c1 = (double)(x1 - minX2)/(double)(x2 - minX2); + c2 = (double)(maxX2 - x1)/(double)(maxX2 - x2); + + ymin = (c1*y2 - y1)/(c1-1); + ymax = (c2*y2 - y1)/(c2-1); + + if (maxX2 == x2) ymax = y2; + if (minX2 == x1) ymin = y1; + + //getting start and end of the second line + point3 = cvPoint(minX2, ymin); + point4 = cvPoint(maxX2, ymax); + + points[0]=cvPoint(minX2, max(0,ymin-10)); + points[1]=cvPoint(minX2, min(color_dst->height,ymin+10)); + points[2]=cvPoint(maxX2, min(color_dst->height,ymax+10)); + points[3]=cvPoint(maxX2, max(0,ymax-10)); + + pts[0]=points; + + cvPolyLine(color_dst, pts, npts,1,1, CV_RGB(0,0,0), 20, 8 );//removing the group + cvLine(color_dst, point3, point4,CV_RGB(0,255,0),3, 8 ); //draw the second line! + cvLine(color_dst, point1, point2,CV_RGB(0,255,0),3, 8 ); //draw the first line! + + //removing everything to the left of the left line and to the right of the right line + if (point4.x > point2.x){ + if (color_dst->width > point4.x){ + cvRectangle(color_dst,cvPoint(point4.x,0),cvPoint(color_dst->width,color_dst->height),CV_RGB(0,0,0),CV_FILLED); + } + if (point1.x > 0){ + cvRectangle(color_dst,cvPoint(point1.x,0),cvPoint(0,color_dst->height),CV_RGB(0,0,0),CV_FILLED); + } + } + if (point4.x < point2.x){ + if (color_dst->width > point2.x){ + cvRectangle(color_dst,cvPoint(point2.x,0),cvPoint(color_dst->width,color_dst->height),CV_RGB(0,0,0),CV_FILLED); + } + if (point3.x > 0){ + cvRectangle(color_dst,cvPoint(point3.x,0),cvPoint(0,color_dst->height),CV_RGB(0,0,0),CV_FILLED); + } + } + //at this point we have to lines which we drew in green...which means all the red pixels that remain on the image + //are supposed to be laying on the object. Make them blue (for no particular reason..just looked nicer :) ) + unsigned char* pixels = (unsigned char*)color_dst->imageData; + + for (int row = 1; row < color_dst->height-1; row++){ + for (int col = 1; col < color_dst->width-1; col++){ + int R = pixels[ row * color_dst->widthStep + col * 3 + 2 ]; + if (R == 255){ + pixels[ row * color_dst->widthStep + col * 3 + 0 ]=255; + pixels[ row * color_dst->widthStep + col * 3 + 1 ]=0; + pixels[ row * color_dst->widthStep + col * 3 + 2 ]=0; + } + } + } + + } else continue; + + //take points on planes + CvPoint left1, left2, right1; + if (point1.x < point3.x){ + left1 = point1; + left2 = point2; + right1 = point3; + } else { + left1 = point3; + left2 = point4; + right1 = point1; + } + + //find 3d coordinate of the 2 points on the line on the left plane + //(x,y,z).t() = s*R.i()*A.i()*(u,v,1).t() - R.i()*T + + CvMat* imagepoint1 = cvCreateMat( 3, 1, CV_32F ); + CV_MAT_ELEM(*imagepoint1, float, 0, 0) = left1.x; + CV_MAT_ELEM(*imagepoint1, float, 1, 0) = left1.y; + CV_MAT_ELEM(*imagepoint1, float, 2, 0) = 1; + + + CvMat* b1 = cvCreateMat(3, 1, CV_32F); + cvMatMul(R1iAi, imagepoint1, b1); + + //calculate scalar s based on the fact that point we take is on the wall => z coordinate is 0 + float s1 = CV_MAT_ELEM(*a1, float, 2, 0)/CV_MAT_ELEM(*b1, float, 2, 0); + + CvMat* identity = cvCreateMat(3,3,CV_32F); + cvSetIdentity(identity); + for (int i = 0; i < 3; i++){ + CV_MAT_ELEM(*identity, float, i, i)=s1; + } + CvMat* temp = cvCreateMat(3,1,CV_32F); + cvMatMul(identity,b1, temp); + + CvMat* dpoint1 = cvCreateMat(3,1,CV_32F); + cvSub(temp, a1, dpoint1); //first 3d point on the left plane + + //same thing for the second point + CvMat* imagepoint2 = cvCreateMat( 3, 1, CV_32F ); + + CV_MAT_ELEM(*imagepoint2, float, 0, 0) = left2.x; + CV_MAT_ELEM(*imagepoint2, float, 1, 0) = left2.y; + CV_MAT_ELEM(*imagepoint2, float, 2, 0) = 1; + + CvMat* b2 = cvCreateMat(3, 1, CV_32F); + cvMatMul(R1iAi, imagepoint2, b2); + + float s2 = CV_MAT_ELEM(*a1, float, 2, 0)/CV_MAT_ELEM(*b2, float, 2, 0); + + cvSetIdentity(identity, cvRealScalar(s2)); + cvMatMul(identity,b2, b2); + + CvMat* dpoint2 = cvCreateMat(3,1,CV_32F); + cvSub(b2, a1, dpoint2); //second 3d point on the left plane + + //same for the point on the right plane + CvMat* imagepoint3 = cvCreateMat( 3, 1, CV_32F ); + CV_MAT_ELEM(*imagepoint3, float, 0, 0) = right1.x; + CV_MAT_ELEM(*imagepoint3, float, 1, 0) = right1.y; + CV_MAT_ELEM(*imagepoint3, float, 2, 0) = 1; + + CvMat* b3 = cvCreateMat(3, 1, CV_32F); + cvMatMul(R2iAi, imagepoint3, b3); + + float s3 = CV_MAT_ELEM(*a2, float, 2, 0)/CV_MAT_ELEM(*b3, float, 2, 0); + + cvSetIdentity(identity, cvRealScalar(s3)); + cvMatMul(identity,b3, b3); + + CvMat* dpoint3 = cvCreateMat(3,1,CV_32F); + cvSub(b3, a2, dpoint3); //point on the right plane + + //convert point from the right plane into the coord. system of the left plane + //p1 = R1.i()*[R2|T2]*p2 - R1.i()*T1 + CvMat* dpoint3left = cvCreateMat(3,1,CV_32F); + CvMat* pw = cvCreateMat(4,1,CV_32F); + for (int i = 0; i<3; i++){ + CV_MAT_ELEM(*pw, float, i, 0) = CV_MAT_ELEM(*dpoint3, float, i, 0); + } + CV_MAT_ELEM(*pw, float, 3, 0) = 1.0; + CvMat* r2t2pw = cvCreateMat(3,1,CV_32F); + cvMatMul(r2t2, pw, r2t2pw); + CvMat* r1invr2t2pw = cvCreateMat(3,1,CV_32F); + cvMatMul(r1inv, r2t2pw, r1invr2t2pw); + + cvSub(r1invr2t2pw, a1, dpoint3left); + //now that we have 3 non-colinear point in the same coordinate system we can find the equation of the plane + /* + A = y1 (z2 - z3) + y2 (z3 - z1) + y3 (z1 - z2) + B = z1 (x2 - x3) + z2 (x3 - x1) + z3 (x1 - x2) + C = x1 (y2 - y3) + x2 (y3 - y1) + x3 (y1 - y2) + - D = x1 (y2 z3 - y3 z2) + x2 (y3 z1 - y1 z3) + x3 (y1 z2 - y2 z1) + */ + float x1 = CV_MAT_ELEM(*dpoint1, float,0,0); + float y1 = CV_MAT_ELEM(*dpoint1, float,1,0); + float z1 = CV_MAT_ELEM(*dpoint1, float,2,0); + float x2 = CV_MAT_ELEM(*dpoint2, float,0,0); + float y2 = CV_MAT_ELEM(*dpoint2, float,1,0); + float z2 = CV_MAT_ELEM(*dpoint2, float,2,0); + float x3 = CV_MAT_ELEM(*dpoint3left, float,0,0); + float y3 = CV_MAT_ELEM(*dpoint3left, float,1,0); + float z3 = CV_MAT_ELEM(*dpoint3left, float,2,0); + float planeA = (y1 * (z2 - z3)) + (y2 * (z3 - z1)) + (y3 * (z1 - z2)); + float planeB = (z1 * (x2 - x3)) + (z2 * (x3 - x1)) + (z3 * (x1 - x2)); + float planeC = (x1 * (y2 - y3)) + (x2 * (y3 - y1)) + (x3 * (y1 - y2)); + float planeD = -((x1 * (y2 * z3 - y3 * z2)) + (x2 * (y3 * z1 - y1 * z3)) + (x3 * (y1 * z2 - y2 * z1))); + + //calculate normal to the lazer plane + CvMat* planeNormal = cvCreateMat(3, 1, CV_32F); + CV_MAT_ELEM(*planeNormal, float,0,0) = planeA; + CV_MAT_ELEM(*planeNormal, float,1,0) = planeB; + CV_MAT_ELEM(*planeNormal, float,2,0) = planeC; + + pixels = (unsigned char*)color_dst->imageData; + unsigned char* color_pixels = (unsigned char*)image_empty->imageData; + + //go through all the pixels on the object and calculate the 3d coordinate + for (int row = 1; row < color_dst->height-1; row++){ + for (int col = 1; col < color_dst->width-1; col++){ + int B = pixels[ row * color_dst->widthStep + col * 3]; + if (B == 255){ + //get RGB of the pixel on the original image + int realB = color_pixels[ row * color_dst->widthStep + col * 3]; + int realG = color_pixels[ row * color_dst->widthStep + col * 3 + 1]; + int realR = color_pixels[ row * color_dst->widthStep + col * 3 + 2]; + //Used http://www.cs.princeton.edu/courses/archive/fall00/cs426/lectures/raycast/sld017.htm for reference + //on how to find intersection of ray and a plane + float p0dotN = cvDotProduct(a1,planeNormal); + + CvMat* vtmp = cvCreateMat(3,1,CV_32F); + CV_MAT_ELEM(*vtmp, float,0,0) = col; + CV_MAT_ELEM(*vtmp, float,1,0) = row; + CV_MAT_ELEM(*vtmp, float,2,0) = 1; + + CvMat* v = cvCreateMat(3,1,CV_32F); + cvMatMul(R1iAi, vtmp, v); + + float vdotN = cvDotProduct(v,planeNormal); + float t = (p0dotN - planeD)/vdotN; + + cvSetIdentity(identity, cvRealScalar(t)); + cvMatMul(identity,v,v); + + CvMat* final = cvCreateMat(3,1,CV_32F); + cvSub(v,a1,final); //final point is still in the coordinate system of the left plane. + + CvMat* final_rotated = cvCreateMat(3,1,CV_32F); //translate it into the coordinate system of the camera + cvMatMul(rotation_matrix_left,final,final_rotated); + cvAdd(final_rotated,translation_left, final_rotated); + + //add point to the file (minus next to the y coordinate is there to compensate for the left-handed coordinate system of slam6d, otherwise + //dwarf is shown upside-down. + scanfile< > > extendedIMap; + unsigned int iWidth; + unsigned int iHeight; + projection_method pMethod; + unsigned int nImages; + double pParam; + panorama_map_method mapMethod; + + void init(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mapMethod); + void map(int x, int y, cv::MatIterator_ it, double range); + public: + /** + * constructor of class panorama + * @param width the width of the panorama image + * @param height the height of the panorama image + * @param method the projection method + * @param images number of subsets to crate panorama image + * @param param special parameter for pannini or stereographic projections + * @param mapMethod mapping method for panorama image and 3D points + */ + panorama (unsigned int width, unsigned int height, projection_method method); + panorama (unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages); + panorama (unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param); + panorama (unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mapMethod); + /** + * @brief creates the panorama reflectance image and map. + */ + void createPanorama(cv::Mat scan); + /** + * @brief recovers the point cloud from the panorama image and range information + * @param image - input range image to be converted to point cloud + * @param file - destination of .3d file containing the point cloud + */ + void recoverPointCloud(const cv::Mat& range_image, cv::Mat& reflectance_image, vector &reduced_points); + + unsigned int getImageWidth(); + unsigned int getImageHeight(); + projection_method getProjectionMethod(); + unsigned int getNumberOfImages(); + double getProjectionParam(); + cv::Mat getReflectanceImage(); + cv::Mat getMap(); + cv::Mat getRangeImage(); + vector > > getExtendedMap(); + panorama_map_method getMapMethod(); + void getDescription(); + }; +} +#endif /* PANORAMA_H_ */ diff --git a/.svn/pristine/24/249ba60a45df1038bd29b029022bcd6fecb9ed57.svn-base b/.svn/pristine/24/249ba60a45df1038bd29b029022bcd6fecb9ed57.svn-base new file mode 100644 index 0000000..9296166 --- /dev/null +++ b/.svn/pristine/24/249ba60a45df1038bd29b029022bcd6fecb9ed57.svn-base @@ -0,0 +1,346 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +#include +#include +using namespace std; + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#include +#else +#include +#endif + +#include "cvblob.h" + +namespace cvb +{ + + CvLabel cvGreaterBlob(const CvBlobs &blobs) + { + CvLabel label=0; + unsigned int maxArea=0; + + for (CvBlobs::const_iterator it=blobs.begin();it!=blobs.end();++it) + { + CvBlob *blob=(*it).second; + //if ((!blob->_parent)&&(blob->area>maxArea)) + if (blob->area>maxArea) + { + label=blob->label; + maxArea=blob->area; + } + } + + return label; + } + + void cvFilterByArea(CvBlobs &blobs, unsigned int minArea, unsigned int maxArea) + { + CvBlobs::iterator it=blobs.begin(); + while(it!=blobs.end()) + { + CvBlob *blob=(*it).second; + if ((blob->areaarea>maxArea)) + { + cvReleaseBlob(blob); + + CvBlobs::iterator tmp=it; + ++it; + blobs.erase(tmp); + } + else + ++it; + } + } + + void cvFilterByLabel(CvBlobs &blobs, CvLabel label) + { + CvBlobs::iterator it=blobs.begin(); + while(it!=blobs.end()) + { + CvBlob *blob=(*it).second; + if (blob->label!=label) + { + delete blob; + CvBlobs::iterator tmp=it; + ++it; + blobs.erase(tmp); + } + else + ++it; + } + } + + /*void cvCentralMoments(CvBlob *blob, const IplImage *img) + { + CV_FUNCNAME("cvCentralMoments"); + __CV_BEGIN__; + if (!blob->centralMoments) + { + CV_ASSERT(img&&(img->depth==IPL_DEPTH_LABEL)&&(img->nChannels==1)); + + //cvCentroid(blob); // Here? + + blob->u11=blob->u20=blob->u02=0.; + + // Only in the bounding box + int stepIn = img->widthStep / (img->depth / 8); + int img_width = img->width; + int img_height = img->height; + int img_offset = 0; + if(0 != img->roi) + { + img_width = img->roi->width; + img_height = img->roi->height; + img_offset = img->roi->xOffset + (img->roi->yOffset * stepIn); + } + + CvLabel *imgData=(CvLabel *)img->imageData + (blob->miny * stepIn) + img_offset; + for (unsigned int r=blob->miny; + rmaxy; + r++,imgData+=stepIn) + for (unsigned int c=blob->minx;cmaxx;c++) + if (imgData[c]==blob->label) + { + double tx=(c-blob->centroid.x); + double ty=(r-blob->centroid.y); + blob->u11+=tx*ty; + blob->u20+=tx*tx; + blob->u02+=ty*ty; + } + + blob->centralMoments = true; + } + __CV_END__; + }*/ + + void cvRenderBlob(const IplImage *imgLabel, CvBlob *blob, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvScalar const &color, double alpha) + { + CV_FUNCNAME("cvRenderBlob"); + __CV_BEGIN__; + + CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1)); + CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3)); + + if (mode&CV_BLOB_RENDER_COLOR) + { + int stepLbl = imgLabel->widthStep/(imgLabel->depth/8); + int stepSrc = imgSource->widthStep/(imgSource->depth/8); + int stepDst = imgDest->widthStep/(imgDest->depth/8); + int imgLabel_width = imgLabel->width; + int imgLabel_height = imgLabel->height; + int imgLabel_offset = 0; + int imgSource_width = imgSource->width; + int imgSource_height = imgSource->height; + int imgSource_offset = 0; + int imgDest_width = imgDest->width; + int imgDest_height = imgDest->height; + int imgDest_offset = 0; + if(imgLabel->roi) + { + imgLabel_width = imgLabel->roi->width; + imgLabel_height = imgLabel->roi->height; + imgLabel_offset = (imgLabel->nChannels * imgLabel->roi->xOffset) + (imgLabel->roi->yOffset * stepLbl); + } + if(imgSource->roi) + { + imgSource_width = imgSource->roi->width; + imgSource_height = imgSource->roi->height; + imgSource_offset = (imgSource->nChannels * imgSource->roi->xOffset) + (imgSource->roi->yOffset * stepSrc); + } + if(imgDest->roi) + { + imgDest_width = imgDest->roi->width; + imgDest_height = imgDest->roi->height; + imgDest_offset = (imgDest->nChannels * imgDest->roi->xOffset) + (imgDest->roi->yOffset * stepDst); + } + + CvLabel *labels = (CvLabel *)imgLabel->imageData + imgLabel_offset + (blob->miny * stepLbl); + unsigned char *source = (unsigned char *)imgSource->imageData + imgSource_offset + (blob->miny * stepSrc); + unsigned char *imgData = (unsigned char *)imgDest->imageData + imgDest_offset + (blob->miny * stepDst); + + for (unsigned int r=blob->miny; rmaxy; r++, labels+=stepLbl, source+=stepSrc, imgData+=stepDst) + for (unsigned int c=blob->minx; cmaxx; c++) + { + if (labels[c]==blob->label) + { + imgData[imgDest->nChannels*c+0] = (unsigned char)((1.-alpha)*source[imgSource->nChannels*c+0]+alpha*color.val[0]); + imgData[imgDest->nChannels*c+1] = (unsigned char)((1.-alpha)*source[imgSource->nChannels*c+1]+alpha*color.val[1]); + imgData[imgDest->nChannels*c+2] = (unsigned char)((1.-alpha)*source[imgSource->nChannels*c+2]+alpha*color.val[2]); + } + } + } + + if (mode) + { + if (mode&CV_BLOB_RENDER_TO_LOG) + { + std::clog << "Blob " << blob->label << std::endl; + std::clog << " - Bounding box: (" << blob->minx << ", " << blob->miny << ") - (" << blob->maxx << ", " << blob->maxy << ")" << std::endl; + std::clog << " - Bounding box area: " << (1 + blob->maxx - blob->minx) * (1 + blob->maxy - blob->miny) << std::endl; + std::clog << " - Area: " << blob->area << std::endl; + std::clog << " - Centroid: (" << blob->centroid.x << ", " << blob->centroid.y << ")" << std::endl; + std::clog << std::endl; + } + + if (mode&CV_BLOB_RENDER_TO_STD) + { + std::cout << "Blob " << blob->label << std::endl; + std::cout << " - Bounding box: (" << blob->minx << ", " << blob->miny << ") - (" << blob->maxx << ", " << blob->maxy << ")" << std::endl; + std::cout << " - Bounding box area: " << (1 + blob->maxx - blob->minx) * (1 + blob->maxy - blob->miny) << std::endl; + std::cout << " - Area: " << blob->area << std::endl; + std::cout << " - Centroid: (" << blob->centroid.x << ", " << blob->centroid.y << ")" << std::endl; + std::cout << std::endl; + } + + if (mode&CV_BLOB_RENDER_BOUNDING_BOX) + cvRectangle(imgDest, cvPoint(blob->minx, blob->miny), cvPoint(blob->maxx-1, blob->maxy-1), CV_RGB(255., 0., 0.)); + + if (mode&CV_BLOB_RENDER_ANGLE) + { + double angle = cvAngle(blob); + + double x1,y1,x2,y2; + double lengthLine = MAX(blob->maxx-blob->minx, blob->maxy-blob->miny)/2.; + + x1=blob->centroid.x-lengthLine*cos(angle); + y1=blob->centroid.y-lengthLine*sin(angle); + x2=blob->centroid.x+lengthLine*cos(angle); + y2=blob->centroid.y+lengthLine*sin(angle); + cvLine(imgDest,cvPoint(int(x1),int(y1)),cvPoint(int(x2),int(y2)),CV_RGB(0.,255.,0.)); + } + + if (mode&CV_BLOB_RENDER_CENTROID) + { + cvLine(imgDest,cvPoint(int(blob->centroid.x)-3,int(blob->centroid.y)),cvPoint(int(blob->centroid.x)+3,int(blob->centroid.y)),CV_RGB(0.,0.,255.)); + cvLine(imgDest,cvPoint(int(blob->centroid.x),int(blob->centroid.y)-3),cvPoint(int(blob->centroid.x),int(blob->centroid.y)+3),CV_RGB(0.,0.,255.)); + } + } + + __CV_END__; + } + + /////////////////////////////////////////////////////////////////////////////////////////////////// + // Based on http://en.wikipedia.org/wiki/HSL_and_HSV + + /// \def _HSV2RGB_(H, S, V, R, G, B) + /// \brief Color translation between HSV and RGB. +#define _HSV2RGB_(H, S, V, R, G, B) \ + { \ + double _h = H/60.; \ + int _hf = (int)floor(_h); \ + int _hi = ((int)_h)%6; \ + double _f = _h - _hf; \ + \ + double _p = V * (1. - S); \ + double _q = V * (1. - _f * S); \ + double _t = V * (1. - (1. - _f) * S); \ + \ + switch (_hi) \ + { \ + case 0: \ + R = 255.*V; G = 255.*_t; B = 255.*_p; \ + break; \ + case 1: \ + R = 255.*_q; G = 255.*V; B = 255.*_p; \ + break; \ + case 2: \ + R = 255.*_p; G = 255.*V; B = 255.*_t; \ + break; \ + case 3: \ + R = 255.*_p; G = 255.*_q; B = 255.*V; \ + break; \ + case 4: \ + R = 255.*_t; G = 255.*_p; B = 255.*V; \ + break; \ + case 5: \ + R = 255.*V; G = 255.*_p; B = 255.*_q; \ + break; \ + } \ + } + /////////////////////////////////////////////////////////////////////////////////////////////////// + + typedef std::map Palete; + + void cvRenderBlobs(const IplImage *imgLabel, CvBlobs &blobs, IplImage *imgSource, IplImage *imgDest, unsigned short mode, double alpha) + { + CV_FUNCNAME("cvRenderBlobs"); + __CV_BEGIN__; + { + + CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1)); + CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3)); + + Palete pal; + if (mode&CV_BLOB_RENDER_COLOR) + { + + unsigned int colorCount = 0; + for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it) + { + CvLabel label = (*it).second->label; + + double r, g, b; + + _HSV2RGB_((double)((colorCount*77)%360), .5, 1., r, g, b); + colorCount++; + + pal[label] = CV_RGB(r, g, b); + } + } + + for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it) + cvRenderBlob(imgLabel, (*it).second, imgSource, imgDest, mode, pal[(*it).second->label], alpha); + + } + __CV_END__; + } + + // Returns radians + double cvAngle(CvBlob *blob) + { + CV_FUNCNAME("cvAngle"); + __CV_BEGIN__; + + return .5*atan2(2.*blob->u11,(blob->u20-blob->u02)); + + __CV_END__; + } + + void cvSaveImageBlob(const char *filename, IplImage *img, CvBlob const *blob) + { + CvRect roi = cvGetImageROI(img); + cvSetImageROItoBlob(img, blob); + cvSaveImage(filename, img); + cvSetImageROI(img, roi); + } + +} + +ostream& operator<< (ostream& output, const cvb::CvBlob& b) +{ + output << b.label << ": " << b.area << ", (" << b.centroid.x << ", " << b.centroid.y << "), [(" << b.minx << ", " << b.miny << ") - (" << b.maxx << ", " << b.maxy << ")]"; + + return output; +} diff --git a/.svn/pristine/24/24d2fc5d8114c5568f0372b03b49648efa5e2988.svn-base b/.svn/pristine/24/24d2fc5d8114c5568f0372b03b49648efa5e2988.svn-base new file mode 100644 index 0000000..ae0c948 --- /dev/null +++ b/.svn/pristine/24/24d2fc5d8114c5568f0372b03b49648efa5e2988.svn-base @@ -0,0 +1,1580 @@ +/** + * @file + * @brief Efficient representation of an octree + * @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany. + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#ifndef BOCTREE_H +#define BOCTREE_H + +#include "searchTree.h" +#include "point_type.h" +#include "data_types.h" +#include "allocator.h" +#include "limits.h" +#include "nnparams.h" +#include "globals.icc" + + +#include + +#include +using std::vector; +#include +using std::deque; +#include +using std::set; +#include +using std::list; +#include +#include +#include + +#if __GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) + #define POPCOUNT(mask) __builtin_popcount(mask) +#else + #define POPCOUNT(mask) _my_popcount_3(mask) +#endif + +#include // to avoid ifdeffing for offset_ptr.get(), use &(*ptr) +namespace { namespace ip = boost::interprocess; } + + +// forward declaration +template union bitunion; + +/** + * This is our preferred representation for the leaf nodes (as it is the most compact). + * BOctTree makes an array of this, the first containing the number of points (not the + * number of coordinates) stored. + */ +template union dunion { + T v; + unsigned int length; + dunion() : length(0) {}; + +}; +// typedefs in combination with templates are weird +//typedef dunion pointrep; +#define pointrep union dunion + + + + +/** + * This struct represents the nodes of the octree + * + * child_pointer is a relative pointer to the first child of this node, as it is only + * 48 bit this will cause issues on systems with more than 268 TB of memory. All children + * of this node must be stored sequentially. If one of the children is a leaf, that + * child will be a pointer to however a set of points is represented (pointrep *). + * + * valid is a bitmask describing whether the corresponding buckets are filled. + * + * leaf is a bitmask describing whether the correpsonding bucket is a leaf node. + * + * The representation of the bitmask is somewhat inefficient. We use 16 bits for only + * 3^8 possible states, so in essence we could save 3 bits by compression. + * + */ +class bitoct{ + public: + +#ifdef _MSC_VER + __int64 child_pointer : 48; + unsigned valid : 8; + unsigned leaf : 8; +#else + signed long child_pointer : 48; + unsigned valid : 8; + unsigned leaf : 8; +#endif + /** + * sets the child pointer of parent so it points to child + */ + template + static inline void link(bitoct &parent, bitunion *child) { + parent.child_pointer = (long)((char*)child - (char*)&parent); + } + + /** + * Returns the children of this node (given as parent). + */ + template + static inline void getChildren(const bitoct &parent, bitunion* &children) { + children = (bitunion*)((char*)&parent + parent.child_pointer); + } + + template + inline bitunion* getChild(unsigned char index) { + bitunion *children = (bitunion*)((char*)this + this->child_pointer); + for (unsigned char i = 0; i < index; i++) { + if ( ( 1 << i ) & valid ) { // if ith node exists + children++; + } + } + return children; + } +}; + + +/** + * This union combines an octree node with a pointer to a set of points. This allows + * us to use both nodes and leaves interchangeably. + * + * points is a pointer to the point representation in use + * + * node is simply the octree node + * + */ +template union bitunion { + pointrep *points; + //union dunion *points; + bitoct node; + + bitunion(pointrep *p) : points(p) {}; + bitunion(bitoct b) : node(b) {}; + bitunion() : points(0) { + node.child_pointer = 0; + node.valid = 0; + node.leaf = 0; + }; // needed for new [] + + //! Leaf node: links a pointrep array [length+values] to this union, saved as an offset pointer + static inline void link(bitunion* leaf, pointrep* points) { + // use node child_pointer as offset_ptr, not pointrep + leaf->node.child_pointer = (long)((char*)points - (char*)leaf); + } + + //! Leaf node: points in the array + inline T* getPoints() const { + // absolute pointer + //return &(this->points[1].v); + // offset pointer + return reinterpret_cast( + reinterpret_cast((char*)this + node.child_pointer) + 1 + ); + } + + //! Leaf node: length in the array + inline unsigned int getLength() const { + // absolute pointer + //return this->points[0].length; + // offset pointer + return (reinterpret_cast((char*)this + node.child_pointer))[0].length; + } + + //! Leaf node: all points + inline pointrep* getPointreps() const { + return reinterpret_cast((char*)this + node.child_pointer); + } + + inline bitunion* getChild(unsigned char index) const { + bitunion *children = (bitunion*)((char*)this + this->node.child_pointer); + for (unsigned char i = 0; i < index; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + children++; + } + } + return children; + } + + inline bool isValid(unsigned char index) { + return ( ( 1 << index ) & node.valid ); + } + /* + inline pointrep* getChild(unsigned char index) { + bitunion *children = (bitunion*)((char*)this + this->node.child_pointer); + return children[index].points; + }*/ + + inline bool childIsLeaf(unsigned char index) { + return ( ( 1 << index ) & node.leaf ); // if ith node is leaf get center + } +}; + + +// initialized in Boctree.cc, sequence intialized on startup +extern char amap[8][8]; +extern char imap[8][8]; +extern char sequence2ci[8][256][8]; // maps preference to index in children array for every valid_mask and every case + + + +/** + * @brief Octree + * + * A cubic bounding box is calculated + * from the given 3D points. Then it + * is recusivly subdivided into smaller + * subboxes + */ +template +class BOctTree : public SearchTree { +public: + BOctTree() { + } + + template + BOctTree(P * const* pts, int n, T voxelSize, PointType _pointtype = PointType(), bool _earlystop = false ) : pointtype(_pointtype), earlystop(_earlystop) + { + alloc = new PackedChunkAllocator; + + this->voxelSize = voxelSize; + + this->POINTDIM = pointtype.getPointDim(); + + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + + // initialising + for (unsigned int i = 0; i < POINTDIM; i++) { + mins[i] = pts[0][i]; + maxs[i] = pts[0][i]; + } + + for (unsigned int i = 0; i < POINTDIM; i++) { + for (int j = 1; j < n; j++) { + mins[i] = min(mins[i], (T)pts[j][i]); + maxs[i] = max(maxs[i], (T)pts[j][i]); + } + } + + center[0] = 0.5 * (mins[0] + maxs[0]); + center[1] = 0.5 * (mins[1] + maxs[1]); + center[2] = 0.5 * (mins[2] + maxs[2]); + size = max(max(0.5 * (maxs[0] - mins[0]), 0.5 * (maxs[1] - mins[1])), 0.5 * (maxs[2] - mins[2])); + size += 1.0; // for numerical reasons we increase size + + // calculate new buckets + T newcenter[8][3]; + T sizeNew = size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(center, newcenter[i], size, i); + } + // set up values + uroot = alloc->allocate >(); + root = &uroot->node; + + countPointsAndQueueFast(pts, n, newcenter, sizeNew, *root, center); + init(); + } + + BOctTree(std::string filename) { + alloc = new PackedChunkAllocator; + deserialize(filename); + init(); + } + + template + BOctTree(vector

&pts, T voxelSize, PointType _pointtype = PointType(), bool _earlystop = false) : earlystop(_earlystop) + { + alloc = new PackedChunkAllocator; + + this->voxelSize = voxelSize; + + this->POINTDIM = pointtype.getPointDim(); + + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + + // initialising + for (unsigned int i = 0; i < POINTDIM; i++) { + mins[i] = pts[0][i]; + maxs[i] = pts[0][i]; + } + + for (unsigned int i = 0; i < POINTDIM; i++) { + for (unsigned int j = 1; j < pts.size(); j++) { + mins[i] = min(mins[i], pts[j][i]); + maxs[i] = max(maxs[i], pts[j][i]); + } + } + + center[0] = 0.5 * (mins[0] + maxs[0]); + center[1] = 0.5 * (mins[1] + maxs[1]); + center[2] = 0.5 * (mins[2] + maxs[2]); + size = max(max(0.5 * (maxs[0] - mins[0]), 0.5 * (maxs[1] - mins[1])), 0.5 * (maxs[2] - mins[2])); + + size += 1.0; // for numerical reasons we increase size + + // calculate new buckets + T newcenter[8][3]; + T sizeNew = size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(center, newcenter[i], size, i); + } + // set up values + uroot = alloc->allocate >(); + root = &uroot->node; + + countPointsAndQueue(pts, newcenter, sizeNew, *root, center); + } + + virtual ~BOctTree() + { + if(alloc) { + delete alloc; + } + } + + void init() { + // compute maximal depth as well as the size of the smalles leaf + real_voxelSize = size; + max_depth = 1; + while (real_voxelSize > voxelSize) { + real_voxelSize = real_voxelSize/2.0; + max_depth++; + } + + child_bit_depth = alloc->allocate(max_depth); + child_bit_depth_inv = alloc->allocate(max_depth); + + for(int d=0; d < max_depth; d++) { + child_bit_depth[d] = 1 << (max_depth - d - 1); + child_bit_depth_inv[d] = ~child_bit_depth[d]; + } + + mult = 1.0/real_voxelSize; + add[0] = -center[0] + size; + add[1] = -center[1] + size; + add[2] = -center[2] + size; + + largest_index = child_bit_depth[0] * 2 -1; + } + +protected: + + /** + * Serialization critical variables + */ + //! the root of the octree + ip::offset_ptr root; + ip::offset_ptr > uroot; + + //! storing the center + T center[3]; + + //! storing the dimension + T size; + + //! storing the voxel size + T voxelSize; + + //! The real voxelsize of the leaves + T real_voxelSize; + + //! Offset and real voxelsize inverse factor for manipulation points + T add[3]; + T mult; + + //! Dimension of each point: 3 (xyz) + N (attributes) + unsigned int POINTDIM; + + //! storing minimal and maximal values for all dimensions + ip::offset_ptr mins; + ip::offset_ptr maxs; + + //! Details of point attributes + PointType pointtype; + + //! ? + unsigned char max_depth; + ip::offset_ptr child_bit_depth; + ip::offset_ptr child_bit_depth_inv; + int largest_index; + + /** + * Serialization uncritical, runtime relevant variables + */ + + //! Threadlocal storage of parameters used in SearchTree operations + static NNParams params[100]; + + /** + * Serialization uncritical, runtime irrelevant variables (constructor-stuff) + */ + + //! Whether to stop subdividing at N<10 nodes or not + bool earlystop; + + //! Allocator used for creating nodes in the constructor + Allocator* alloc; + +public: + + inline const T* getMins() const { return &(*mins); } + inline const T* getMaxs() const { return &(*maxs); } + inline const T* getCenter() const { return center; } + inline T getSize() const { return size; } + inline unsigned int getPointdim() const { return POINTDIM; } + inline const bitoct& getRoot() const { return *root; } + inline unsigned int getMaxDepth() const { return max_depth; } + + inline void getCenter(double _center[3]) const { + _center[0] = center[0]; + _center[1] = center[1]; + _center[2] = center[2]; + } + + void GetOctTreeCenter(vector&c) { GetOctTreeCenter(c, *root, center, size); } + void GetOctTreeRandom(vector&c) { GetOctTreeRandom(c, *root); } + void GetOctTreeRandom(vector&c, unsigned int ptspervoxel) { GetOctTreeRandom(c, ptspervoxel, *root); } + void AllPoints(vector &vp) { AllPoints(*BOctTree::root, vp); } + + long countNodes() { return 1 + countNodes(*root); } // computes number of inner nodes + long countLeaves() { return countLeaves(*root); } // computes number of leaves + points + long countOctLeaves() { return countOctLeaves(*root); } // computes number of leaves + + void deserialize(std::string filename ) { + char buffer[sizeof(T) * 20]; + T *p = reinterpret_cast(buffer); + + std::ifstream file; + file.open (filename.c_str(), std::ios::in | std::ios::binary); + + // read magic bits + file.read(buffer, 2); + if ( buffer[0] != 'X' || buffer[1] != 'T') { + std::cerr << "Not an octree file!!" << endl; + file.close(); + return; + } + + // read header + pointtype = PointType::deserialize(file); + + file.read(buffer, 5 * sizeof(T)); + voxelSize = p[0]; + center[0] = p[1]; + center[1] = p[2]; + center[2] = p[3]; + size = p[4]; + + file.read(buffer, sizeof(int)); + int *ip = reinterpret_cast(buffer); + POINTDIM = *ip; + + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + + file.read(reinterpret_cast(&(*mins)), POINTDIM * sizeof(T)); + file.read(reinterpret_cast(&(*maxs)), POINTDIM * sizeof(T)); + + // read root node + uroot = alloc->allocate >(); + root = &uroot->node; + + deserialize(file, *root); + file.close(); + } + + static void deserialize(std::string filename, vector &points ) { + char buffer[sizeof(T) * 20]; + + std::ifstream file; + file.open (filename.c_str(), std::ios::in | std::ios::binary); + + // read magic bits + file.read(buffer, 2); + if ( buffer[0] != 'X' || buffer[1] != 'T') { + std::cerr << "Not an octree file!!" << endl; + file.close(); + return; + } + + // read header + PointType pointtype = PointType::deserialize(file); + + file.read(buffer, 5 * sizeof(T)); // read over voxelsize, center and size + file.read(buffer, sizeof(int)); + + int *ip = reinterpret_cast(buffer); + unsigned int POINTDIM = *ip; + + file.read(buffer, POINTDIM * sizeof(T)); + file.read(buffer, POINTDIM * sizeof(T)); + + // read root node + deserialize(file, points, pointtype); + file.close(); + } + + void serialize(std::string filename) { + char buffer[sizeof(T) * 20]; + T *p = reinterpret_cast(buffer); + + std::ofstream file; + file.open (filename.c_str(), std::ios::out | std::ios::binary); + + // write magic bits + buffer[0] = 'X'; + buffer[1] = 'T'; + file.write(buffer, 2); + + // write header + pointtype.serialize(file); + + p[0] = voxelSize; + p[1] = center[0]; + p[2] = center[1]; + p[3] = center[2]; + p[4] = size; + + int *ip = reinterpret_cast(&(buffer[5 * sizeof(T)])); + *ip = POINTDIM; + + file.write(buffer, 5 * sizeof(T) + sizeof(int)); + + + for (unsigned int i = 0; i < POINTDIM; i++) { + p[i] = mins[i]; + } + for (unsigned int i = 0; i < POINTDIM; i++) { + p[i+POINTDIM] = maxs[i]; + } + + file.write(buffer, 2*POINTDIM * sizeof(T)); + + // write root node + serialize(file, *root); + + file.close(); + } + + static PointType readType(std::string filename ) { + char buffer[sizeof(T) * 20]; + + std::ifstream file; + file.open (filename.c_str(), std::ios::in | std::ios::binary); + + // read magic bits + file.read(buffer, 2); + if ( buffer[0] != 'X' || buffer[1] != 'T') { + std::cerr << "Not an octree file!!" << endl; + file.close(); + return PointType(); + } + + // read header + PointType pointtype = PointType::deserialize(file); + + file.close(); + + return pointtype; + } + + + /** + * Picks the first point in depth first order starting from the given node + * + */ + T* pickPoint(bitoct &node) { + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + // absolute pointer + //return &(children->points[1].v); + // offset pointer + return children->getPoints(); + } else { // recurse + return pickPoint(children->node); + } + ++children; // next child + } + } + return 0; + } + + static void childcenter(const T *pcenter, T *ccenter, T size, unsigned char i) { + switch (i) { + case 0: // 000 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 1: // 001 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 2: // 010 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 3: // 011 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 4: // 100 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 5: // 101 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 6: // 110 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 7: // 111 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + default: + break; + } + } + + static void childcenter(int x, int y, int z, int &cx, int &cy, int &cz, char i, int size) { + switch (i) { + case 0: // 000 + cx = x - size ; + cy = y - size ; + cz = z - size ; + break; + case 1: // 001 + cx = x + size ; + cy = y - size ; + cz = z - size ; + break; + case 2: // 010 + cx = x - size ; + cy = y + size ; + cz = z - size ; + break; + case 3: // 011 + cx = x + size ; + cy = y + size ; + cz = z - size ; + break; + case 4: // 100 + cx = x - size ; + cy = y - size ; + cz = z + size ; + break; + case 5: // 101 + cx = x + size ; + cy = y - size ; + cz = z + size ; + break; + case 6: // 110 + cx = x - size ; + cy = y + size ; + cz = z + size ; + break; + case 7: // 111 + cx = x + size ; + cy = y + size ; + cz = z + size ; + break; + default: + break; + } + } + +protected: + + void AllPoints( bitoct &node, vector &vp) { + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + T *point = &(points[1].v); // first point + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + //T *p = new T[BOctTree::POINTDIM]; +// T *p = new T[3]; +// p[0] = point[0]; p[1] = point[1]; p[2] = point[2]; + T *p = new T[BOctTree::POINTDIM]; + for (unsigned int k = 0; k < BOctTree::POINTDIM; k++) + p[k] = point[k]; + + vp.push_back(p); + + //glVertex3f( point[0], point[1], point[2]); + point+=BOctTree::POINTDIM; + } + } else { // recurse + AllPoints( children->node, vp); + } + ++children; // next child + } + } + } + + static void deserialize(std::ifstream &f, vector &vpoints, PointType &pointtype) { + char buffer[2]; + pointrep *point = new pointrep[pointtype.getPointDim()]; + f.read(buffer, 2); + bitoct node; + node.valid = buffer[0]; + node.leaf = buffer[1]; + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf read points + pointrep first; + f.read(reinterpret_cast(&first), sizeof(pointrep)); + unsigned int length = first.length; // read first element, which is the length + for (unsigned int k = 0; k < length; k++) { + f.read(reinterpret_cast(point), sizeof(pointrep) * pointtype.getPointDim()); // read the points + vpoints.push_back( pointtype.createPoint( &(point->v ) ) ); + } + } else { // write child + deserialize(f, vpoints, pointtype); + } + } + } + delete [] point; + } + + void deserialize(std::ifstream &f, bitoct &node) { + char buffer[2]; + f.read(buffer, 2); + node.valid = buffer[0]; + node.leaf = buffer[1]; + + unsigned short n_children = POPCOUNT(node.valid); + + // create children + bitunion *children = alloc->allocate >(n_children); + bitoct::link(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf read points + pointrep first; + f.read(reinterpret_cast(&first), sizeof(pointrep)); + unsigned int length = first.length; // read first element, which is the length + pointrep *points = alloc->allocate (POINTDIM*length + 1); + // absolute pointer + //children->points = points; + // offset pointer + bitunion::link(children, points); + points[0] = first; + points++; + f.read(reinterpret_cast(points), sizeof(pointrep) * length * POINTDIM); // read the points + } else { // write child + deserialize(f, children->node); + } + ++children; // next child + } + } + } + + void serialize(std::ofstream &of, bitoct &node) { + char buffer[2]; + buffer[0] = node.valid; + buffer[1] = node.leaf; + of.write(buffer, 2); + + + // write children + bitunion *children; + bitoct::getChildren(node, children); + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf write points + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + of.write(reinterpret_cast(points), sizeof(pointrep) * (length * POINTDIM +1)); + } else { // write child + serialize(of, children->node); + } + ++children; // next child + } + } + } + + void GetOctTreeCenter(vector&c, bitoct &node, T *center, T size) { + T ccenter[3]; + bitunion *children; + bitoct::getChildren(node, children); + + for (unsigned char i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + childcenter(center, ccenter, size, i); // childrens center + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center + T * cp = new T[POINTDIM]; + // compute average of reflectance values and store in last position of cp + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + T *point = &(points[1].v); + float reflectance_center = 0.; + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + reflectance_center += point[POINTDIM-1]; // add current reflectance + point+=BOctTree::POINTDIM; + } + reflectance_center /= length * 1.0; + + for (unsigned int iterator = 0; iterator < POINTDIM-1; iterator++) { + cp[iterator] = ccenter[iterator]; + } + cp[POINTDIM-1] = reflectance_center; // reflectance is the last dimension in POINTDIM + c.push_back(cp); + + } else { // recurse + GetOctTreeCenter(c, children->node, ccenter, size/2.0); + } + ++children; // next child + } + } + } + + void GetOctTreeRandom(vector&c, bitoct &node) { + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + // new version to ignore leaves with less than 3 points + /* + if(points[0].length > 2) { + for(int tmp = 0; tmp < points[0].length; tmp++) { + T *point = &(points[POINTDIM*tmp+1].v); + c.push_back(point); + } + } + */ + //old version + + int tmp = rand(points[0].length); + T *point = &(points[POINTDIM*tmp+1].v); + c.push_back(point); + + + } else { // recurse + GetOctTreeRandom(c, children->node); + } + ++children; // next child + } + } + } + + void GetOctTreeRandom(vector&c, unsigned int ptspervoxel, bitoct &node) { + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + if (ptspervoxel >= length) { + for (unsigned int j = 0; j < length; j++) + c.push_back(&(points[POINTDIM*j+1].v)); + + ++children; // next child + continue; + } + set indices; + while(indices.size() < ptspervoxel) { + int tmp = rand(length-1); + indices.insert(tmp); + } + for(set::iterator it = indices.begin(); it != indices.end(); it++) + c.push_back(&(points[POINTDIM*(*it)+1].v)); + + } else { // recurse + GetOctTreeRandom(c, ptspervoxel, children->node); + } + ++children; // next child + } + } + } + + long countNodes(bitoct &node) { + long result = 0; + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + //++result; + } else { // recurse + result += countNodes(children->node) + 1; + } + ++children; // next child + } + } + return result; + } + + long countLeaves(bitoct &node) { + long result = 0; + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + long nrpts = children->getLength(); + result += POINTDIM*nrpts; + } else { // recurse + result += countLeaves(children->node); + } + ++children; // next child + } + } + return result; + } + + long countOctLeaves(bitoct &node) { + long result = 0; + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + result ++; + } else { // recurse + result += countTrueLeaves(children->node); + } + ++children; // next child + } + } + return result; + } + + // TODO: is this still needed? nodes and pointreps are all in the Allocator + void deletetNodes(bitoct &node) { + bitunion *children; + bitoct::getChildren(node, children); + bool haschildren = false; + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf + // absolute pointer + //delete [] children->points; + // offset pointer + delete [] children->getPointreps(); + } else { // recurse + deletetNodes(children->node); + } + ++children; // next child + haschildren = true; + } + } + // delete children + if (haschildren) { + bitoct::getChildren(node, children); + } + } + + template + void* branch( bitoct &node, P * const * splitPoints, int n, T _center[3], T _size) { + // if bucket is too small stop building tree + // ----------------------------------------- + if ((_size <= voxelSize) || (earlystop && n <= 10) ) { + + // copy points + pointrep *points = alloc->allocate (POINTDIM*n + 1); + + points[0].length = n; + int i = 1; + for (int j = 0; j < n; j++) { + for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) { + points[i++].v = splitPoints[j][iterator]; + } + } + return points; + } + + // calculate new buckets + T newcenter[8][3]; + T sizeNew; + + sizeNew = _size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(_center, newcenter[i], _size, i); + } + + countPointsAndQueueFast(splitPoints, n, newcenter, sizeNew, node, _center); + return 0; + } + + template + void* branch( bitoct &node, vector &splitPoints, T _center[3], T _size) { + // if bucket is too small stop building tree + // ----------------------------------------- + if ((_size <= voxelSize) || (earlystop && splitPoints.size() <= 10) ) { + // copy points + pointrep *points = alloc->allocate (POINTDIM*splitPoints.size() + 1); + points[0].length = splitPoints.size(); + int i = 1; + for (typename vector

::iterator itr = splitPoints.begin(); + itr != splitPoints.end(); itr++) { + for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) { + points[i++].v = (*itr)[iterator]; + } + } + return points; + } + + // calculate new buckets + T newcenter[8][3]; + T sizeNew; + + sizeNew = _size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(_center, newcenter[i], _size, i); + } + + countPointsAndQueue(splitPoints, newcenter, sizeNew, node, _center); + return 0; + } + + template + void countPointsAndQueue(vector &i_points, T center[8][3], T size, bitoct &parent, T *pcenter) { + vector points[8]; + int n_children = 0; + for (typename vector

::iterator itr = i_points.begin(); itr != i_points.end(); itr++) { + points[childIndex

(pcenter, *itr)].push_back( *itr ); + } + + i_points.clear(); + vector().swap(i_points); + for (int j = 0; j < 8; j++) { + if (!points[j].empty()) { + parent.valid = ( 1 << j ) | parent.valid; + ++n_children; + } + } + // create children + bitunion *children = alloc->allocate >(n_children); + bitoct::link(parent, children); + + int count = 0; + for (int j = 0; j < 8; j++) { + if (!points[j].empty()) { + pointrep *c = (pointrep*)branch(children[count].node, points[j], center[j], size); // leaf node + if (c) { + // absolute pointer + //children[count].points = c; // set this child to deque of points + // offset pointer + bitunion::link(&children[count], c); + parent.leaf = ( 1 << j ) | parent.leaf; // remember this is a leaf + } + points[j].clear(); + vector().swap(points[j]); + ++count; + } + } + } + + template + void countPointsAndQueueFast(P * const* points, int n, T center[8][3], T size, bitoct &parent, T pcenter[3]) { + P * const *blocks[9]; + blocks[0] = points; + blocks[8] = points + n; + fullsort(points, n, pcenter, blocks+1); + + int n_children = 0; + + for (int j = 0; j < 8; j++) { + // if non-empty set valid flag for this child + if (blocks[j+1] - blocks[j] > 0) { + parent.valid = ( 1 << j ) | parent.valid; + ++n_children; + } + } + + // create children + bitunion *children = alloc->allocate >(n_children); + bitoct::link(parent, children); + int count = 0; + for (int j = 0; j < 8; j++) { + if (blocks[j+1] - blocks[j] > 0) { + pointrep *c = (pointrep*)branch(children[count].node, blocks[j], blocks[j+1] - blocks[j], center[j], size); // leaf node + if (c) { + // absolute pointer + //children[count].points = c; // set this child to vector of points + // offset pointer + bitunion::link(&children[count], c); // set this child to vector of points + parent.leaf = ( 1 << j ) | parent.leaf; // remember this is a leaf + } + ++count; + } + } + } + + + void getByIndex(T *point, T *&points, unsigned int &length) { + unsigned int x,y,z; + x = (point[0] + add[0]) * mult; + y = (point[1] + add[1]) * mult; + z = (point[2] + add[2]) * mult; + + bitunion *node = uroot; + unsigned char child_index; + unsigned int child_bit; + unsigned int depth = 0; + + while (true) { + child_bit = child_bit_depth[depth]; + child_index = ((x & child_bit )!=0) | (((y & child_bit )!=0 )<< 1) | (((z & child_bit )!=0) << 2); + if (node->childIsLeaf(child_index) ) { + node = node->getChild(child_index); + points = node->getPoints(); + length = node->getLength(); + return; + } else { + node = node->getChild(child_index); + } + depth++; + } + } + + template + inline unsigned char childIndex(const T *center, const P *point) { + return (point[0] > center[0] ) | ((point[1] > center[1] ) << 1) | ((point[2] > center[2] ) << 2) ; + } + + /** + * Given a leaf node, this function looks for the closest point to params[threadNum].closest + * in the list of points. + */ + inline void findClosestInLeaf(bitunion *node, int threadNum) const { + if (params[threadNum].count >= params[threadNum].max_count) return; + params[threadNum].count++; + T* points = node->getPoints(); + unsigned int length = node->getLength(); + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + double myd2 = Dist2(params[threadNum].p, points); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = points; + if (myd2 <= 0.0001) { + params[threadNum].closest_v = 0; // the search radius in units of voxelSize + } else { + params[threadNum].closest_v = sqrt(myd2) * mult + 1; // the search radius in units of voxelSize + } + } + points+=BOctTree::POINTDIM; + } + } + + + +/** + * This function finds the closest point in the octree given a specified + * radius. This implementation is quit complex, although it is already + * simplified. The simplification incurs a significant loss in speed, as + * several calculations have to be performed repeatedly and a high number of + * unnecessary jumps are executed. + */ + double *FindClosest(double *point, double maxdist2, int threadNum) const + { + params[threadNum].closest = 0; // no point found currently + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = point; + params[threadNum].x = (point[0] + add[0]) * mult; + params[threadNum].y = (point[1] + add[1]) * mult; + params[threadNum].z = (point[2] + add[2]) * mult; + params[threadNum].closest_v = sqrt(maxdist2) * mult + 1; // the search radius in units of voxelSize + params[threadNum].count = 0; + params[threadNum].max_count = 10000; // stop looking after this many buckets + + + // box within bounds in voxel coordinates + int xmin, ymin, zmin, xmax, ymax, zmax; + xmin = max(params[threadNum].x-params[threadNum].closest_v, 0); + ymin = max(params[threadNum].y-params[threadNum].closest_v, 0); + zmin = max(params[threadNum].z-params[threadNum].closest_v, 0); + +// int largest_index = child_bit_depth[0] * 2 -1; + + xmax = min(params[threadNum].x+params[threadNum].closest_v, largest_index); + ymax = min(params[threadNum].y+params[threadNum].closest_v, largest_index); + zmax = min(params[threadNum].z+params[threadNum].closest_v, largest_index); + + unsigned char depth = 0; + unsigned int child_bit; + unsigned int child_index_min; + unsigned int child_index_max; + + bitunion *node = &(*uroot); + + int cx, cy, cz; + + child_bit = child_bit_depth[depth]; + cx = child_bit_depth[depth]; + cy = child_bit_depth[depth]; + cz = child_bit_depth[depth]; + + while (true) { // find the first node where branching is required + child_index_min = ((xmin & child_bit )!=0) | (((ymin & child_bit )!=0 )<< 1) | (((zmin & child_bit )!=0) << 2); + child_index_max = ((xmax & child_bit )!=0) | (((ymax & child_bit )!=0 )<< 1) | (((zmax & child_bit )!=0) << 2); + + // if these are the same, go there + // TODO: optimization: also traverse if only single child... + if (child_index_min == child_index_max) { + if (node->childIsLeaf(child_index_min) ) { // luckily, no branching is required + findClosestInLeaf(node->getChild(child_index_min), threadNum); + return static_cast(params[threadNum].closest); + } else { + if (node->isValid(child_index_min) ) { // only descend when there is a child + childcenter(cx,cy,cz, cx,cy,cz, child_index_min, child_bit/2 ); + node = node->getChild(child_index_min); + child_bit /= 2; + } else { // there is no child containing the bounding box => no point is close enough + return 0; + } + } + } else { + // if min and max are not in the same child we must branch + break; + } + } + + // node contains all box-within-bounds cells, now begin best bin first search + _FindClosest(threadNum, node->node, child_bit/2, cx, cy, cz); + return static_cast(params[threadNum].closest); + } + + /** + * This is the heavy duty search function doing most of the (theoretically unneccesary) work. The tree is recursively searched. + * Depending on which of the 8 child-voxels is closer to the query point, the children are examined in a special order. + * This order is defined in map, imap is its inverse and sequence2ci is a speedup structure for faster access to the child indices. + */ + void _FindClosest(int threadNum, bitoct &node, int size, int x, int y, int z) const + { + // Recursive case + + // compute which child is closest to the query point + unsigned char child_index = ((params[threadNum].x - x) >= 0) | + (((params[threadNum].y - y) >= 0) << 1) | + (((params[threadNum].z - z) >= 0) << 2); + + char *seq2ci = sequence2ci[child_index][node.valid]; // maps preference to index in children array + char *mmap = amap[child_index]; // maps preference to area index + + bitunion *children; + bitoct::getChildren(node, children); + int cx, cy, cz; + cx = cy = cz = 0; // just to shut up the compiler warnings + for (unsigned char i = 0; i < 8; i++) { // in order of preference + child_index = mmap[i]; // the area index of the node + if ( ( 1 << child_index ) & node.valid ) { // if ith node exists + childcenter(x,y,z, cx,cy,cz, child_index, size); + if ( params[threadNum].closest_v == 0 || max(max(abs( cx - params[threadNum].x ), + abs( cy - params[threadNum].y )), + abs( cz - params[threadNum].z )) - size + > params[threadNum].closest_v ) { + continue; + } + // find the closest point in leaf seq2ci[i] + if ( ( 1 << child_index ) & node.leaf ) { // if ith node is leaf + findClosestInLeaf( &children[seq2ci[i]], threadNum); + } else { // recurse + _FindClosest(threadNum, children[seq2ci[i]].node, size/2, cx, cy, cz); + } + } + } + } + + + /** + * This function shows the possible speedup that can be gained by using the + * octree for nearest neighbour search, if a more sophisticated + * implementation were given. Here, only the bucket in which the query point + * falls is looked up. If doing the same thing in the kd-tree search, this + * function is about 3-5 times as fast + */ + double *FindClosestInBucket(double *point, double maxdist2, int threadNum) { + params[threadNum].closest = 0; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = point; + unsigned int x,y,z; + x = (point[0] + add[0]) * mult; + y = (point[1] + add[1]) * mult; + z = (point[2] + add[2]) * mult; + T * points; + unsigned int length; + + bitunion *node = uroot; + unsigned char child_index; + + unsigned int child_bit = child_bit_depth[0]; + + while (true) { + child_index = ((x & child_bit )!=0) | (((y & child_bit )!=0 )<< 1) | (((z & child_bit )!=0) << 2); + if (node->childIsLeaf(child_index) ) { + node = node->getChild(child_index); + points = node->getPoints(); + length = node->getLength(); + + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + double myd2 = Dist2(params[threadNum].p, points); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = points; + } + points+=BOctTree::POINTDIM; + } + return static_cast(params[threadNum].closest); + } else { + if (node->isValid(child_index) ) { + node = node->getChild(child_index); + } else { + return 0; + } + } + child_bit >>= 1; + } + return static_cast(params[threadNum].closest); + } + + +template + void fullsort(P * const * points, int n, T splitval[3], P * const * blocks[9]) { + P* const * L0; + P* const * L1; + P* const * L2; + unsigned int n0L, n0R, n1L, n1R ; + + // sort along Z + L0 = sort(points, n, splitval[2], 2); + + n0L = L0 - points; + // sort along Y (left of Z) points -- L0 + L1 = sort(points, n0L, splitval[1], 1); + + n1L = L1 - points; + // sort along X (left of Y) points -- L1 + L2 = sort(points, n1L, splitval[0], 0); + + blocks[0] = L2; + + n1R = n0L - n1L; + // sort along X (right of Y) // L1 -- L0 + L2 = sort(L1, n1R, splitval[0], 0); + + blocks[1] = L1; + blocks[2] = L2; + + n0R = n - n0L; + // sort along Y (right of Z) L0 -- end + L1 = sort(L0, n0R, splitval[1], 1); + + n1L = L1 - L0; + // sort along X (left of Y) points -- L1 + L2 = sort(L0, n1L, splitval[0], 0); + + blocks[3] = L0; + blocks[4] = L2; + + n1R = n0R - n1L; + // sort along X (right of Y) // L1 -- L0 + L2 = sort(L1, n1R, splitval[0], 0); + + blocks[5] = L1; + blocks[6] = L2; + } + + + template + P* const * sort(P* const * points, unsigned int n, T splitval, unsigned char index) { + if (n==0) return points; + + if (n==1) { + if (points[0][index] < splitval) + return points+1; + else + return points; + } + + P **left = const_cast(points); + P **right = const_cast(points + n - 1); + + + while (1) { + while ((*left)[index] < splitval) + { + left++; + if (right < left) + break; + } + while ((*right)[index] >= splitval) + { + right--; + if (right < left) + break; + } + if (right < left) + break; + + std::swap(*left, *right); + } + return left; + } + +public: + /** + * Copies another (via new constructed) octtree into cache allocated memory and makes it position independant + */ + BOctTree(const BOctTree& other, unsigned char* mem_ptr, unsigned int mem_max) + { + alloc = new SequentialAllocator(mem_ptr, mem_max); + + // "allocate" space for *this + alloc->allocate >(); + + // take members + unsigned int i; + for(i = 0; i < 3; ++i) + center[i] = other.center[i]; + size = other.size; + voxelSize = other.voxelSize; + real_voxelSize = other.real_voxelSize; + for(i = 0; i < 3; ++i) + add[i] = other.add[i]; + mult = other.mult; + POINTDIM = other.POINTDIM; + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + for(i = 0; i < POINTDIM; ++i) { + mins[i] = other.mins[i]; + maxs[i] = other.maxs[i]; + } + pointtype = other.pointtype; + max_depth = other.max_depth; + child_bit_depth = alloc->allocate(max_depth); + child_bit_depth_inv = alloc->allocate(max_depth); + for(i = 0; i < max_depth; ++i) { + child_bit_depth[i] = other.child_bit_depth[i]; + child_bit_depth_inv[i] = other.child_bit_depth_inv[i]; + } + largest_index = other.largest_index; + + // take node structure + uroot = alloc->allocate >(); + root = &uroot->node; + copy_children(*other.root, *root); + + // test if allocator has used up his space + //alloc->printSize(); + + // discard allocator, space is managed by the cache manager + delete alloc; alloc = 0; + } + +private: + void copy_children(const bitoct& other, bitoct& my) { + // copy node attributes + my.valid = other.valid; + my.leaf = other.leaf; + + // other children + bitunion* other_children; + bitoct::getChildren(other, other_children); + + // create own children + unsigned int n_children = POPCOUNT(other.valid); + bitunion* my_children = alloc->allocate >(n_children); + bitoct::link(my, my_children); + + // iterate over all (valid) children and copy them + for(unsigned int i = 0; i < 8; ++i) { + if((1<getLength(); + pointrep* other_pointreps = other_children->getPointreps(); + pointrep* my_pointreps = alloc->allocate(POINTDIM * length + 1); + for(unsigned int j = 0; j < POINTDIM * length + 1; ++j) + my_pointreps[j] = other_pointreps[j]; + // assign + bitunion::link(my_children, my_pointreps); + } else { + // child is already created, copy and create children for it + copy_children(other_children->node, my_children->node); + } + ++other_children; + ++my_children; + } + } + } + +public: + //! Size of the whole tree structure, including the main class, its serialize critical allocated variables and nodes, not the allocator + unsigned int getMemorySize() + { + return sizeof(*this) // all member variables + + 2*POINTDIM*sizeof(T) // mins, maxs + + 2*max_depth*sizeof(unsigned int) // child_bit_depth(_inv) + + sizeof(bitunion) // uroot + + sizeChildren(*root); // all nodes + } + +private: + //! Recursive size of a node's children + unsigned int sizeChildren(const bitoct& node) { + unsigned int s = 0; + bitunion* children; + bitoct::getChildren(node, children); + + // size of children allocation + unsigned int n_children = POPCOUNT(node.valid); + s += sizeof(bitunion)*n_children; + + // iterate over all (valid) children and sum them up + for(unsigned int i = 0; i < 8; ++i) { + if((1<getLength()*POINTDIM+1); + } else { + // childe node is already accounted for, add its children + s += sizeChildren(children->node); + } + ++children; // next (valid) child + } + } + return s; + } +}; + +typedef SingleObject > DataOcttree; + +template +NNParams BOctTree::params[100]; + +#endif diff --git a/.svn/pristine/2c/2c4034be4395b561e9d9827bded810ae7e8ca510.svn-base b/.svn/pristine/2c/2c4034be4395b561e9d9827bded810ae7e8ca510.svn-base new file mode 100644 index 0000000..4d56468 --- /dev/null +++ b/.svn/pristine/2c/2c4034be4395b561e9d9827bded810ae7e8ca510.svn-base @@ -0,0 +1,440 @@ +/* + * basicScan implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/basicScan.h" + +#include "scanio/scan_io.h" +#include "slam6d/kd.h" +#include "slam6d/Boctree.h" +#include "slam6d/ann_kd.h" + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif //WITH_METRICS + +#include +#include +#include +using std::ifstream; +using std::ofstream; +using std::flush; +using std::string; +using std::map; +using std::pair; +using std::vector; + +#include +using namespace boost::filesystem; + + + +void BasicScan::openDirectory(const std::string& path, IOType type, int start, int end) +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::read_scan_time.start(); +#endif //WITH_METRICS + + // create an instance of ScanIO + ScanIO* sio = ScanIO::getScanIO(type); + + // query available scans in the directory from the ScanIO + std::list identifiers(sio->readDirectory(path.c_str(), start, end)); + + Scan::allScans.reserve(identifiers.size()); + + // for each identifier, create a scan + for(std::list::iterator it = identifiers.begin(); it != identifiers.end(); ++it) { + Scan::allScans.push_back(new BasicScan(path, *it, type)); + } + +#ifdef WITH_METRICS + ClientMetric::read_scan_time.end(t); +#endif //WITH_METRICS +} + +void BasicScan::closeDirectory() +{ + // clean up the scan vector + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) + delete *it; + Scan::allScans.clear(); +} + +BasicScan::BasicScan(double *_rPos, double *_rPosTheta, vector points) { + init(); + for(int i = 0; i < 3; i++) { + rPos[i] = _rPos[i]; + rPosTheta[i] = _rPosTheta[i]; + } + // write original pose matrix + EulerToMatrix4(rPos, rPosTheta, transMatOrg); + + // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead + transformMatrix(transMatOrg); + + // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one + M4identity(dalignxf); + PointFilter filter; + if(m_filter_range_set) + filter.setRange(m_filter_max, m_filter_min); + if(m_filter_height_set) + filter.setHeight(m_filter_top, m_filter_bottom); + if(m_range_mutation_set) + filter.setRangeMutator(m_range_mutation); + + + double* data = reinterpret_cast(create("xyz", sizeof(double) * 3 * points.size()).get_raw_pointer()); + int tmp = 0; + for(unsigned int i = 0; i < points.size(); ++i) { + for(unsigned int j = 0; j < 3; j++) { + data[tmp++] = points[i][j]; + } + } +} + +BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOType type) : + m_path(path), m_identifier(identifier), m_type(type) +{ + init(); + + // request pose from file + double euler[6]; + ScanIO* sio = ScanIO::getScanIO(m_type); + sio->readPose(m_path.c_str(), m_identifier.c_str(), euler); + rPos[0] = euler[0]; + rPos[1] = euler[1]; + rPos[2] = euler[2]; + rPosTheta[0] = euler[3]; + rPosTheta[1] = euler[4]; + rPosTheta[2] = euler[5]; + + // write original pose matrix + EulerToMatrix4(euler, &euler[3], transMatOrg); + + // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead + transformMatrix(transMatOrg); + + // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one + M4identity(dalignxf); +} + +BasicScan::~BasicScan() +{ + for (map>::iterator it = m_data.begin(); it != m_data.end(); it++) { + delete it->second.first; + } + +} + +void BasicScan::init() +{ + m_filter_max = 0.0; + m_filter_min = 0.0; + m_filter_top = 0.0; + m_filter_bottom = 0.0; + m_range_mutation = 0.0; + m_filter_range_set = false; + m_filter_height_set = false; + m_range_mutation_set = false; +} + + +void BasicScan::setRangeFilter(double max, double min) +{ + m_filter_max = max; + m_filter_min = min; + m_filter_range_set = true; +} + +void BasicScan::setHeightFilter(double top, double bottom) +{ + m_filter_top = top; + m_filter_bottom = bottom; + m_filter_height_set = true; +} + +void BasicScan::setRangeMutation(double range) +{ + m_range_mutation_set = true; + m_range_mutation = range; +} + +void BasicScan::get(unsigned int types) +{ + ScanIO* sio = ScanIO::getScanIO(m_type); + + vector xyz; + vector rgb; + vector reflectance; + vector temperature; + vector amplitude; + vector type; + vector deviation; + + PointFilter filter; + if(m_filter_range_set) + filter.setRange(m_filter_max, m_filter_min); + if(m_filter_height_set) + filter.setHeight(m_filter_top, m_filter_bottom); + if(m_range_mutation_set) + filter.setRangeMutator(m_range_mutation); + + sio->readScan(m_path.c_str(), + m_identifier.c_str(), + filter, + &xyz, + &rgb, + &reflectance, + &temperature, + &litude, + &type, + &deviation); + + // for each requested and filled data vector, allocate and write contents to their new data fields + if(types & DATA_XYZ && !xyz.empty()) { + double* data = reinterpret_cast(create("xyz", sizeof(double) * xyz.size()).get_raw_pointer()); + for(unsigned int i = 0; i < xyz.size(); ++i) data[i] = xyz[i]; + } + if(types & DATA_RGB && !rgb.empty()) { + unsigned char* data = reinterpret_cast(create("rgb", sizeof(unsigned char) * rgb.size()).get_raw_pointer()); + for(unsigned int i = 0; i < rgb.size(); ++i) data[i] = rgb[i]; + } + if(types & DATA_REFLECTANCE && !reflectance.empty()) { + float* data = reinterpret_cast(create("reflectance", sizeof(float) * reflectance.size()).get_raw_pointer()); + for(unsigned int i = 0; i < reflectance.size(); ++i) data[i] = reflectance[i]; + } + if(types & DATA_TEMPERATURE && !temperature.empty()) { + float* data = reinterpret_cast(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer()); + for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i]; + } + if(types & DATA_AMPLITUDE && !amplitude.empty()) { + int* data = reinterpret_cast(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer()); + for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i]; + } + if(types & DATA_TYPE && !type.empty()) { + float* data = reinterpret_cast(create("type", sizeof(double) * type.size()).get_raw_pointer()); + for(unsigned int i = 0; i < type.size(); ++i) data[i] = type[i]; + } + if(types & DATA_DEVIATION && !deviation.empty()) { + float* data = reinterpret_cast(create("deviation", sizeof(float) * deviation.size()).get_raw_pointer()); + for(unsigned int i = 0; i < deviation.size(); ++i) data[i] = deviation[i]; + } +} + +DataPointer BasicScan::get(const std::string& identifier) +{ + // try to get data + map>::iterator it = m_data.find(identifier); + + // create data fields + if(it == m_data.end()) { + // load from file + if(identifier == "xyz") get(DATA_XYZ); else + if(identifier == "rgb") get(DATA_RGB); else + if(identifier == "reflectance") get(DATA_REFLECTANCE); else + if(identifier == "temperature") get(DATA_TEMPERATURE); else + if(identifier == "amplitude") get(DATA_AMPLITUDE); else + if(identifier == "type") get(DATA_TYPE); else + if(identifier == "deviation") get(DATA_DEVIATION); else + // reduce on demand + if(identifier == "xyz reduced") calcReducedOnDemand(); else + if(identifier == "xyz reduced original") calcReducedOnDemand(); else + // show requests reduced points, manipulate in showing the same entry + if(identifier == "xyz reduced show") { + calcReducedOnDemand(); + m_data["xyz reduced show"] = m_data["xyz reduced"]; + } else + if(identifier == "octtree") { + createOcttree(); + } + + it = m_data.find(identifier); + } + + // if nothing can be loaded, return an empty pointer + if(it == m_data.end()) + return DataPointer(0, 0); + else + return DataPointer(it->second.first, it->second.second); +} + +DataPointer BasicScan::create(const std::string& identifier, unsigned int size) +{ + map>::iterator it = m_data.find(identifier); + if(it != m_data.end()) { + // try to reuse, otherwise reallocate + if(it->second.second != size) { + delete it->second.first; + it->second.first = new unsigned char[size]; + it->second.second = size; + } + } else { + // create a new block of data + it = m_data.insert( + std::make_pair( + identifier, + std::make_pair( + new unsigned char[size], + size + ) + ) + ).first; + } + return DataPointer(it->second.first, it->second.second); +} + +void BasicScan::clear(const std::string& identifier) +{ + map>::iterator it = m_data.find(identifier); + if(it != m_data.end()) { + delete it->second.first; + m_data.erase(it); + } +} + + +void BasicScan::createSearchTreePrivate() +{ + DataXYZ xyz_orig(get("xyz reduced original")); + PointerArray ar(xyz_orig); + switch(searchtree_nnstype) + { + case simpleKD: + kd = new KDtree(ar.get(), xyz_orig.size()); + break; + case ANNTree: + kd = new ANNtree(ar, xyz_orig.size()); + break; + case BOCTree: + kd = new BOctTree(ar.get(), xyz_orig.size(), 10.0, PointType(), true); + break; + case -1: + throw runtime_error("Cannot create a SearchTree without setting a type."); + default: + throw runtime_error("SearchTree type not implemented"); + } + + // TODO: make the switch cases above work with CUDA + if (searchtree_cuda_enabled) createANNTree(); +} + +void BasicScan::calcReducedOnDemandPrivate() +{ + // create reduced points and transform to initial position, save a copy of this for SearchTree + calcReducedPoints(); + transformReduced(transMatOrg); + copyReducedToOriginal(); +} + +void BasicScan::createANNTree() +{ + // TODO: metrics +#ifdef WITH_CUDA + if(!ann_kd_tree) { + DataXYZ xyz_orig(get("xyz reduced original")); + ann_kd_tree = new ANNkd_tree(PointArray(xyz_orig).get(), xyz_orig.size(), 3, 1, ANN_KD_STD); + cout << "Cuda tree was generated with " << xyz_orig.size() << " points" << endl; + } else { + cout << "Cuda tree exists. No need for another creation" << endl; + } +#endif +} + +void BasicScan::createOcttree() +{ + string scanFileName = m_path + "scan" + m_identifier + ".oct"; + BOctTree* btree = 0; + + // try to load from file, if successful return + if(octtree_loadOct && exists(scanFileName)) { + btree = new BOctTree(scanFileName); + m_data.insert( + std::make_pair( + "octtree", + std::make_pair( + reinterpret_cast(btree), + 0 // or memorySize()? + ) + ) + ); + return; + } + + // create octtree from scan + if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points + DataXYZ xyz_r(get("xyz reduced show")); + btree = new BOctTree(PointerArray(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true); + } else { // without reduction, xyz + attribute points + float** pts = octtree_pointtype.createPointArray(this); + unsigned int nrpts = size("xyz"); + btree = new BOctTree(pts, nrpts, octtree_voxelSize, octtree_pointtype, true); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + } + + // save created octtree + if(octtree_saveOct) { + cout << "Saving octree " << scanFileName << endl; + btree->serialize(scanFileName); + } + + m_data.insert( + std::make_pair( + "octtree", + std::make_pair( + reinterpret_cast(btree), + 0 // or memorySize()? + ) + ) + ); +} + +unsigned int BasicScan::readFrames() +{ + string filename = m_path + "scan" + m_identifier + ".frames"; + ifstream file(filename.c_str()); + file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + try { + double transformation[16]; + unsigned int type; + do { + file >> transformation >> type; + m_frames.push_back(Frame(transformation, type)); + } while(file.good()); + } catch(...) {} + + return m_frames.size(); +} + +void BasicScan::saveFrames() +{ + string filename = m_path + "scan" + m_identifier + ".frames"; + ofstream file(filename.c_str()); + for(vector::iterator it = m_frames.begin(); it != m_frames.end(); ++it) { + file << it->transformation << it->type << '\n'; + } + file << flush; + file.close(); +} + +unsigned int BasicScan::getFrameCount() +{ + return m_frames.size(); +} + +void BasicScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) +{ + const Frame& frame(m_frames.at(i)); + pose_matrix = frame.transformation; + type = static_cast(frame.type); +} + +void BasicScan::addFrame(AlgoType type) +{ + m_frames.push_back(Frame(transMat, type)); +} diff --git a/.svn/pristine/2e/2ee912faecf7ff82fcc4ae28fd7ba12e91c60209.svn-base b/.svn/pristine/2e/2ee912faecf7ff82fcc4ae28fd7ba12e91c60209.svn-base new file mode 100644 index 0000000..803cda3 --- /dev/null +++ b/.svn/pristine/2e/2ee912faecf7ff82fcc4ae28fd7ba12e91c60209.svn-base @@ -0,0 +1,1319 @@ +/** + * @file + * @brief Globally used functions + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#ifndef __GLOBALS_ICC__ +#define __GLOBALS_ICC__ + +#ifdef _MSC_VER +#include +#define _USE_MATH_DEFINES +#include + +inline int gettimeofday (struct timeval* tp, void* tzp) +{ + unsigned long t; + t = timeGetTime(); + tp->tv_sec = t / 1000; + tp->tv_usec = t % 1000; + return 0; /* 0 indicates success. */ +} +#else +#include +#endif + +#define _USE_MATH_DEFINES +#include + +#if defined(__CYGWIN__) +# ifndef M_PI +# define M_PI 3.14159265358979323846 +# define M_PI_2 1.57079632679489661923 +# define M_PI_4 0.78539816339744830962 +# define M_1_PI 0.31830988618379067154 +# define M_2_PI 0.63661977236758134308 +# define M_SQRT2 1.41421356237309504880 +# define M_SQRT1_2 0.70710678118654752440 +# endif +#endif + +#include +using std::min; +using std::max; +#include +#include +using std::stringstream; +#include +using std::ostream; +using std::istream; +#include +using std::cout; +using std::endl; +#include +#include +using std::runtime_error; + +/** + * Set bits count + * + * @param unsigned char x + * + * @return char + * + */ +inline unsigned char _my_popcount_3(unsigned char x) { + x -= (x >> 1) & 0x55; //put count of each 2 bits into those 2 bits + x = (x & 0x33) + ((x >> 2) & 0x33); //put count of each 4 bits into those 4 bits + x = (x + (x >> 4)) & 0x0f; //put count of each 8 bits into those 8 bits + return x; +} + +/** + * Converts a class T to a string of width with padding 0 + * + * @param t output + * @param width length + * + * @return string of t + * + */ +template +inline std::string to_string(const T& t, int width) +{ + stringstream ss; + ss << std::setfill('0') << std::setw(width) << t; + return ss.str(); +} + +/** + * Converts a class T to a string of width with padding 0 + * + * @param t output + * @return string of t + * + */ +template +inline std::string to_string(const T& t) +{ + stringstream ss; + ss << t; + return ss.str(); +} + + +/** + * Overridden "<<" operator for sending a (4x4)-matrix to a stream + * + * @param os stream + * @param matrix 4x4 matrix sent to stream + * @return stream + */ +inline ostream& operator<<(ostream& os, const double matrix[16]) +{ + for (int i = 0; i < 16; os << matrix[i++] << " "); + return os; +} + +/** + * Overridden ">>" operator for reading a (4x4)-matrix from a stream.
+ * Throws a runtime error if not enough data in the stream. + * + * @param is stream + * @param matrix 4x4 matrix sent to stream + * @return stream +*/ +inline istream& operator>>(istream& is, double matrix[16]) +{ + for (int i = 0; i < 16; i++) { + if (!is.good()) throw runtime_error("Not enough elements to read for >>(istream&, double[16])"); + is >> matrix[i]; + } + return is; +} + + +/** + * Converts an angle (given in deg) to rad + * + * @param deg integer indicating, whether the figure to be drawn to show + * the clusters should be circles (0) or rectangles(1) + * + * @return the clustered image, with the clusters marked by colored figures + * + */ +template +inline T rad(const T deg) +{ + return ( (2 * M_PI * deg) / 360 ); +} + +/** + * Converts an angle (given in rad) to deg + * + * @param rad angle in rad + * @return angle in deg + */ +template +inline T deg(const T rad) +{ + return ( (rad * 360) / (2 * M_PI) ); +} + + +/** + * Calculates x^2 + * + * @param x input scalar value + * @return squared value + */ +template +static inline T sqr(const T &x) +{ + return x*x; +} + + +/** + * Computes the squared length of a 3-vector + * + * @param x input 3-vector + * @return length^2 of vector + */ +template +inline T Len2(const T *x) +{ + return sqr(x[0]) + sqr(x[1]) + sqr(x[2]); +} + + +/** + * Computes the length of a 3-vector + * + * @param x input 3-vector + * @return length of vector + */ +template +inline T Len(const T *x) +{ + return sqrt(Len2(x)); +} + + +/** + * Computes the squared Eucledian distance between two points + * in 3-space + * + * @param x1 first input vector + * @param x2 decond input vecotr + * @return Eucledian distance^2 between the two locations + */ +template +inline T Dist2(const T *x1, const F *x2) +{ + T dx = x2[0] - x1[0]; + T dy = x2[1] - x1[1]; + T dz = x2[2] - x1[2]; + + return sqr(dx) + sqr(dy) + sqr(dz); +} + +/* + * Normalization of the input 3-vector + * + * @param x input/output 3-vector + */ +template +static inline void Normalize3(T *x) +{ + T norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]); + x[0] /= norm; + x[1] /= norm; + x[2] /= norm; +} + +/* + * Normalization of the input 4-vector + * + * @param x input/output 4-vector + */ +template +static inline void Normalize4(T *x) +{ + T norm = sqrt((x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3])); + + x[0] /= norm; + x[1] /= norm; + x[2] /= norm; + x[3] /= norm; +} + +/** + * Sets a 4x4 matrix to identity + * + * @param M 4x4 matrix + */ +template +inline void M4identity( T *M ) +{ + M[0] = M[5] = M[10] = M[15] = 1.0; + M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0; +} + +/** + * Multiplies a 4x4 matrices in OpenGL + * (column-major) order + * + * @param M1 first input matrix + * @param M2 second input matrix + * @param Mout output matrix + * + */ +template +inline void MMult(const T *M1, + const T *M2, + T *Mout) +{ + Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; + Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; + Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; + Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; + Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; + Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; + Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; + Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; + Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; + Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; + Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; + Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; + Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; + Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; + Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; + Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; +} + +template +inline void MMult(const T *M1, + const T *M2, + float *Mout) +{ + Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; + Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; + Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; + Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; + Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; + Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; + Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; + Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; + Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; + Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; + Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; + Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; + Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; + Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; + Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; + Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; +} + +/** + * Transforms a vector with a matrix (P = M * V) + * @param M the transformation matrix + * @param v the initial vector + * @param p the transformt vector + */ +template +inline void VTrans(const T *M, const T *V, T *P) +{ + P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12]; + P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13]; + P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14]; +} + +/** + * Converts an Euler angle to a 3x3 matrix + * + * @param rPosTheta vector of Euler angles + * @param alignxf 3x3 matrix corresponding to the Euler angles + */ +inline void EulerToMatrix3(const double *rPosTheta, double *alignxf) +{ + double sx = sin(rPosTheta[0]); + double cx = cos(rPosTheta[0]); + double sy = sin(rPosTheta[1]); + double cy = cos(rPosTheta[1]); + double sz = sin(rPosTheta[2]); + double cz = cos(rPosTheta[2]); + + alignxf[0] = cy*cz; + alignxf[1] = sx*sy*cz + cx*sz; + alignxf[2] = -cx*sy*cz + sx*sz; + alignxf[3] = -cy*sz; + alignxf[4] = -sx*sy*sz + cx*cz; + alignxf[5] = cx*sy*sz + sx*cz; + alignxf[6] = sy; + alignxf[7] = -sx*cy; + alignxf[8] = cx*cy; +} + +/** + * Calculates the determinant of a 3x3 matrix + * + * @param M input 3x3 matrix + * @return determinant of input matrix + */ +template +inline double M3det( const T *M ) +{ + double det; + det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] ) + - M[1] * ( M[3]*M[8] - M[6]*M[5] ) + + M[2] * ( M[3]*M[7] - M[6]*M[4] )); + return ( det ); +} + +/** + * Inverts a 3x3 matrix + * + * @param Min input 3x3 matrix + * @param Mout output 3x3 matrix + */ +template +inline void M3inv( const T *Min, T *Mout ) +{ + double det = M3det( Min ); + + if ( fabs( det ) < 0.0005 ) { + M3identity( Mout ); + return; + } + + Mout[0] = (double)( Min[4]*Min[8] - Min[5]*Min[7] ) / det; + Mout[1] = (double)(-( Min[1]*Min[8] - Min[7]*Min[2] )) / det; + Mout[2] = (double)( Min[1]*Min[5] - Min[4]*Min[2] ) / det; + + Mout[3] = (double)(-( Min[3]*Min[8] - Min[5]*Min[6] )) / det; + Mout[4] = (double)( Min[0]*Min[8] - Min[6]*Min[2] ) / det; + Mout[5] = (double)(-( Min[0]*Min[5] - Min[3]*Min[2] )) / det; + + Mout[6] = (double) ( Min[3]*Min[7] - Min[6]*Min[4] ) / det; + Mout[7] = (double)(-( Min[0]*Min[7] - Min[6]*Min[1] )) / det; + Mout[8] = (double) ( Min[0]*Min[4] - Min[1]*Min[3] ) / det; +} + + +/** + * Converts a pose into a RT matrix + * @param *rPos Pointer to the position (double[3]) + * @param *rPosTheta Pointer to the angles (double[3]) + * @param *alignxf The calculated matrix + */ +inline void EulerToMatrix4(const double *rPos, const double *rPosTheta, double *alignxf) +{ + double sx = sin(rPosTheta[0]); + double cx = cos(rPosTheta[0]); + double sy = sin(rPosTheta[1]); + double cy = cos(rPosTheta[1]); + double sz = sin(rPosTheta[2]); + double cz = cos(rPosTheta[2]); + + alignxf[0] = cy*cz; + alignxf[1] = sx*sy*cz + cx*sz; + alignxf[2] = -cx*sy*cz + sx*sz; + alignxf[3] = 0.0; + alignxf[4] = -cy*sz; + alignxf[5] = -sx*sy*sz + cx*cz; + alignxf[6] = cx*sy*sz + sx*cz; + alignxf[7] = 0.0; + alignxf[8] = sy; + alignxf[9] = -sx*cy; + alignxf[10] = cx*cy; + + alignxf[11] = 0.0; + + alignxf[12] = rPos[0]; + alignxf[13] = rPos[1]; + alignxf[14] = rPos[2]; + alignxf[15] = 1; +} + +/** + * Converts a 4x4 matrix to Euler angles. + * + * @param alignxf input 4x4 matrix + * @param rPosTheta output 3-vector of Euler angles + * @param rPos output vector of trnaslation (position) if set + * + */ +static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, double *rPos = 0) +{ + + double _trX, _trY; + + if(alignxf[0] > 0.0) { + rPosTheta[1] = asin(alignxf[8]); + } else { + rPosTheta[1] = M_PI - asin(alignxf[8]); + } + // rPosTheta[1] = asin( alignxf[8]); // Calculate Y-axis angle + + double C = cos( rPosTheta[1] ); + if ( fabs( C ) > 0.005 ) { // Gimball lock? + _trX = alignxf[10] / C; // No, so get X-axis angle + _trY = -alignxf[9] / C; + rPosTheta[0] = atan2( _trY, _trX ); + _trX = alignxf[0] / C; // Get Z-axis angle + _trY = -alignxf[4] / C; + rPosTheta[2] = atan2( _trY, _trX ); + } else { // Gimball lock has occurred + rPosTheta[0] = 0.0; // Set X-axis angle to zero + _trX = alignxf[5]; //1 // And calculate Z-axis angle + _trY = alignxf[1]; //2 + rPosTheta[2] = atan2( _trY, _trX ); + } + + rPosTheta[0] = rPosTheta[0]; + rPosTheta[1] = rPosTheta[1]; + rPosTheta[2] = rPosTheta[2]; + + if (rPos != 0) { + rPos[0] = alignxf[12]; + rPos[1] = alignxf[13]; + rPos[2] = alignxf[14]; + } +} + + +/** + * Sets a 3x3 matrix to the identity matrix + * + * @param M input 3x3 matrix + */ +template +static inline void M3identity( T *M ) +{ + M[0] = M[4] = M[8] = 1.0; + M[1] = M[2] = M[3] = M[5] = M[6] = M[7] = 0.0; +} + +/** + * Gets the current time (in ms) + * + * @return current time (in ms) + */ +static inline unsigned long GetCurrentTimeInMilliSec() +{ + static unsigned long milliseconds; +#ifdef _MSC_VER + SYSTEMTIME stime; + GetSystemTime(&stime); + milliseconds = ((stime.wHour * 60 + stime.wMinute) * 60 + stime.wSecond) * 1000 + stime.wMilliseconds; +#else + static struct timeval tv; + gettimeofday(&tv, NULL); + milliseconds = tv.tv_sec * 1000 + tv.tv_usec / 1000; +#endif + return milliseconds; +} + +/** + * generates random numbers in [0..rnd] + * + * @param rnd maximum number + * @return random number between 0 and rnd + */ +inline int rand(int rnd) +{ + return (int) ((double)rnd * (double)std::rand() / (RAND_MAX + 1.0)); +} + +/** + * generates unsigned character random numbers in [0..rnd] + * + * @param rnd maximum number + * @return random number between 0 and rnd + */ +inline unsigned char randUC(unsigned char rnd) +{ + return (unsigned char) ((float)rnd * std::rand() / (RAND_MAX + 1.0)); +} + +/** + * Computes the angle between 2 points in polar coordinates + */ +inline double polardist(double* p, double *p2) { + double stheta = sin(p[0]) * sin(p2[0]); + double myd2 = acos( stheta * cos(p[1]) * cos(p2[1]) + + stheta * sin(p[1]) * sin(p2[1]) + + cos(p[0]) * cos(p2[0])); + return myd2; +} + + + +inline void toKartesian(double *polar, double *kart) { + kart[0] = polar[2] * cos( polar[1] ) * sin( polar[0] ); + kart[1] = polar[2] * sin( polar[1] ) * sin( polar[0] ); + kart[2] = polar[2] * cos( polar[0] ); + +} + +/** + * Transforms a point in cartesian coordinates into polar + * coordinates + */ + +inline void toPolar(double *n, double *polar) { + double phi, theta, rho; + rho = Len(n); + Normalize3(n); + + // if(fabs(1 - fabs(n[1])) < 0.001) { + // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl; + + phi = acos(n[2]); + //if ( fabs(phi) < 0.0001) phi = 0.0001; + //if ( fabs(M_PI - phi) < 0.0001) phi = 0.0001; + + double theta0; + + if(fabs(phi) < 0.0001) { + theta = 0.0; + } else if(fabs(M_PI - phi) < 0.0001) { + theta = 0.0; + } else { + if(fabs(n[0]/sin(phi)) > 1.0) { + if(n[0]/sin(phi) < 0) { + theta0 = M_PI; + } else { + theta0 = 0.0; + } + } else { + theta0 = acos(n[0]/sin(phi)); + + } + + + double sintheta = n[1]/sin(phi); + + double EPS = 0.0001; + + if(fabs(sin(theta0) - sintheta) < EPS) { + theta = theta0; + } else if(fabs( sin( 2*M_PI - theta0 ) - sintheta ) < EPS) { + theta = 2*M_PI - theta0; + } else { + theta = 0; + cout << "Fehler" << endl; + } + + } + /* } else { + theta = 0.0; + phi = 0.0; + }*/ + polar[0] = phi; + polar[1] = theta; + polar[2] = rho; + +} + +/* + * Computes the submatrix without + * row i and column j + * + * @param Min input 4x4 matrix + * @param Mout output 3x3 matrix + * @param i row index i + * @param j column index j + */ +template +static inline void M4_submat(const T *Min, T *Mout, int i, int j ) { + int di, dj, si, sj; + // loop through 3x3 submatrix + for( di = 0; di < 3; di ++ ) { + for( dj = 0; dj < 3; dj ++ ) { + // map 3x3 element (destination) to 4x4 element (source) + si = di + ( ( di >= i ) ? 1 : 0 ); + sj = dj + ( ( dj >= j ) ? 1 : 0 ); + // copy element + Mout[di * 3 + dj] = Min[si * 4 + sj]; + } + } +} + +/* + * Computes the determinant of a 4x4 matrix + * + * @param 4x4 matrix + * @return determinant + */ +template +static inline double M4det(const T *M ) +{ + T det, result = 0, i = 1.0; + T Msub3[9]; + int n; + for ( n = 0; n < 4; n++, i *= -1.0 ) { + M4_submat( M, Msub3, 0, n ); + det = M3det( Msub3 ); + result += M[n] * det * i; + } + return( result ); +} + + +/* + * invert a 4x4 Matrix + * + * @param Min input 4x4 matrix + * @param Mout output matrix + * @return 1 if successful + */ +template +static inline int M4inv(const T *Min, T *Mout ) +{ + T mdet = M4det( Min ); + if ( fabs( mdet ) < 0.0005 ) { + cout << "Error matrix inverting!" << endl; + M4identity( Mout ); + return( 0 ); + } + T mtemp[9]; + int i, j, sign; + for ( i = 0; i < 4; i++ ) { + for ( j = 0; j < 4; j++ ) { + sign = 1 - ( (i +j) % 2 ) * 2; + M4_submat( Min, mtemp, i, j ); + Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet; + } + } + return( 1 ); +} + +/* + * transposes a 4x4 matrix + * + * @param Min input 4x4 matrix + * @param Mout output 4x4 matrix + */ +template +static inline int M4transpose(const T *Min, T *Mout ) +{ + Mout[0] = Min[0]; + Mout[4] = Min[1]; + Mout[8] = Min[2]; + Mout[12] = Min[3]; + Mout[1] = Min[4]; + Mout[5] = Min[5]; + Mout[9] = Min[6]; + Mout[13] = Min[7]; + Mout[2] = Min[8]; + Mout[6] = Min[9]; + Mout[10] = Min[10]; + Mout[14] = Min[11]; + Mout[3] = Min[12]; + Mout[7] = Min[13]; + Mout[11] = Min[14]; + Mout[15] = Min[15]; + return( 1 ); +} + +/* +++++++++-------------++++++++++++ + * NAME + * choldc + * DESCRIPTION + * Cholesky Decomposition of a symmetric + * positive definite matrix + * Overwrites lower triangle of matrix + * Numerical Recipes, but has a bit of + * the fancy C++ template thing happening + * +++++++++-------------++++++++++++ */ +static inline bool choldc(double A[3][3], double diag[3]) +{ + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = i; j < 3; j++) { + double sum = A[i][j]; + for (int k=i-1; k >= 0; k--) + sum -= A[i][k] * A[j][k]; + if (i == j) { + if (sum < 1.0e-7) + return false; + diag[i] = sqrt(sum); + } else { + A[j][i] = sum / diag[i]; + } + } + } + return true; +} + + +/* +++++++++-------------++++++++++++ + * NAME + * choldc + * DESCRIPTION + * Cholesky Decomposition of a symmetric + * positive definite matrix + * Overwrites lower triangle of matrix + * Numerical Recipes, but has a bit of + * the fancy C++ template thing happening + * +++++++++-------------++++++++++++ */ +static inline bool choldc(unsigned int n, double **A, double *diag) +{ + for (unsigned int i = 0; i < n; i++) { + for (unsigned int j = i; j < n; j++) { + double sum = A[i][j]; + for (int k=i-1; k >= 0; k--) + sum -= A[i][k] * A[j][k]; + if (i == j) { + if (sum < 1.0e-7) + return false; + diag[i] = sqrt(sum); + } else { + A[j][i] = sum / diag[i]; + } + } + } + return true; +} + + +/* +++++++++-------------++++++++++++ + * NAME + * cholsl + * DESCRIPTION + * Solve Ax=B after choldc + * +++++++++-------------++++++++++++ */ +static inline void cholsl(double A[3][3], + double diag[3], + double B[3], + double x[3]) +{ + for (int i=0; i < 3; i++) { + double sum = B[i]; + for (int k=i-1; k >= 0; k--) + sum -= A[i][k] * x[k]; + x[i] = sum / diag[i]; + } + for (int i=2; i >= 0; i--) { + double sum = x[i]; + for (int k=i+1; k < 3; k++) + sum -= A[k][i] * x[k]; + x[i] = sum / diag[i]; + } +} + + +/* +++++++++-------------++++++++++++ + * NAME + * cholsl + * DESCRIPTION + * Solve Ax=B after choldc + * +++++++++-------------++++++++++++ */ +static inline void cholsl(unsigned int n, + double **A, + double *diag, + double *B, + double *x) +{ + for (unsigned int i=0; i < n; i++) { + double sum = B[i]; + for (int k=(int)i-1; k >= 0; k--) + sum -= A[i][k] * x[k]; + x[i] = sum / diag[i]; + } + for (int i=(int)n-1; i >= 0; i--) { + double sum = x[i]; + for (unsigned int k=i+1; k < n; k++) + sum -= A[k][i] * x[k]; + x[i] = sum / diag[i]; + } +} + + +/** + * Transforms a a quaternion and a translation vector into a 4x4 + * Matrix + * + * @param quat input quaternion + * @param t input translation + * @param mat output matrix + */ +static inline void QuatToMatrix4(const double *quat, const double *t, double *mat) +{ + // double q00 = quat[0]*quat[0]; + double q11 = quat[1]*quat[1]; + double q22 = quat[2]*quat[2]; + double q33 = quat[3]*quat[3]; + double q03 = quat[0]*quat[3]; + double q13 = quat[1]*quat[3]; + double q23 = quat[2]*quat[3]; + double q02 = quat[0]*quat[2]; + double q12 = quat[1]*quat[2]; + double q01 = quat[0]*quat[1]; + mat[0] = 1 - 2 * (q22 + q33); + mat[5] = 1 - 2 * (q11 + q33); + mat[10] = 1 - 2 * (q11 + q22); + mat[4] = 2.0*(q12-q03); + mat[1] = 2.0*(q12+q03); + mat[8] = 2.0*(q13+q02); + mat[2] = 2.0*(q13-q02); + mat[9] = 2.0*(q23-q01); + mat[6] = 2.0*(q23+q01); + + mat[3] = mat[7] = mat[11] = 0.0; + + if (t == 0) { + mat[12] = mat[13] = mat[14] = 0.0; + } else { + mat[12] = t[0]; + mat[13] = t[1]; + mat[14] = t[2]; + } + mat[15] = 1.0; +} + + +/** + * Transforms a 4x4 Transformation Matrix into a quaternion + * + * @param mat matrix to be converted + * @param quat resulting quaternion + * @param t resulting translation + */ +static inline void Matrix4ToQuat(const double *mat, double *quat, double *t = 0) +{ + + double T, S, X, Y, Z, W; + T = 1 + mat[0] + mat[5] + mat[10]; + if ( T > 0.00000001 ) { // to avoid large distortions! + + S = sqrt(T) * 2; + X = ( mat[9] - mat[6] ) / S; + Y = ( mat[2] - mat[8] ) / S; + Z = ( mat[4] - mat[1] ) / S; + W = 0.25 * S; + } else if ( mat[0] > mat[5] && mat[0] > mat[10] ) { // Column 0: + S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2; + X = 0.25 * S; + Y = (mat[4] + mat[1] ) / S; + Z = (mat[2] + mat[8] ) / S; + W = (mat[9] - mat[6] ) / S; + } else if ( mat[5] > mat[10] ) { // Column 1: + S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2; + X = (mat[4] + mat[1] ) / S; + Y = 0.25 * S; + Z = (mat[9] + mat[6] ) / S; + W = (mat[2] - mat[8] ) / S; + } else { // Column 2: + S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2; + X = (mat[2] + mat[8] ) / S; + Y = (mat[9] + mat[6] ) / S; + Z = 0.25 * S; + W = (mat[4] - mat[1] ) / S; + } + quat[0] = W; + quat[1] = -X; + quat[2] = -Y; + quat[3] = -Z; + + Normalize4(quat); + if (t != 0) { + t[0] = mat[12]; + t[1] = mat[13]; + t[2] = mat[14]; + } +} + +/** + * Transforms a Quaternion to the corresponding Axis-Angle representation + * + * @param quat 4-vector of quaternion + * gets overridden by the axis/angle representation + */ +static inline void QuatToAA(double *quat){ + //double x, y, z, w; + double sum = 0.0; + + double cos_a, angle, x, y, z, sin_a; + + for(int i = 0; i < 4; i++){ + sum += quat[i]*quat[i]; + } + sum = sqrt(sum); + + //quaternion_normalise( |W,X,Y,Z| ); + cos_a = quat[0]/sum; + angle = acos( cos_a ) * 2; + sin_a = sqrt( 1.0 - cos_a * cos_a ); + if ( fabs( sin_a ) < 0.0005 ) sin_a = 1; + x = quat[1] / sin_a; + y = quat[2] / sin_a; + z = quat[3] / sin_a; + + quat[0] = angle; + quat[1] = x; + quat[2] = y; + quat[3] = z; +} + +/** + * Quaternion Multiplication q1 * q2 = q3 + */ +static inline void QMult(const double *q1, const double *q2, double *q3) { + q3[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + q3[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + q3[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + q3[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; +} + +/** + * Quaternion SLERP + * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ + */ +static inline void slerp(const double *qa, const double *qb, const double t, double *qm) { + // Calculate angle between them. + double cosHalfTheta = qa[0] * qb[0] + qa[1] * qb[1] + qa[2] * qb[2] + qa[3] * qb[3]; + // if qa=qb or qa=-qb then theta = 0 and we can return qa + if (fabs(cosHalfTheta) >= 1.0) { + qm[0] = qa[0]; + qm[1] = qa[1]; + qm[2] = qa[2]; + qm[3] = qa[3]; + return; + } + // Calculate temporary values. + double halfTheta = acos(cosHalfTheta); + double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + if (fabs(sinHalfTheta) < 0.001){ + qm[0] = (qa[0] * 0.5 + qb[0] * 0.5); + qm[1] = (qa[1] * 0.5 + qb[1] * 0.5); + qm[2] = (qa[2] * 0.5 + qb[2] * 0.5); + qm[3] = (qa[3] * 0.5 + qb[3] * 0.5); + Normalize4(qm); + return; + } + double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; + double ratioB = sin(t * halfTheta) / sinHalfTheta; + //calculate Quaternion. + qm[0] = (qa[0] * ratioA + qb[0] * ratioB); + qm[1] = (qa[1] * ratioA + qb[1] * ratioB); + qm[2] = (qa[2] * ratioA + qb[2] * ratioB); + qm[3] = (qa[3] * ratioA + qb[3] * ratioB); + Normalize4(qm); +} + +/* taken from ROOT (CERN) + * as well in: + * Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning + * James J. Kuffner + * + * Distance between two rotations in Quaternion form + * Note: The rotation group is isomorphic to a 3-sphere + * with diametrically opposite points identified. + * The (rotation group-invariant) is the smaller + * of the two possible angles between the images of + * the two rotations on that sphere. Thus the distance + * is never greater than pi/2. + */ +inline double quat_dist(double quat1[4], double quat2[4]) { + double chordLength = std::fabs(quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3]); + if (chordLength > 1) chordLength = 1; // in case roundoff fouls us up + return acos(chordLength) / M_PI * 180.0; +} + + + +/** + * Converts a Rotation given by Axis-Angle and a Translation into a + * 4x4 Transformation matrix + * + * @param aa axis and angle aa[0] is the angle + * @param trans vector containing the translation + * @param matrix matrix to be computed + */ +inline void AAToMatrix(double *aa, double *trans, double *matrix ){ + double rcos = cos(aa[0]); + double rsin = sin(aa[0]); + double u = aa[1]; + double v = aa[2]; + double w = aa[3]; + + matrix[0] = rcos + u*u*(1-rcos); + matrix[1] = w * rsin + v*u*(1-rcos); + matrix[2] = -v * rsin + w*u*(1-rcos); + matrix[3] = 0.0; + matrix[4] = -w * rsin + u*v*(1-rcos); + matrix[5] = rcos + v*v*(1-rcos); + matrix[6] = u * rsin + w*v*(1-rcos); + matrix[7] = 0.0; + matrix[8] = v * rsin + u*w*(1-rcos); + matrix[9] = -u * rsin + v*w*(1-rcos); + matrix[10] = rcos + w*w*(1-rcos); + matrix[11] = 0.0; + matrix[12] = trans[0]; + matrix[13] = trans[1]; + matrix[14] = trans[2]; + matrix[15] = 1.0; + +} + + +/** + * Factors matrix A into lower and upper triangular matrices + * (L and U respectively) in solving the linear equation Ax=b. + * + * @param A (input/output) Matrix(1:n, 1:n) In input, matrix to be + * factored. On output, overwritten with lower and + * upper triangular factors. + * + * @param indx (output) Vector(1:n) Pivot vector. Describes how + * the rows of A were reordered to increase + * numerical stability. + * + * @return return int(0 if successful, 1 otherwise) + */ +inline int LU_factor( double A[4][4], int indx[4]) +{ + int M = 4; + int N = 4; + + int i=0,j=0,k=0; + int jp=0; + + double t; + + int minMN = 4; + + for (j = 0; j < minMN; j++) + { + + // find pivot in column j and test for singularity. + + jp = j; + t = fabs(A[j][j]); + for (i = j+1; i < M; i++) + if ( fabs(A[i][j]) > t) + { + jp = i; + t = fabs(A[i][j]); + } + + indx[j] = jp; + + // jp now has the index of maximum element + // of column j, below the diagonal + + if ( A[jp][j] == 0 ) + return 1; // factorization failed because of zero pivot + + + if (jp != j) // swap rows j and jp + for (k = 0; k < N; k++) + { + t = A[j][k]; + A[j][k] = A[jp][k]; + A[jp][k] =t; + } + + if (j < M) // compute elements j+1:M of jth column + { + // note A(j,j), was A(jp,p) previously which was + // guarranteed not to be zero (Label #1) + // + double recp = 1.0 / A[j][j]; + + for (k = j+1; k < M; k++) + A[k][j] *= recp; + } + + if (j < minMN) + { + // rank-1 update to trailing submatrix: E = E - x*y; + // + // E is the region A(j+1:M, j+1:N) + // x is the column vector A(j+1:M,j) + // y is row vector A(j,j+1:N) + + int ii,jj; + + for (ii = j+1; ii < M; ii++) + for (jj = j+1; jj < N; jj++) + A[ii][jj] -= A[ii][j]*A[j][jj]; + } + } + + return 0; +} + +/** + * Solves a linear system via LU after LU factor + * + * @param A 4x4 matrix + * @param indx indices + * @param b 4 vectort + * + * @return 0 + * + */ +inline int LU_solve(const double A[4][4], const int indx[4], double b[4]) +{ + int i,ii=0,ip,j; + int n = 4; + double sum = 0.0; + + for (i = 0; i < n; i++) + { + ip=indx[i]; + sum=b[ip]; + b[ip]=b[i]; + if (ii) + for (j = ii;j <= i-1; j++) + sum -= A[i][j]*b[j]; + else if (sum) ii=i; + b[i]=sum; + } + for (i = n-1; i >= 0; i--) + { + sum=b[i]; + for (j = i+1; j < n; j++) + sum -= A[i][j]*b[j]; + b[i]=sum/A[i][i]; + } + + return 0; +} + +/** + * Calculates the cross product of two 4-vectors + * + * @param x input 1 + * @param y input 2 + * @param T output + * + */ +template +static inline void Cross(const T *x, const T *y, T *result) +{ + result[0] = x[1] * y[2] - x[2] * y[1]; + result[1] = x[2] * y[0] - x[0] * y[2]; + result[2] = x[0] * y[1] - x[1] * y[0]; + return; +} + +/** + * Computes the dot product of two 3-vector + * + * @param x input 3-vector + * @param y input 3-vector + * @return dot product of x and y + */ +template +inline T Dot(const T *x, const T *y) +{ + return x[0] * y[0] + x[1] * y[1] + x[2] * y[2]; +} + +/** + * converts a quaternion to Euler angels in the roll pitch yaw system + */ +static inline void QuatRPYEuler(const double *quat, double *euler) +{ + double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]); + double s = n > 0?2./(n*n):0.; + + double m00, m10, m20, m21, m22; + + + double xs = quat[1]*s; + double ys = quat[2]*s; + double zs = quat[3]*s; + + double wx = quat[0]*xs; + double wy = quat[0]*ys; + double wz = quat[0]*zs; + + double xx = quat[1]*xs; + double xy = quat[1]*ys; + double xz = quat[1]*zs; + + double yy = quat[2]*ys; + double yz = quat[2]*zs; + + double zz = quat[3]*zs; + + m00 = 1.0 - (yy + zz); + m22 = 1.0 - (xx + yy); + + + m10 = xy + wz; + + m20 = xz - wy; + m21 = yz + wx; + + euler[0] = atan2(m21,m22); + euler[1] = atan2(-m20,sqrt(m21*m21 + m22*m22)); + euler[2] = atan2(m10,m00); +} + +/** + * converts from Euler angels in the roll pitch yaw system to a quaternion + */ +static inline void RPYEulerQuat(const double *euler, double *quat) +{ + double sphi = sin(euler[0]); + double stheta = sin(euler[1]); + double spsi = sin(euler[2]); + double cphi = cos(euler[0]); + double ctheta = cos(euler[1]); + double cpsi = cos(euler[2]); + + double _r[3][3] = { //create rotational Matrix + {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi}, + {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi}, + { -stheta, ctheta*sphi, ctheta*cphi} + }; + +#define MY_MAX(a,b) (((a)>(b))?(a):(b)) + double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0; + double _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0; + double _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0; + double _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0; + quat[0] = _w; + quat[1] = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x); + quat[2] = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y); + quat[3] = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z); +} + + +inline void transform3(const double *alignxf, double *point) +{ + double x_neu, y_neu, z_neu; + x_neu = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8]; + y_neu = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9]; + z_neu = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10]; + point[0] = x_neu + alignxf[12]; + point[1] = y_neu + alignxf[13]; + point[2] = z_neu + alignxf[14]; +} + +inline void transform3(const double *alignxf, const double *point, double *tpoint) +{ + tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12]; + tpoint[1] = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9] + alignxf[13]; + tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14]; +} + +inline std::string trim(const std::string& source) +{ + unsigned int start = 0, end = source.size() - 1; + while(source[start] == ' ') start++; + while(source[end] == ' ') end--; + return source.substr(start, end - start + 1); +} + +#endif diff --git a/.svn/pristine/33/331fbbb0d8c97d430c612ef62a32000f278676e9.svn-base b/.svn/pristine/33/331fbbb0d8c97d430c612ef62a32000f278676e9.svn-base new file mode 100644 index 0000000..8f9ad24 --- /dev/null +++ b/.svn/pristine/33/331fbbb0d8c97d430c612ef62a32000f278676e9.svn-base @@ -0,0 +1,128 @@ +/** + * @file fbr_global.h + * @brief Globally used headers, functions, structures + * @author HamidReza Houshiar. Jacobs University Bremen gGmbH, Germany. + * @date 2012/05/9 14:00 + */ + +#ifndef FBR_GLOBAL_H_ +#define FBR_GLOBAL_H_ + +#include +#include +#include +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif +//for opencv 2.4 +#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4) +#include +#endif +#include +#include +#include "slam6d/io_types.h" +#include "slam6d/globals.icc" + +using namespace std; + +namespace fbr{ + + //Vertical angle of view of scanner +#define MAX_ANGLE 60.0 +#define MIN_ANGLE -40.0 + + /** + * @enum projection_method + */ + enum projection_method{ + EQUIRECTANGULAR, + CYLINDRICAL, + MERCATOR, + RECTILINEAR, + PANNINI, + STEREOGRAPHIC, + ZAXIS, + CONIC + }; + /** + * @enum panorama_map_method + */ + enum panorama_map_method{ + FARTHEST, + EXTENDED, + }; + /** + * @enum feature_method + */ + enum feature_detector_method{ + SIFT_DET, + SURF_DET, + ORB_DET, + FAST_DET, + STAR_DET, + }; + enum feature_descriptor_method{ + SIFT_DES, + SURF_DES, + ORB_DES, + }; + /** + * @enum matching_method + */ + enum matcher_method{ + BRUTEFORCE, + FLANN, + KNN, + RADIUS, + RATIO, + }; + /** + * @enum registration_method + */ + enum registration_method{ + ALL, + RANSAC, + DISABLE, + }; + /** + * @enum feature_filtration_method + */ + enum feature_filtration_method{ + OCCLUSION, + STANDARD_DEVIATION, + DISABLE_FILTER, + }; + /** + * @enum matching_filtration_method + */ + enum matching_filtration_method{ + FUNDEMENTAL_MATRIX, + DISABLE_MATCHING_FILTER, + }; + //RANSAC iteration +#define RANSACITR 20000 + //Inlier influence +#define iInfluence 0.5 + + string scanFormatToString(IOType format); + IOType stringToScanFormat(string format); + string projectionMethodToString(projection_method method); + projection_method stringToProjectionMethod(string method); + string panoramaMapMethodToString(panorama_map_method method); + panorama_map_method stringToPanoramaMapMethod(string method); + string featureDetectorMethodToString(feature_detector_method method); + feature_detector_method stringToFeatureDetectorMethod(string method); + string featureDescriptorMethodToString(feature_descriptor_method method); + feature_descriptor_method stringToFeatureDescriptorMethod(string method); + string matcherMethodToString(matcher_method method); + matcher_method stringToMatcherMethod(string method); + string registrationMethodToString(registration_method method); + registration_method stringToRegistrationMethod(string method); + string featureFiltrationMethodToString(feature_filtration_method method); + feature_filtration_method stringToFeatureFiltrationMethod(string method); + string matchingFiltrationMethodToString(matching_filtration_method method); + matching_filtration_method stringToMatchingFiltrationMethod(string method); +} +#endif /* FBR_GLOBAL_H_ */ diff --git a/.svn/pristine/35/35a47a72bc3b6b1c1c5548f978e7ab6bb7992ad4.svn-base b/.svn/pristine/35/35a47a72bc3b6b1c1c5548f978e7ab6bb7992ad4.svn-base new file mode 100644 index 0000000..40086f9 --- /dev/null +++ b/.svn/pristine/35/35a47a72bc3b6b1c1c5548f978e7ab6bb7992ad4.svn-base @@ -0,0 +1,52 @@ +#MAKEFLAGS += -j + +ifeq ($(MAKE),) + MAKE=make +endif + +all: .configured + cd .build && $(MAKE) --no-print-directory + +config: .build + cd .build && ccmake .. + touch .configured + +.configured: .build + cd .build && cmake .. && cmake .. + touch .configured + +.build: + mkdir -p .build + +clean: .build + -cd .build && $(MAKE) clean --no-print-directory + -rm -rf .build + rm -f .configured + +DOC = doc/ +docu: docu_html docu_latex docu_hl + echo + echo + echo + Reference documentation generated: $(DOC)html/index.html + echo + Reference documentation generated: $(DOC)refman.pdf + echo + Highlevel documentation generated: $(DOC)documentation_HL.pdf + echo + +docu_html: + doxygen doc/doxygen.cfg + cd $(DOC) ; zip -q html.zip html/* + echo + echo + +docu_latex: + $(MAKE) -C $(DOC)latex + cd $(DOC)latex ; dvips refman + cd $(DOC)latex ; ps2pdf14 refman.ps refman.pdf + cp $(DOC)latex/refman.pdf $(DOC) + +docu_hl: $(DOC)high_level_doc/documentation.tex + cd $(DOC)high_level_doc ; latex documentation.tex + cd $(DOC)high_level_doc ; bibtex documentation + cd $(DOC)high_level_doc ; latex documentation.tex + cd $(DOC)high_level_doc ; dvips documentation + cd $(DOC)high_level_doc ; ps2pdf14 documentation.ps ../documentation_HL.pdf diff --git a/.svn/pristine/3c/3ccd2ec774f97990fca103e9a511cac775db7d41.svn-base b/.svn/pristine/3c/3ccd2ec774f97990fca103e9a511cac775db7d41.svn-base new file mode 100644 index 0000000..bf7c9b4 --- /dev/null +++ b/.svn/pristine/3c/3ccd2ec774f97990fca103e9a511cac775db7d41.svn-base @@ -0,0 +1,418 @@ +#ifndef SCAN_H +#define SCAN_H + +#include "io_types.h" +#include "data_types.h" +#include "point_type.h" +#include "ptpair.h" + +#include +#include + +#include +#include + +//! SearchTree types +enum nns_type { + simpleKD, ANNTree, BOCTree +}; + +class Scan; +typedef std::vector ScanVector; + +class SearchTree; +class ANNkd_tree; + +/** HOWTO scan +First: Load scans (if you want to use the scanmanager, use ManagedScan) + + BasicScan/ManagedScan::openDirectory(path, type, start, end); + +Pass it to functions (by reference to link it to the same instance) or store it in a global variable + +After loading you might want to set parameters + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + scan->setRangeFilter(maxDist, minDist); + scan->setHeightFilter(top, bottom); // thermo + scan->setReductionParameter(voxelSize, nrpts[, pointtype]); + scan->setSearchTreeParameter(nns_method, use_cuda); + } + +Access the contained data, will be loaded and calculated on demand + + DataXYZ xyz = scan->get("xyz"); + DataXYZ reduced = scan->get("xyz reduced"); + DataRGB rgb = scan->get("rgb"); + + xyz[i][0..2] + reflectance[i] + + unsigned int size = scan->size("xyz reduced"); + +In order to use the prefetching of all requested data field in the scanserver, mark them for use. This is relevant for efficiency, which would otherwise cause loading the files each time another data field is requested. + + scan->get(DATA_XYZ | DATA_RGB | ...); + +Under circumstances the data fields are not available (e.g. no color in uos-type scans) + + DataRGB rgb = scan->get("rgb"); + if(rgb.valid()) { ok, do something } + +If backward-compability to pointer arrays is needed, the PointerArray class can adapt + + BOctTree(PointerArray(scan->get("xyz")).get(), scan->size("xyz"), ...); + +If data isn't needed anymore, flag it for removal + + scan->clear("xyz"); + scan->clear(DATA_XYZ | DATA_RGB | ...); + +Creating data fields with the correct byte size + + scan->create("xyz somethingelse", sizeof(double)*3*N); + +Reading frames in show: + + unsigned int size = scan->readFrames(); + + const double* pose; + AlgoType type; + scan->getFrame(i, pose, type); + +Last, if program ends, clean up + + Scan::closeDirectory(scans); + +**/ + + + +/** + * This class bundles common features of different scan implementations by + * abstraction. It handles the algorithmic parts and leaves IO and other + * features to the deriving classes. + */ +class Scan { + //friend class SearchTree; // TODO: is this neccessary? +public: + enum AlgoType { + INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO, + GRAPHHOGMAN + }; + + // delete copy-ctor and assignment, scans shouldn't be copied by basic class + Scan(const Scan& other) = delete; + Scan& operator=(const Scan& other) = delete; + + virtual ~Scan(); + + //! Holder of all scans - also used in transform for adding frames for each scan at the same time + static std::vector allScans; + + /** + * Attempt to read a directory under \a path and return its read scans. + * No scans are loaded at this point, only checked if all exist. + * + * @param scanserver whether to use managed scans in the scanserver or not + * @param path to the directory containing the scans + * @param type determining which ScanIO to use + * @param start first scan to use + * @param end last scan to use, -1 means from start to last available + */ + static void openDirectory(bool scanserver, const std::string& path, IOType type, + int start, int end = -1); + + /** + * "Close" a directory by deleting all its scans and emptying the + * Scan::allScans vector. + */ + static void closeDirectory(); + + + /* Input filtering and parameter functions */ + + + //! Input filtering for all points based on their euclidean length + virtual void setRangeFilter(double max, double min) = 0; + + //! Input filtering for all points based on their height + virtual void setHeightFilter(double top, double bottom) = 0; + + //! Input mutation to set range of all points to a constant value; + virtual void setRangeMutation(double range) { } + + //! Set reduction parameters, but don't reduce yet + virtual void setReductionParameter(double voxelSize, int nrpts = 0, + PointType pointtype = PointType()); + + //! Set SearchTree type, but don't create it yet + void setSearchTreeParameter(int nns_method, bool cuda_enabled); + + /** + * Set octtree parameters for show + * @param loadOct will load the serialized octtree from disk regardless + * @param saveOct serialize octtree if not loaded by loadOct after creation + */ + virtual void setOcttreeParameter(double reduction_voxelSize, + double octtree_voxelSize, PointType pointtype, + bool loadOct, bool saveOct); + + + /* Basic getter functions */ + + + inline const double* get_rPos() const; + inline const double* get_rPosTheta() const; + inline const double* get_rPosQuat() const; + //! Pose matrix after initial and match transformations (org+dalign) + inline const double* get_transMat() const; + //! Original pose matrix after initial transform + inline const double* get_transMatOrg() const; + //! Accumulated delta transformation matrix + inline const double* getDAlign() const; + + inline SearchTree* getSearchTree(); + inline ANNkd_tree* getANNTree() const; + + virtual const char* getIdentifier() const = 0; + + //! Determine the maximum number of reduced points in \a scans + static unsigned int getMaxCountReduced(ScanVector& scans); + + + /* Functions for altering data fields, implementation specific */ + + + /** + * Get the data field \a identifier, calculate it on demand if neccessary. + * + * If "xyz reduced" or "xyz reduced original" is requested, the reduction is + * started with "xyz" as input. + */ + virtual DataPointer get(const std::string& identifier) = 0; + + /** + * Load the requested IODataTypes, joined by |, from the scan file. + * + * This feature is neccessary to load multiple data fields at once, not all + * one by one with each get("...") access. + */ + virtual void get(unsigned int types) = 0; + + /** + * Creates a data field \a identifier with \a size bytes. + */ + virtual DataPointer create(const std::string& identifier, unsigned int size) = 0; + + /** + * Clear the data field \a identifier, removing its allocated memory if + * possible or marking it for prioritized removal. + */ + virtual void clear(const std::string& identifier) = 0; + + //! Extension to clear for more than one identifier, e.g. clear(DATA_XYZ | DATA_RGB); + void clear(unsigned int types); + + /** + * Get the size of \a identifier as if it were requested and size() called + * upon its type specialized DataPointer class. + * e.g size("xyz reduced") + */ + template + unsigned int size(const std::string& identifier) { + return (T(get(identifier))).size(); + } + + + /* Frame handling functions */ + + + /** + * Open the .frames-file and read its contents. If not read, the frame list + * will be empty. + * @return count of frames if file has been read, zero otherwise + */ + virtual unsigned int readFrames() = 0; + + /** + * Write the accumulated frames into a .frames-file. + */ + virtual void saveFrames() = 0; + + //! Count of frames + virtual unsigned int getFrameCount() = 0; + + //! Get contents of a frame, pass matrix pointer and type by reference + virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) = 0; + +protected: + /** + * Called from transform, this will add its current transMat pose with + * the given type as a frame into the list of frames + */ + virtual void addFrame(AlgoType type) = 0; + +public: + + /* Direct creation of reduced points and search tree */ + + //! Apply reduction and initial transMatOrg transformation + void toGlobal(); + + //! Copy reduced points to original and create search tree on it + void createSearchTree(); + + + /* Common transformation and matching functions */ + void mergeCoordinatesWithRoboterPosition(Scan* prevScan); + void transformAll(const double alignxf[16]); + void transformAll(const double alignQuat[4], const double alignt[3]); + + void transform(const double alignxf[16], + const AlgoType type, int islum = 0); + void transform(const double alignQuat[4], + const double alignt[3], const AlgoType type, int islum = 0); + void transformToMatrix(double alignxf[16], + const AlgoType type, int islum = 0); + void transformToEuler(double rP[3], double rPT[3], + const AlgoType type, int islum = 0); + void transformToQuat(double rP[3], double rPQ[4], + const AlgoType type, int islum = 0); + + // Scan matching functions + static void getPtPairs(std::vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d); + static void getNoPairsSimple(std::vector &diff, + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2); + static void getPtPairsSimple(std::vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, + double *centroid_m, double *centroid_d); + static void getPtPairsParallel(std::vector *pairs, + Scan* Source, Scan* Target, + int thread_num, int step, + int rnd, double max_dist_match2, + double *sum, + double centroid_m[OPENMP_NUM_THREADS][3], + double centroid_d[OPENMP_NUM_THREADS][3]); + +protected: + /** + * The pose of the scan + * Note: rPos/rPosTheta and transMat _should_ + * always represent the same pose!!! + */ + double rPos[3], //!< 3D position + rPosTheta[3], //!< 3D rotation in Euler representation + rQuat[4], //!< 3D rotation in Quaternion representation + transMat[16], //!< (4x4) transformation matrix + transMatOrg[16]; //!< The original pose of the scan, e.g., from odometry + + /** + * The dalignxf transformation represents the delta transformation virtually applied + * to the tree and is used to compute are actual corresponding points. + */ + double dalignxf[16]; + + //! Run ICP on GPU instead of CPU + bool cuda_enabled; + + //! Defines the method used for nearest neighbor search and which tree to use + int nns_method; + + //! SearchTree for point pair matching, works on the search points + SearchTree* kd; + + //! This KD tree is created only for the CUDA usages + ANNkd_tree* ann_kd_tree; + + //! Voxelsize of the octtree used for reduction + double reduction_voxelSize; + + //! Which point to take out of the reduction octtree, 0 for center + int reduction_nrpts; + + //! Pointtype used for the reduction octtree + PointType reduction_pointtype; + + //! Type of the searchtree to be created + int searchtree_nnstype; + + //! Use CUDA for searching + bool searchtree_cuda_enabled; + + //! Flag whether "xyz reduced" has been initialized for this Scan yet + bool m_has_reduced; + + //! Reduction value used for octtree input + double octtree_reduction_voxelSize; + + //! Voxelsize used in the octtree itself + double octtree_voxelSize; + + //! Pointtype for the Octtree + PointType octtree_pointtype; + + //! Flags to load or save the octtrees from/to storage + bool octtree_loadOct, octtree_saveOct; + + /** + * Basic initializing constructor calling the initalization function. + * Can only be called from deriving classes. + */ + Scan(); + + /** + * This function handles the reduction of points. It builds a lock for + * multithread-safety and calls caldReducedOnDemandPrivate. + * + * The intention is to reduce points, transforme them to the initial pose and + * then copy them to original for the SearchTree. + */ + void calcReducedOnDemand(); + + //! Create specific SearchTree variants matching the capability of the Scan + virtual void createSearchTreePrivate() = 0; + + //! Create reduced points in a multithread-safe environment matching the capability of the Scan + virtual void calcReducedOnDemandPrivate() = 0; + + //! Internal function of transform which alters the reduced points + void transformReduced(const double alignxf[16]); + + //! Internal function of transform which handles the matrices + void transformMatrix(const double alignxf[16]); + + //@FIXME + public: + //! Creating reduced points + void calcReducedPoints(); + + protected: + //! Copies reduced points to original points without any transformation. + void copyReducedToOriginal(); + + //! Inverse functionality of copyReducedToOriginal. + void copyOriginalToReduced(); + +private: + //! flag for openDirectory and closeDirectory to distinguish the scans + static bool scanserver; + +public: + //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment + // it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily + boost::mutex m_mutex_reduction, m_mutex_create_tree; +}; + +#include "scan.icc" + +#endif //SCAN_H diff --git a/.svn/pristine/43/43a85f5dd7c533f0c3fcc35cc952435c5898b845.svn-base b/.svn/pristine/43/43a85f5dd7c533f0c3fcc35cc952435c5898b845.svn-base new file mode 100644 index 0000000..846583c --- /dev/null +++ b/.svn/pristine/43/43a85f5dd7c533f0c3fcc35cc952435c5898b845.svn-base @@ -0,0 +1,753 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include +#include "newmat/newmat.h" +#include "newmat/newmatap.h" + +using namespace NEWMAT; + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// validate IO types +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +/// Parse commandline options +void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, + IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose format from [uos|uosr|uos_map|" + "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" + "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") + ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than 'units") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + po::positional_options_description pd; + pd.add("input-dir", 1); + + po::options_description all; + all.add(cmd_options).add(hidden); + + po::variables_map vmap; + po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); + po::notify(vmap); + + if (vmap.count("help")) { + cout << cmd_options << endl << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &normals,vector &points, int k, const double _rPos[3] ) +{ + cout<<"Total number of points: "< &normals,vector &points, + int kmin, int kmax, const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) + break; + } + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1rd column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } + annDeallocPts(pa); +} + +/////////////////////////////////////////////////////// +/////////////NORMALS USING IMAGE NEIGHBORS //////////// +/////////////////////////////////////////////////////// +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< neighbors; + for (size_t i=0; i< extendedMap.size(); i++) + { + for (size_t j=0; j= 0 && x < (int)extendedMap.size() && + y >= 0 && y < (int)extendedMap[x].size()) { + for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { + neighbors.push_back(extendedMap[x][y][k]); + } + } + } + + nr_neighbors = neighbors.size(); + cv::Vec3f p = extendedMap[i][j][0]; + + //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero + if (nr_neighbors < 3) + { + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(0.0,0.0,0.0)); + continue; + } + + //calculate mean for all the points + Matrix X(nr_neighbors,3); + SymmetricMatrix A(3); + Matrix U(3,3); + DiagonalMatrix D(3); + + //calculate mean for all the points + for(int k = 0; k < nr_neighbors; k++) + { + cv::Vec3f pp = neighbors[k]; + mean.x += pp[0]; + mean.y += pp[1]; + mean.z += pp[2]; + } + + mean.x /= nr_neighbors; + mean.y /= nr_neighbors; + mean.z /= nr_neighbors; + //calculate covariance = A for all the points + for (int i = 0; i < nr_neighbors; ++i) { + cv::Vec3f pp = neighbors[i]; + X(i+1, 1) = pp[0] - mean.x; + X(i+1, 2) = pp[1] - mean.y; + X(i+1, 3) = pp[2] - mean.z; + } + + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + + for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { + cv::Vec3f p = extendedMap[i][j][k]; + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(n(1), n(2), n(3))); + } + } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE ////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////// +/* + void calculateNormalsFAST(vector &normals,vector &points,cv::Mat &img,vector>> extendedMap) + { + cout<<"Total number of points: "<(i-1,j); + dRdPhi += 3 *img.at(i-1,j-1); + dRdPhi += 3 *img.at(i-1,j+1); + dRdPhi -= 10*img.at(i+1,j); + dRdPhi -= 3 *img.at(i+1,j-1); + dRdPhi -= 3 *img.at(i+1,j+1); + + dRdTheta += 10*img.at(i,j-1); + dRdTheta += 3 *img.at(i-1,j-1); + dRdTheta += 3 *img.at(i+1,j-1); + dRdTheta -= 10*img.at(i,j+1); + dRdTheta -= 3 *img.at(i-1,j+1); + dRdTheta -= 3 *img.at(i+1,j+1); + + n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) + + cos(theta) * cos(phi) * dRdPhi / rho; + + n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) + + sin(theta) * cos(phi) * dRdPhi / rho; + + n[2] = cos(phi) - sin(phi) * dRdPhi / rho; + + //n[2] = -n[2]; + + m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]); + n[0] /= m; n[1] /= m; n[2] /= m; + + points.push_back(Point(x, y, z)); + normals.push_back(Point(n[0],n[1],n[2])); + } + } + } + } +*/ + +/* + * retrieve a cv::Mat with x,y,z,r from a scan object + * functionality borrowed from scan_cv::convertScanToMat but this function + * does not allow a scanserver to be used, prints to stdout and can only + * handle a single scan + */ +cv::Mat scan2mat(Scan *source) +{ + DataXYZ xyz = source->get("xyz"); + + DataReflectance xyz_reflectance = source->get("reflectance"); + + unsigned int nPoints = xyz.size(); + cv::Mat scan(nPoints,1,CV_32FC(4)); + scan = cv::Scalar::all(0); + + cv::MatIterator_ it; + + it = scan.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float x, y, z, reflectance; + x = xyz[i][0]; + y = xyz[i][1]; + z = xyz[i][2]; + if(xyz_reflectance.size() != 0) + { + reflectance = xyz_reflectance[i]; + + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; + } + + (*it)[0] = x; + (*it)[1] = y; + (*it)[2] = z; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++it; + } + return scan; +} +/* + * convert a matrix of float values (range image) to a matrix of unsigned + * eight bit characters using different techniques + */ +cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff) +{ + cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0)); + float max = 0; + // find maximum value + if (cutoff == 0.0) { + // without cutoff, just iterate through all values to find the largest + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it) { + float val = *it; + if (val > max) { + max = val; + } + } + } else { + // when a cutoff is specified, sort all the points by value and then + // specify the max so that values are larger than it + vector sorted(source.cols*source.rows); + int i = 0; + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it, ++i) { + sorted[i] = *it; + } + std::sort(sorted.begin(), sorted.end()); + max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))]; + cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl; + } + + cv::MatIterator_ src = source.begin(); + cv::MatIterator_ dst = result.begin(); + cv::MatIterator_ end = source.end(); + if (logarithm) { + // stretch values from 0 to max logarithmically over 0 to 255 + // using the logarithm allows to represent smaller values with more + // precision and larger values with less + max = log(max+1); + for (; src != end; ++src, ++dst) { + float val = (log(*src+1)*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } else { + // stretch values from 0 to max linearly over 0 to 255 + for (; src != end; ++src, ++dst) { + float val = (*src*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } + return result; +} +/// Write a pose file with the specofied name +void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber) +{ + string poseFileName = dir + "/scan" + to_string(scanNumber, 3) + ".pose"; + ofstream posout(poseFileName.c_str()); + + posout << rPos[0] << " " + << rPos[1] << " " + << rPos[2] << endl + << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + posout.clear(); + posout.close(); +} + +/// write scan files for all segments +void writeScanFiles(string dir, vector &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + /// read scan into points + DataXYZ xyz(scan->get("xyz")); + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsAKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + else + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} + diff --git a/.svn/pristine/46/46988a2b6b32bd065e5baa48b3bfffc1faa4d4cc.svn-base b/.svn/pristine/46/46988a2b6b32bd065e5baa48b3bfffc1faa4d4cc.svn-base new file mode 100644 index 0000000..4fff493 --- /dev/null +++ b/.svn/pristine/46/46988a2b6b32bd065e5baa48b3bfffc1faa4d4cc.svn-base @@ -0,0 +1,101 @@ +/* + * grabFramesCam implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#include +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif +//TODO: flip image flag + +void usage(char *progName) { + printf("%s \n", progName); + printf("i.e.: %s 6 4 0\n", progName); + printf("press space to detect chessboard and (again) to proceed.\n"); +} + +int main(int argc, char **argv) +{ + /* TODO: + * flags: + * subpixel + * camera id + * flip x and y + * pmd mode? + */ + + if(argc < 4) { + usage(argv[0]); + exit(1); + } + CvCapture *capture = cvCaptureFromCAM(atoi(argv[3])); + + IplImage *imgColor = cvQueryFrame(capture); + IplImage *img = cvCreateImage(cvGetSize(imgColor), 8, 1); + + int patx = atoi(argv[1]); + int paty = atoi(argv[2]); + CvSize patternSize = cvSize(patx, paty); + + int cornersTotal = patternSize.width * patternSize.height; + + CvPoint2D32f *corners = (CvPoint2D32f*) malloc(cornersTotal * sizeof(CvPoint2D32f)); + + cvNamedWindow("Camera", 0); + + int imageCnt = 0; + bool grabFrame = false; + + while(1) { + imgColor = cvQueryFrame(capture); + + cvFlip(imgColor, 0, -1); // flips image around the x and y axes + + cvCvtColor(imgColor, img, CV_BGR2GRAY); + + if(grabFrame) { + /* ----- Chessboard detection ----- + -------------------------------- */ + int cornersCount; // should be the same for wcam and pmd and equal totalCorners + + int found = cvFindChessboardCorners(img, patternSize, corners, + &cornersCount, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + cvFindCornerSubPix(img, corners, cornersCount, cvSize(11,11), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + + if(found && (cornersCount == cornersTotal)) { + cvDrawChessboardCorners(imgColor, patternSize, corners, cornersCount, found); + cvShowImage("Camera", imgColor); + printf("grab?\n"); + int k = cvWaitKey(0); + if(k == (int)' ') { + imageCnt++; + char *filename = (char*) malloc(11 * sizeof(char)); + sprintf(filename, "image%02i.jpg", imageCnt); + printf("saved %s.\n", filename); + cvSaveImage(filename, img); + grabFrame = false; + continue; + } + } + } + + cvShowImage("Camera", imgColor); + if((int)' ' == cvWaitKey(5)) grabFrame = true; + } + + return 0; +} + diff --git a/.svn/pristine/46/46e9db605eb52a92ba2bb1a5379b76a05667047f.svn-base b/.svn/pristine/46/46e9db605eb52a92ba2bb1a5379b76a05667047f.svn-base new file mode 100644 index 0000000..530cd90 --- /dev/null +++ b/.svn/pristine/46/46e9db605eb52a92ba2bb1a5379b76a05667047f.svn-base @@ -0,0 +1,130 @@ +------------------------------------------------------------------- + +To compile the project simply call "make". This will configure slam6d +using the default settings. If you wish to configure the project using +custom settings do: "make config". This command requires ccmake be +installed on your system. Alternatively you may change into the build +directory ".build" and configure the project with your preferred cmake +configurator, i.e.: + +cd .build && cmake -i ../ + +For Microsoft Windows, use the cmake-gui application provided by cmake +to configure and generate project files for the appropriate version of +Microsoft Visual Studio C++ of your system. Use the INSTALL target to +built the entire project. Executables (and .dll's) will then reside +in the "windows" folder. For running the binaries you need to install +the proper redistributable package. + +Some Boost libraries (graph, regex, serialization, filesystem, +interprocess) are needed to compile the slam6D program. + +If you are using Debian just do: + + aptitude install libboost-graph-dev libboost-regex-dev libboost-serialization-dev freeglut3-dev libxmu-dev libxi-dev + +or, if you are still using Debian stable (lenny): + + aptitude install libboost-graph1.35-dev libboost-regex1.35-dev libboost-serialization1.35-dev freeglut3-dev libxmu-dev libxi-dev + +for Ubuntu this would be: + + sudo apt-get install libboost-all-dev libcv-dev freeglut3-dev libxmu-dev libxi-dev + +SuSE users please use yast2 for installing the missing packages + +Additionally you need some external tools (exemplarily for Ubuntu): + + sudo apt-get install imagemagick ffmpeg libx264-120 + +------------------------------------------------------------------- + +For a detailed explanation of the programm, its usage, etc., please +refer to the high level documentation doc/documentation_HL.pdf +(esp. sections 4-6, for starters). + +IMPORTANT: +Take care to register scans first (bin/slam6D) before trying to +display them (bin/show), and think about using the point reduction +(see section 6) for a much faster scan matching result. Extremely +large scans might need to be reduced (using bin/scan_red) before +registration. This will write reduced scans in the uos format into a +directory "reduced" in the data directory. + +Three example scans are included in the dat directory, several +larger data sets can be downloaded from the data repository, +http://kos.informatik.uni-osnabrueck.de/3Dscans/ +(Alternatively, click on the "Data Repository" link on this project's +web site on Sourceforge, http://slam6d.sourceforge.net/) + +EXAMPLES: +(using the data set in the slam6d repository) +bin/slam6D -m 500 -R 5 -d 25.0 --metascan dat +bin/show dat + +(using the data set in the slam6d repository) +bin/slam6D --max=500 -r 10.2 -i 20 --metascan dat +bin/show dat + +(using hannover1.tgz from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -s 1 -e 65 -r 10 -i 100 -d 75 -D 250 --epsICP=0.00001 + -I 50 --cldist=750 -L 0 -G 1 /home/nuechter/dat/dat_hannover1 +bin/show -s 1 -e 65 /home/nuechter/dat/dat_hannover1 + +(using hannover2.tgz from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -q -r 10 -f rts -s 23 -d 75 -L 4 --cldist=1500 -G 1 -D -1 + --DlastSLAM 250 --graphDist 200 -I 50 hannover2 +bin/show -f rts -s 23 hannover2 + +(using kvarntorp_mine.tgz (dat_mine1) form http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -s 1 -e 76 -r 10 -m 3000 -d 50 -i 1000 --epsICP=0.000001 + -I 50 -D 75 --clpairs=5000 -f old /home/nuechter/dat/dat_mine1/ +bin/show -s 1 -e 76 -m 3000 -f old /home/nuechter/dat/dat_mine1/ + +(using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/scan_red -s 0 -e 12 -r OCTREE -v 10 --octree 0 -f RIEGL_TXT /home/nuechter/dat/bremen_city +bin/slam6D -a 2 -q /home/nuechter/dat/bremen_city/reduced -f uos -d 150 + -s 0 -e 12 --anim=1 -n /home/nuechter/dat/bremen_city/bremen.net + -G 1 -D 100 -i 0 -I 50 -p --epsSLAM=0.0 +bin/show -s 0 -e 12 /home/nuechter/dat/bremen_city/reduced + +(using UniKoblenz_CampusTour3_OsnabrueckFormat.tar.gz from +http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -s 1 -e 320 -r 20 -i 300 --epsICP=0.000001 -d 45 -D 45 + -f uos --algo=2 -l 10 -L 4 -I 100 -G 1 + /home/nuechter/dat/UniKoblenz_CampusTour3_OsnabrueckFormat/ +bin/show -s 1 -e 320 -f uos /home/nuechter/dat/UniKoblenz_CampusTour3_OsnabrueckFormat/ +------------------------------------------------------------------- + +For detecting planes compile with the WITH_PLANE option. +Adapt the settings in bin/hough.cfg for your data set. + +EXAMPLE: (using the data set in the slam6d repository, no modification +of bin/hough.cfg necessary) + +bin/planes -s 0 dat +bin/show -s 0 -e 0 dat -l dat/planes/planes.list + +(using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +adapt these settings in bin/hough.cfg: +RhoNum 500 +RhoMax 5000 +MaxPointPlaneDist 50.0 +MinPlaneSize 50 +PointDist 100.0 +/bin/planes -f riegl_txt -s 0 /home/nuechter/dat/bremen_city/ -r 50 -O 1 -m 5000 +/bin/show -s 0 -e 0 /home/nuechter/dat/bremen_city/ -f riegl_txt -l dat/planes/planes.list -r 10 -O 1 -m 5000 + +------------------------------------------------------------------- +The IO relevant parameters -f(ormat), -s(tart), -e(nd) can be omitted +in slam6D and show if a 'format' file exists in the directory, which +contains key=value lines (spaces are trimmed automatically) for +format, start, end with the same values as in the commandline. These +format-file parameters will be overwritten by commandline parameters +so that the format-file will provide the right IO type and full index +range and the user can overwrite the index range as he sees fit. + +------------------------------------------------------------------- +A reference manual can be found in doc/refman.pdf resp. +doc/html/index.html (type in 'make docu' to compile the doxygen +documentation for the HTML files). diff --git a/.svn/pristine/56/56e5e8fa24eb30e5fee849605e5637e9624128a7.svn-base b/.svn/pristine/56/56e5e8fa24eb30e5fee849605e5637e9624128a7.svn-base new file mode 100644 index 0000000..a6ec492 --- /dev/null +++ b/.svn/pristine/56/56e5e8fa24eb30e5fee849605e5637e9624128a7.svn-base @@ -0,0 +1,154 @@ +/* + * calibrate implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + + +void usage(char *progName) { + printf("%s \n", progName); + printf("\twhere board-size-x and y is count of *inner* corners of the booard"); + printf("i.e.: %s 6 4 0.04 image*\n", progName); + printf("Use more then ten images.\nPress space bar to proceed.\n"); +} + +void calibrate( CvMat *intrinsic, CvMat *distortion, CvSize imgSz, CvSize boardSz + , double boardSide, CvPoint2D32f **corners, int boardsCnt) { + + int totalPoints = boardSz.width * boardSz.height; + // object points (model) + CvMat *objPts = cvCreateMat(totalPoints * boardsCnt, 3, CV_32FC1); + // found points + CvMat *imgPts = cvCreateMat(totalPoints * boardsCnt, 2, CV_32FC1); + // points count + CvMat *ptsCnt = cvCreateMat(boardsCnt, 1, CV_32SC1); + + // copy corners to matrix and fill model matrix + for(int i = 0; i < boardsCnt; i++) { + for(int j = 0; j < totalPoints; j++) { + int s = i * totalPoints; + CV_MAT_ELEM(*imgPts, float, s+j, 0) = corners[i][j].x; + CV_MAT_ELEM(*imgPts, float, s+j, 1) = corners[i][j].y; + CV_MAT_ELEM(*objPts, float, s+j, 0) = boardSide * (j / boardSz.width); + CV_MAT_ELEM(*objPts, float, s+j, 1) = boardSide * (j % boardSz.width); + CV_MAT_ELEM(*objPts, float, s+j, 2) = 0.0f; + } + CV_MAT_ELEM(*ptsCnt, int, i, 0) = totalPoints; + } + + // initial guess + CV_MAT_ELEM(*intrinsic, float, 0, 0) = 1.0f; + CV_MAT_ELEM(*intrinsic, float, 1, 1) = 1.0f; + + cvCalibrateCamera2( objPts, imgPts, ptsCnt, imgSz + , intrinsic, distortion + , NULL, NULL, 0 ); + return; +} + + +int main(int argc, char **argv) +{ + if(argc < 5) { + usage(argv[0]); + exit(1); + } + + int patx = atoi(argv[1]); + int paty = atoi(argv[2]); + CvSize boardSz = cvSize(patx, paty); + int loadImageCnt = argc - 4; + int cornersTotal = boardSz.width * boardSz.height; + float boardSide = atof(argv[3]);; + char **images = &argv[4]; + + // intrinsic matrices and ditortion params + CvMat *intrinsic = cvCreateMat(3, 3, CV_32F); + CvMat *distortion = cvCreateMat(1, 4, CV_32F); + + + IplImage *img = cvLoadImage(images[0], CV_LOAD_IMAGE_GRAYSCALE); + IplImage *imgColor = cvCreateImage(cvGetSize(img), 8, 3); + + int subpixel; + if(cvGetSize(img).width < 300) { + subpixel = 4; + } + else { + subpixel = 11; + } + + int imagesWithBoard = 0; + + CvPoint2D32f **corners = (CvPoint2D32f**)calloc(100, sizeof(CvPoint2D32f*)); + corners[0] = (CvPoint2D32f*) malloc(cornersTotal * sizeof(CvPoint2D32f)); + + + cvNamedWindow("Cam", 0); + + for(int imagesLoaded = 0; imagesLoaded < loadImageCnt; imagesLoaded++) { + img = cvLoadImage(images[imagesLoaded], CV_LOAD_IMAGE_GRAYSCALE); + + cvCvtColor(img, imgColor, CV_GRAY2BGR); + + int cornersCount; + + int found = cvFindChessboardCorners(img, boardSz, corners[imagesWithBoard], + &cornersCount, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + if(found) cvFindCornerSubPix(img, corners[imagesWithBoard], cornersCount, cvSize(subpixel, subpixel), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + + if(found && (cornersCount == cornersTotal)) { + cvDrawChessboardCorners(imgColor, boardSz, corners[imagesWithBoard], cornersCount, found); + imagesWithBoard++; + corners[imagesWithBoard] = (CvPoint2D32f*) malloc(cornersTotal * sizeof(CvPoint2D32f)); + } + cvShowImage("Cam", imgColor); + cvWaitKey(0); + + } + + cvDestroyAllWindows(); + + printf("calibrating...\n"); + fflush(stdout); + + //TODO: can be started in parallel to watch calibration on image undistortion :) + calibrate(intrinsic, distortion, cvGetSize(img), boardSz, boardSide, corners, imagesWithBoard); + + // save to xml files + cvSave("./intrinsic.xml", intrinsic); + cvSave("./distortion.xml", distortion); + printf("matrices saved to xml files.\n"); + + // let OS clean all images and matrices + return 0; +} + + +/*void usage(char *progName) { + printf( "usage:\n \ + %s [-s]\n \ + \tcamera-id is v4l id or -1 for pmd cam \ + \tpress space or 'a' to grab image\n \ + \tpress 'n' to skip grabbed frame\n \ + \tpress 'c' to finish frame grabbing start calibration\n \ + or:\n \ + %s \n", progName, progName); + fflush(stdout); +}*/ + diff --git a/.svn/pristine/58/587c3070c8982445ad454ec6a9383b8f9f656758.svn-base b/.svn/pristine/58/587c3070c8982445ad454ec6a9383b8f9f656758.svn-base new file mode 100644 index 0000000..51155b7 --- /dev/null +++ b/.svn/pristine/58/587c3070c8982445ad454ec6a9383b8f9f656758.svn-base @@ -0,0 +1,27 @@ +IF (WITH_THERMO) + find_package(OpenCV REQUIRED) + + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + include_directories(${CMAKE_SOURCE_DIR}/include/shapes/) + include_directories(${CMAKE_SOURCE_DIR}/include/thermo/) + include_directories(/usr/include/) + include_directories(/usr/include/opencv) + + add_executable(caliboard caliboard.cc) + add_executable(thermo thermo.cc) +# add_executable(thermo thermo.cc src/cvaux.cpp src/cvblob.cpp src/cvcolor.cpp src/cvcontour.cpp src/cvlabel.cpp src/cvtrack.cpp) + + IF(UNIX) + target_link_libraries(caliboard scan shape newmat dl ANN) + target_link_libraries(thermo scan shape newmat dl ANN) + target_link_libraries(thermo GL GLU cvblob ${OpenCV_LIBS} scan ANN) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(caliboard scan shape newmat XGetopt ANN) + target_link_libraries(thermo scan shape newmat XGetopt ANN) + target_link_libraries(thermo GL GLU cvblob ${OpenCV_LIBS} scan ANN) + ENDIF(WIN32) + + +ENDIF(WITH_THERMO) diff --git a/.svn/pristine/5f/5f8ef3e80ac6799ab7cdb9fcc375f487fe412837.svn-base b/.svn/pristine/5f/5f8ef3e80ac6799ab7cdb9fcc375f487fe412837.svn-base new file mode 100644 index 0000000..c4eea67 --- /dev/null +++ b/.svn/pristine/5f/5f8ef3e80ac6799ab7cdb9fcc375f487fe412837.svn-base @@ -0,0 +1,200 @@ +/* + * PMD implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "pmdsdk2.h" + +#include +#include + +#include "cvpmd.h" +#include + +/** TODO + * inten max amplitude + * error handling + */ + +PMD *initPMD(const char* plugin, const char *ip) { + PMD *io = (PMD*)malloc(sizeof(PMD)); + + if(0 != pmdOpen (&io->hnd, plugin, ip, plugin, "")) { + fprintf(stderr, "ERROR: could not connect!\n"); + exit(1); + } + + //pmdUpdate(io->hnd); + //pmdGetSourceDataDescription(io->hnd, &io->dd); + io->dd.std.numColumns = 64; + io->dd.std.numRows = 50; //FIXME! + + io->data = (float*)malloc(io->dd.std.numColumns * io->dd.std.numRows * sizeof(float)); + + return io; +} + +float *pmdDataPtr(PMD *p) { + return p->data; +} + +void releasePMD(PMD **pmd) { + pmdClose((*pmd)->hnd); + free((*pmd)->data); + free(*pmd); + *pmd = 0; +} + + +static float max(const PMD *p) { + float max = 0.0f; + for(unsigned int k = 0; k < p->dd.std.numRows*p->dd.std.numColumns; k++) { + if(p->data[k] > max) max = p->data[k]; + } + //printf("max = %f\n", max); + return max; +} + +//static inline void setPix(const IplImage *m, const int c, const int r, const char v) { m->imageData[r*m->width + c] = v; } + + +CvSize pmdGetSize(const PMD *p) { + return cvSize(p->dd.std.numColumns, p->dd.std.numRows); +} + + +IplImage *toIplImage(const PMD *p, IplImage *img = 0) { + int cols = p->dd.std.numColumns; + int rows = p->dd.std.numRows; + + IplImage *imgp; + if(!img) imgp = cvCreateImage(pmdGetSize(p), 8, 1); + else imgp = img; + + //FIXME: scaled! + float m = max(p); + for(int r = 0; r < rows; r++) { + for(int c = 0; c < cols; c++) { + //FIXME: mess with the rows and colums + CV_IMAGE_ELEM(imgp, uint8_t, r, c) = (uint8_t) 255.0f * p->data[r*cols + c] / m; + } + } + + return imgp; +} + +/*CvPoint3D32f unionVec(const CvPoint uv, const CvMat *intrinsic) { + //TODO: not defined yet + // with this function pmdProjectToCartesian would look like: + // pmdProjectToCartesian pt depth mat = depth * unionVec pt mat + + return cvPoint3D32f(0,0,0); +}*/ + + +static inline CvPoint3D32f pmdProjectToCartesian(const CvMat *intrinsicMatrix, const CvPoint2D32f uv, const float dist) { + /* Lukas Dierks and Jan Wuelfing's projectToCartesian */ + float fx = cvmGet(intrinsicMatrix, 0, 0); + float cx = cvmGet(intrinsicMatrix, 0, 2); + float fy = cvmGet(intrinsicMatrix, 1, 1); + float cy = cvmGet(intrinsicMatrix, 1, 2); + + float u = uv.x; + float v = uv.y; + float r = dist; + float u2 = u*u; + float v2 = v*v; + + float cx2 = cx*cx; + float cy2 = cy*cy; + float fx2 = fx*fx; + float fy2 = fy*fy; + + // Reverse Projection + float squareroot = sqrt( + cy2 * fx2 + + cx2 * fy2 + + fx2 * fy2 - + 2 * cx * fy2 * u + + fy2 * u2 - + 2 * cy * fx2 * v + + fx2 * v2 + ); + + return cvPoint3D32f((fy * r * (cx - u)) / squareroot, + (fx * r * (cy - v)) / squareroot, + (fx * fy * r) / squareroot); +} + +CvPoint3D32f *cvProjectArrayToCartesian( const CvMat *intrinsicMatrix + , const CvPoint2D32f *pts, const int ptsCnt + , CvPoint3D32f *unitVs) { + + for(int i = 0; i < ptsCnt; i++) + unitVs[i] = pmdProjectToCartesian(intrinsicMatrix, pts[i], 1.0); + return unitVs; +} + + +CvPoint3D32f **pmdProjectArrayToCartesian(const PMD *p, const CvMat *intrinsicMatrix, CvPoint3D32f **pts) { + for(unsigned int i = 0; i < p->dd.std.numRows; i++) + for(unsigned int j = 0; j < p->dd.std.numColumns; j++) + pts[i][j] = pmdProjectToCartesian(intrinsicMatrix, cvPoint2D32f(i,j), p->data[j*p->dd.std.numColumns + i]); + return pts; +} + + + +IplImage *pmdQueryImage(PMD *p, IplImage *img = 0) { + pmdGetIntensities(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return toIplImage(p, img); +} + +IplImage *pmdQueryImageAsync(PMD *p, IplImage *img = 0) { + pmdGetIntensitiesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return toIplImage(p, img); +} + + +void pmdRetriveDistances(PMD *p) { + pmdGetDistances(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return; +} +void pmdRetriveDistancesAsync(PMD *p) { + pmdGetDistancesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return; +} + +IplImage *pmdQueryDistances(PMD *p, IplImage *img = 0) { + pmdGetDistances(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return toIplImage(p, img); + +} + +IplImage *pmdQueryDistancesAsync(PMD *p, IplImage *img = 0) { + pmdGetDistancesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return toIplImage(p, img); + +} + +IplImage *pmdQueryAmplitudes(PMD *p, IplImage *img = 0) { + pmdGetAmplitudesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return toIplImage(p, img); +} + +IplImage *pmdQueryAmplitudesAsync(PMD *p, IplImage *img = 0) { + pmdGetAmplitudes(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float)); + return toIplImage(p, img); +} diff --git a/.svn/pristine/6c/6c51b8b375d3d9376cbaeb0e2b4c72f731ccca1e.svn-base b/.svn/pristine/6c/6c51b8b375d3d9376cbaeb0e2b4c72f731ccca1e.svn-base new file mode 100644 index 0000000..b82ea53 --- /dev/null +++ b/.svn/pristine/6c/6c51b8b375d3d9376cbaeb0e2b4c72f731ccca1e.svn-base @@ -0,0 +1,869 @@ +/* + * pose implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#include +#include +#include +#include +#include + +// OpenCV +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +// GL: GLFW (window etc, ala glut) and FTGL (text rendering) +#include +#include + +// PMD-related stuff +#include "pmdsdk2.h" +#include "cvpmd.h" +#include "pmdWrap.h" +#include "history.h" + +// SLAM6D +#include "slam6d/point.h" +#include "slam6d/ptpair.h" +#include + + +FTGLPixmapFont *font; // opengl text rendring + +void usage(char* progName) { + printf("usage: %s [options]\n", progName); + printf("options:\n \ +\t--ui 0|1\t disable|enable ui (default: 1)\n\ +\t--gl 0|1\t disable|enable 3d rendering (default: 1)\n\ +\t--cfg file\t parse configuration from file (default: pmdc.conf)\n\ +\t--help\t\t display this help.\n"); +} + + + +void render( History *history, CvPoint3D32f *camPts, int trPtsCnt + , CvMat *rot, CvMat *trn + , CvPoint3D32f **camPts3D, char **pts3DStatus + , int reprojected, bool poseEstimated); + +/* + reprojects PMD-aquired 3D points on the web cam image +*/ + +//FIXME this function should be dispached >_> +int projectImageToPMD( CvMat *rot, CvMat *trn, CvMat *intrinsic + , CvPoint3D32f **pmdPts, CvSize pmdSz + , CvSize camSz, CvPoint **pmd2imgIdxs + , CvPoint2D32f *trackedPts + , int *trPtsCnt, CvPoint3D32f *trackedPts3D, char *pts3DStatus + , CvPoint *tr2pmdIdxs) { + /* TODO: + assert null image, not clean image, bad image depth + */ + // pmd point's matrix + CvMat *pt = cvCreateMat(3, 1, CV_32FC1); + // point in cam coorinate system + CvMat *reprojPt = cvCreateMat(3, 1, CV_32FC1); + // points on cam screen + CvMat *camPt3 = cvCreateMat(3, 1, CV_32FC1); + CvMat *camPt = cvCreateMat(2, 1, CV_32FC1); + // rotation matrix + CvMat *rotMat = cvCreateMat(3, 3, CV_32FC1); + cvRodrigues2(rot, rotMat, NULL); + + // clear previous found tracked pts + for(int k = 0; k < *trPtsCnt; k++) pts3DStatus[k] = 0; + + //int l = 0; // count corresponding points + int reprojected = 0; + + for(int j = 0; j < pmdSz.width; j++) + for(int i = 0; i < pmdSz.height; i++) { + if( fabs(pmdPts[i][j].x) > 20.0 + || fabs(pmdPts[i][j].y) > 20.0 + || fabs(pmdPts[i][j].z) > 20.0) continue; //FIXME: more realistic limit, status = false + // convert to mat + CV_MAT_ELEM(*pt, float, 0, 0) = (float)pmdPts[i][j].x; + CV_MAT_ELEM(*pt, float, 1, 0) = (float)pmdPts[i][j].y; + CV_MAT_ELEM(*pt, float, 2, 0) = (float)pmdPts[i][j].z; + + // reproject to the camera coordinate system + cvMatMulAdd(rotMat, pt, trn, reprojPt); + // project to the image + cvMatMul(intrinsic, reprojPt, camPt3); +// cvConvertPointsHomogenious(camPt3, camPt); +/* cvProjectPoints2(pt, rot, trn, intrinsic, NULL, camPt + , NULL, NULL, NULL, NULL, NULL);*/ + float scale = (float)CV_MAT_ELEM(*camPt3, float, 2, 0); + float xf = CV_MAT_ELEM(*camPt3, float, 0, 0) / scale; + float yf = CV_MAT_ELEM(*camPt3, float, 1, 0) / scale; + int x = (int)xf; + int y = camSz.height - (int)yf; //FIXME revise coordinates + + if((x < 0) || (x > camSz.width) || + (y < 0) || (y > camSz.height)) { + pmd2imgIdxs[i][j].x = -1; + pmd2imgIdxs[i][j].y = -1; + continue; + } else { // point is projected to cam + // contains which PMD point is seen from this pixel + pmd2imgIdxs[i][j].x = x; + pmd2imgIdxs[i][j].y = y; + } + + //find tracked 3d points (fused here, can be dispached) + for(int k = 0; k < *trPtsCnt; k++) + if(pts3DStatus[k]) continue; // kill to accumulate (*) + else + if( (abs((trackedPts[k].x - (float)x)) < 3) //TODO: distance SHOULD be chousen depending on depth!!! + && (abs((trackedPts[k].y - (float)y)) < 3)) { //FIXME: hardcoded distance + //TODO: we can accumulate points here to get more presise results (*) + trackedPts3D[k].x = pmdPts[i][j].x; + trackedPts3D[k].y = pmdPts[i][j].y; + trackedPts3D[k].z = pmdPts[i][j].z; + pts3DStatus[k] = 1; + tr2pmdIdxs[k].x = i; //FIXME: cvPoint(i,j)? + tr2pmdIdxs[k].y = j; + reprojected++; + break; // kill to accumulate (*) + } + } + + cvReleaseMat(&pt); + cvReleaseMat(&reprojPt); + cvReleaseMat(&camPt); + cvReleaseMat(&rotMat); + + return reprojected; +} + +static inline float cvPoint3D32fNorm(CvPoint3D32f pt) { + return sqrt( (pt.x - pt.x)*(pt.x - pt.x) + +(pt.y - pt.y)*(pt.y - pt.y) + +(pt.z - pt.z)*(pt.z - pt.z)); +} + +float dpthS( IplImage *img + , CvPoint ptI, CvPoint dp + , CvPoint3D32f **pts + , CvPoint ptD, CvSize pmdSz + , float sD, float sC) { + CvSize imgSz = cvGetSize(img); + if( ptD.x + dp.x > pmdSz.width - 1 || ptD.x + dp.x < 0 + || ptD.y + dp.y > pmdSz.height - 1 || ptD.y + dp.y < 0 + || ptI.x + dp.x > imgSz.width - 1 || ptI.x + dp.x < 0 + || ptI.y + dp.y > imgSz.height - 1 || ptI.y + dp.y < 0) return 0.0; + + uchar *pI = &((uchar*) (img->imageData + img->widthStep * ptI.y))[ptI.x*3]; + uchar *pJ = &((uchar*) (img->imageData + img->widthStep * (ptI.y+dp.y)))[(ptI.x+dp.x)*3]; + float dr = (float)pI[2] - (float)pJ[2]; + float dg = (float)pI[1] - (float)pJ[1]; + float db = (float)pI[0] - (float)pJ[0]; + float wij = exp(-(sqrt(dr*dr + dg*dg + db*db))/sC); + float dpI = cvPoint3D32fNorm(pts[ptD.x][ptD.y]); + float dpJ = cvPoint3D32fNorm(pts[ptD.x + dp.x][ptD.y + dp.y]); + float s = exp(-wij*(dpI - dpJ)*(dpI - dpJ)/sD); + return s; +} + + + +#define DPTHS(dx, dy) (dpthS( img, idx, cvPoint((dx),(dy)) \ + , pmdPts \ + , pmdIdx, pmdSz \ + , sigmaDepth, sigmaColor)) + + +void outliersDepthAndColor( CvPoint3D32f **pmdPts, IplImage *img, CvSize pmdSz + , CvPoint2D32f *trackedPts, int trPtsCnt, CvPoint* tr2pmdIdxs + , char *pts3DStatus // this actually return parameter + , float sigmaDepth, float threshold, float sigmaColor + ) { + if(threshold < 0.0) return; + // depth score outliers removal, see pmdc.conf comments + for(int k = 0; k < trPtsCnt; k++) { + //FIXME: check array bounds + CvPoint pmdIdx = tr2pmdIdxs[k]; + CvPoint idx = cvPointFrom32f(trackedPts[k]); + float s00, s01, s02; + s00 = DPTHS(-1, -1); + s01 = DPTHS( 0, -1); + s02 = DPTHS( 1, -1); + float s10 = DPTHS(-1, 0); + float s12 = DPTHS( 1, 0); + float s20 = DPTHS(-1, 1); + float s21 = DPTHS( 0, 1); + float s22 = DPTHS( 1, 1); + float score = s00 + s01 + s02 + + s10 + 0.0 + s12 + + s20 + s21 + s22; + printf("score = %f\n", score); + if(score < threshold) pts3DStatus[k] = 0; + } + +} + +int motionMeanAndVariance(CvPoint3D32f **camPts3D, char **pts3DStatus, int trPtsCnt, float *mean, float *var) { + +// float *mean = meanAndVariance; +// float *var = &(meanAndVariance[3]); +// float motion[3]; + float dx, dy, dz; + float magSq; + float motionSum = 0; //[3] = {0.0, 0.0, 0.0}; + float motionSqrSum = 0; //[3] = {0.0, 0.0, 0.0}; + + int sz = 0; // pairs count + for(int i = 0; i < trPtsCnt; i++) + if(pts3DStatus[1][i] && pts3DStatus[0][i]) { + sz++; + + dx = camPts3D[1][i].x - camPts3D[0][i].x; + dy = camPts3D[1][i].y - camPts3D[0][i].y; + dz = camPts3D[1][i].z - camPts3D[0][i].z; + + magSq = dx*dx + dy*dy + dz*dz; + + motionSum += sqrt(magSq); //FIXME: optimisation, we can use it without sqrt + + motionSqrSum += magSq; //thus, it would be sqr here + +/* motionSum[0] += motion[0]; + motionSum[1] += motion[1]; + motionSum[2] += motion[2]; + + motionSqrSum[0] += motion[0]*motion[0]; + motionSqrSum[1] += motion[1]*motion[1]; + motionSqrSum[2] += motion[2]*motion[2];*/ + } + + if(0 == sz) return 0; + // mean + *mean = motionSum / sz; +/* mean[0] = motionSum[0] / (float)sz; + mean[1] = motionSum[1] / (float)sz; + mean[2] = motionSum[2] / (float)sz;*/ + + // variance + + for(int i = 0; i < trPtsCnt; i++) + if(pts3DStatus[1][i] && pts3DStatus[0][i]) { + dx = camPts3D[1][i].x - camPts3D[0][i].x; + dy = camPts3D[1][i].y - camPts3D[0][i].y; + dz = camPts3D[1][i].z - camPts3D[0][i].z; + + magSq = dx*dx + dy*dy + dz*dz; + + *var += (magSq - *mean)*(magSq - *mean); + } + + *var /= sz; + // = motionSqrSum / sz - (*mean)*(*mean); + +/* var[0] = motionSqrSum[0] / (float)sz - mean[0]*mean[0]; + var[1] = motionSqrSum[1] / (float)sz - mean[1]*mean[1]; + var[2] = motionSqrSum[2] / (float)sz - mean[2]*mean[2];*/ + return 1; +} + +void outliersSpeedSigma(CvPoint3D32f **camPts3D, char **pts3DStatus, int trPtsCnt, float mean, float var) { + + float dx, dy, dz; + float mag; + float sigma = sqrt(var); + for(int i = 0; i < trPtsCnt; i++) + if(pts3DStatus[1][i] && pts3DStatus[0][i]) { + dx = camPts3D[1][i].x - camPts3D[0][i].x; + dy = camPts3D[1][i].y - camPts3D[0][i].y; + dz = camPts3D[1][i].z - camPts3D[0][i].z; + + mag = sqrt(dx*dx + dy*dy + dz*dz); + + if(fabs(mag - mean) > sigma) { + pts3DStatus[0][i] = 0; + } + } +} + + +int main(int argc, char **argv) { + // settings + bool ui = true; + bool gl = true; + bool fps = false; + const char *config = "./pmdc.conf"; + + // args parsing + for(int i = 1; i < argc; i++) { + if (!strcmp(argv[i], "--ui")) ui = (bool) atoi(argv[++i]); + else if (!strcmp(argv[i], "--gl")) gl = (bool) atoi(argv[++i]); + else if (!strcmp(argv[i], "--fps")) fps = true; + else if (!strcmp(argv[i], "--cfg")) config = argv[++i]; + //TODO: config + else if (!strcmp(argv[i], "--help") + || !strcmp(argv[i], "-h")) { + usage(argv[0]); + return 1; + } else { + fprintf(stderr, "ERROR: unknown flag: %s\n", argv[i]); + return 1; + } + } + // pose guess + CvMat *rotMatGuess = cvCreateMat(3,3, CV_32FC1); + CvMat *rotGuess = cvCreateMat(3, 1, CV_32FC1); + CvMat *trnGuess = cvCreateMat(3, 1, CV_32FC1); + + + /***** init device and allocate images *****/ + PMDCam *pmdc = initPMDCam(config); + CvSize pmdSz = cvGetSize(pmdc->iPMDI); + CvSize camSz = cvGetSize(pmdc->iCam); + printf("pose: pmd init done\n"); + + /***** essential matrix *****/ + + CvMat *rot = (CvMat*)cvLoad("../essential-rot.xml"); //FIXME: load path from cfg + CvMat *trn = (CvMat*)cvLoad("../essential-trn.xml"); + + /***** LK-tracking *****/ + IplImage *swapTemp; + + int featuresMax = pmdc->_track.maxFeatures; + int trPtsCnt = 0; // counts found points + + // eigenvalues for GFTT + IplImage* eig = cvCreateImage(camSz, 32, 1); + IplImage* tmp = cvCreateImage(camSz, 32, 1); + IplImage* mask = cvCreateImage(camSz, IPL_DEPTH_8U, 1); + + // previous image and pyramides + IplImage *imgCamPrv = cvCreateImage(camSz, 8, 1); + IplImage *imgCamPyr = cvCreateImage(camSz, 8, 1); + IplImage *imgCamPyrPrv = cvCreateImage(camSz, 8, 1); + + // prev and curr tracked points + CvPoint2D32f *camPts[2]; + camPts[0] = (CvPoint2D32f*) cvAlloc(featuresMax * sizeof(CvPoint2D32f)); + camPts[1] = (CvPoint2D32f*) cvAlloc(featuresMax * sizeof(CvPoint2D32f)); + CvPoint3D32f *camPts3D[2]; + camPts3D[0] = (CvPoint3D32f*) cvAlloc(featuresMax * sizeof(CvPoint3D32f)); + camPts3D[1] = (CvPoint3D32f*) cvAlloc(featuresMax * sizeof(CvPoint3D32f)); + + CvPoint2D32f *swapPts; + CvPoint3D32f *swapPts3; // i guess i can use void* d:D + char *swapStatus; + char *camPtsStatus = (char*)cvAlloc(featuresMax); + char *pts3DStatus[2]; + pts3DStatus[0] = (char*) cvAlloc(featuresMax * sizeof(char)); + pts3DStatus[1] = (char*) cvAlloc(featuresMax * sizeof(char)); + CvPoint *tr2pmdIdxs = (CvPoint*) cvAlloc(featuresMax * sizeof(CvPoint)); + + // 3d rays where points points :P + CvPoint3D32f *trackedPts = (CvPoint3D32f*) cvAlloc(featuresMax * sizeof(CvPoint3D32f)); + + // contains (row,col) of pmd 3D pts + CvPoint **pmd2imgIdxs = (CvPoint**) cvAlloc(pmdSz.height * sizeof(CvPoint*)); + for(int i = 0; i < pmdSz.height; i++) + pmd2imgIdxs[i] = (CvPoint*) cvAlloc(pmdSz.width * sizeof(CvPoint)); + + // pmd history + History *history = createHistory(); + + /***** ui and gl stuff *****/ + if(ui) { + cvNamedWindow("PMD", 0); + cvNamedWindow("Cam", 0); + } + + if(gl) { + glfwInit(); + if(!glfwOpenWindow(640, 480, 8, 8, 8, 8, 24, 0, GLFW_WINDOW)) { + glfwTerminate(); + fprintf(stderr, "ERROR: can't init glfw window!\n"); + return 1; + } + } + //FIXME: put this in if(gl) + font = new FTGLPixmapFont("./djvm.ttf"); + + if(font->Error()) { + fprintf(stderr, "ERROR: can't load font ./djvm.ttf"); + return 1; + } + font->FaceSize(20); + + // fps counting + time_t tic=time(0); + time_t tac; + + int fpsCnt = 0; + + //icp pairs + vector pairs; + vector motion; + float mean, var; + + /***** main loop *****/ + /*********************/ + int frames = 0; + int goodFrames = 0; + while(1) { + /***** fps counting *****/ + if(fps) { + tac = time(0); + if(tac - tic >= 1) { + printf("%i FPS\n", fpsCnt); + fflush(stdout); + fpsCnt = 0; + tic=tac; + } + fpsCnt++; + } + if(grabData(pmdc)) break; // end of seq? + + + /***** tracking *****/ + //if(trPtsCnt) + cvCalcOpticalFlowPyrLK( imgCamPrv, pmdc->iCam, imgCamPyrPrv, imgCamPyr + , camPts[0], camPts[1], trPtsCnt + , cvSize(pmdc->_track.winSz, pmdc->_track.winSz), 3, camPtsStatus, 0 + , cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03) + , pmdc->_track.trackingFlags); + pmdc->_track.trackingFlags |= CV_LKFLOW_PYR_A_READY; + + // filter out not tracked points + int k = 0; + for(int i = 0; i < trPtsCnt; i++) + if(camPtsStatus[i]) { + camPts3D[0][k] = camPts3D[0][i]; + pts3DStatus[0][k] = pts3DStatus[0][i]; + camPts[1][k++] = camPts[1][i]; + } + + trPtsCnt = k; + + /***** 3d (re)projection *****/ + cvProjectArrayToCartesian(pmdc->intrinsicCam, camPts[1], trPtsCnt, trackedPts); + + int reprojected = projectImageToPMD( rot, trn, pmdc->intrinsicCam, pmdc->pts, pmdSz, camSz, pmd2imgIdxs + , camPts[1], &trPtsCnt, camPts3D[1], pts3DStatus[1], tr2pmdIdxs); + double alignError = 0.0; + bool poseEstimated = false; + if(reprojected >= pmdc->minPts4Pose) { // we need at least minPts4Pose points + + if(motionMeanAndVariance(camPts3D, pts3DStatus, trPtsCnt, &mean, &var)) + // TODO: can be fused with centroid computation + outliersSpeedSigma(camPts3D, pts3DStatus, trPtsCnt, mean, var); + + outliersDepthAndColor( pmdc->pts, pmdc->iCamColor, pmdSz, camPts[1], trPtsCnt + , tr2pmdIdxs, pts3DStatus[1], pmdc->sigmaDepth + , pmdc->dpThreshold, pmdc->sigmaColor); + + + pairs.clear(); + double centroidM[3] = {0.0, 0.0, 0.0}; + double centroidD[3] = {0.0, 0.0, 0.0}; + double pt1[3]; + double pt2[3]; + + for(int i = 0; i < trPtsCnt; i++) + if(pts3DStatus[1][i] && pts3DStatus[0][i]) { + pt1[0] = camPts3D[0][i].x; + pt1[1] = camPts3D[0][i].y; + pt1[2] = camPts3D[0][i].z; + pt2[0] = camPts3D[1][i].x; + pt2[1] = camPts3D[1][i].y; + pt2[2] = camPts3D[1][i].z; + + //TODO can be fused -- (+=) :: a -> a -> a + centroidM[0] += camPts3D[0][i].x; + centroidM[1] += camPts3D[0][i].y; + centroidM[2] += camPts3D[0][i].z; + centroidD[0] += camPts3D[1][i].x; + centroidD[1] += camPts3D[1][i].y; + centroidD[2] += camPts3D[1][i].z; + + PtPair currentPair(pt1, pt2); + pairs.push_back(currentPair); + } + + reprojected = pairs.size(); + + if(reprojected >= pmdc->minPts4Pose) { // enough corresponding points + centroidM[0] /= reprojected; + centroidM[1] /= reprojected; + centroidM[2] /= reprojected; + centroidD[0] /= reprojected; + centroidD[1] /= reprojected; + centroidD[2] /= reprojected; + + double transformMat[16]; + try { alignError = pmdc->icp->Point_Point_Align(pairs, transformMat, centroidM, centroidD); } + catch(...) { fprintf(stderr, "ERROR: matrix is singular!\n"); } + + if(!gl) printf( "%i: align error: %f, 3d pts count: %i, 2d pts count: %i\n" + , frames, alignError, reprojected, trPtsCnt); + + for(int i = 1; i < 16; i++) + if (i%4 > 2) continue; // bottom row + else if(i/4 > 2) CV_MAT_ELEM(*trnGuess, float, i%4, 0) = transformMat[i]; + else CV_MAT_ELEM(*rotMatGuess, float, i/4, i%4) = transformMat[i]; // right col + + cvRodrigues2(rotMatGuess, rotGuess, NULL); + + if(alignError < pmdc->maxError) + poseEstimated = true; + } + } + + + /**** Print pose to file ****/ + //TODO: config option + if(pmdc->savePoses) { + char filename[] = "./dat/scan0000.pose"; + sprintf(filename, "./dat/scan%04d.pose", frames); + FILE *pose = fopen(filename, "wb"); + if(!pose) fprintf(stderr, "cant create file %s!\n", filename); + + if(poseEstimated) { + fprintf(pose, "%f %f %f\n%f %f %f\n" + , 100.0*CV_MAT_ELEM(*rotGuess, float, 0, 0) + , 100.0*CV_MAT_ELEM(*rotGuess, float, 0, 0) + , 100.0*CV_MAT_ELEM(*rotGuess, float, 0, 0) + , 100.0*CV_MAT_ELEM(*trnGuess, float, 0, 0) + , 100.0*CV_MAT_ELEM(*trnGuess, float, 1, 0) + , 100.0*CV_MAT_ELEM(*trnGuess, float, 2, 0)); + goodFrames++; + } else { + fprintf( stderr, "ERROR: %i points found, align error: %f\n" + , reprojected, alignError); + fprintf(pose, "%f %f %f\n%f %f %f\n", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } + + fflush(pose); + fclose(pose); + } + frames++; + + + /***** find features *****/ + if(trPtsCnt < pmdc->_track.minFeatures) { + int trPtsCntFound = featuresMax - trPtsCnt; + cvSet(mask, cvScalarAll(255)); + for(int i = 0; i < trPtsCnt; i++) + cvCircle(mask, cvPointFrom32f(camPts[1][i]), 20, CV_RGB(0,0,0), -1, 8, 0); + cvGoodFeaturesToTrack( pmdc->iCam, eig, tmp, camPts[1] + trPtsCnt, &trPtsCntFound + , pmdc->_track.quality, pmdc->_track.minDist, mask, 3, 0, 0.04); + cvFindCornerSubPix( pmdc->iCam, camPts[1] + trPtsCnt, trPtsCntFound + , cvSize(pmdc->_track.winSz,pmdc->_track.winSz), cvSize(-1,-1) + , cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + trPtsCnt += trPtsCntFound; + } + + Frame *f = allocFrame3DData(pmdSz); + fillFrame(f, pmdc->iCamColor, pmdSz, pmdc->pts, pmd2imgIdxs, rotGuess, trnGuess, alignError); + history = addFrame(history, f); + checkHistoryLen(history, pmdc->historyLen); + + bool pause = false; + do { + /***** ui and rendring *****/ + if(gl) render( history, trackedPts, trPtsCnt + , rot, trn, camPts3D, pts3DStatus + , reprojected, poseEstimated); + if(ui) { + for(int i = 0; i < trPtsCnt; i++) + cvCircle( pmdc->iCamColor, cvPointFrom32f(camPts[1][i]) + , 3, CV_RGB(255,0,255), -1, 8, 0); + + cvShowImage("PMD", pmdc->iPMDI); + cvShowImage("Cam", pmdc->iCamColor); + } + + int key = cvWaitKey(5); + if(27 == key) return 0; // ESC pressed FIXME: release stuff + if((int)' ' == key) pause = !pause; + if(gl) if(glfwGetKey(GLFW_KEY_ESC) == GLFW_PRESS) return 0; //in OpenGL window ;) + } while(pause); + + CV_SWAP(imgCamPrv, pmdc->iCam, swapTemp); + CV_SWAP(imgCamPyrPrv, imgCamPyr, swapTemp); + CV_SWAP(camPts[0], camPts[1], swapPts); + CV_SWAP(camPts3D[0], camPts3D[1], swapPts3); + CV_SWAP(pts3DStatus[0], pts3DStatus[1], swapStatus); + + + } // while(1) + + // let OS clean all images and matrices + // TODO: releasePMD(&pmd); + if(gl) { + glfwTerminate(); + } + printf("%i good frames, %i frames total.\n", goodFrames, frames); + return 0; +} + + +// Global rendering settings +float rotx = 0; +float roty = 0; +float rotz = 0; +float scale = 0.7; +int renderCoords = 1; +int renderCams = 1; +int renderColorPts = 1; +int renderLines = 0; +int renderTracked = 1; +int renderHistory = 1; +int centerCloud = 0; + + + +void renderFrame(Frame *f) { + assert(f->img->imageData); + glBegin(GL_POINTS); + if(!renderColorPts) { + //FIXME: mess with the indices (i,j) + for(int j = 0; j < f->sz.width; j++) + for(int i = 0; i < f->sz.height; i++) + glVertex3f(f->pts[i][j].x, f->pts[i][j].y, f->pts[i][j].z); + glEnd(); + } else { + uchar *imgCamPix = 0; + for(int j = 0; j < f->sz.width; j++) + for(int i = 0; i < f->sz.height; i++) { + int x = f->status[i][j].x; + int y = f->status[i][j].y; + if(x > 0) { + imgCamPix = &((uchar*) + (f->img->imageData + f->img->widthStep * y))[x*3]; + glColor3f( ((float)imgCamPix[2])/255.0 + , ((float)imgCamPix[1])/255.0 + , ((float)imgCamPix[0])/255.0); //BGR + } else glColor3f(1.0, 0.0, 0.0); + glVertex3f(f->pts[i][j].x, f->pts[i][j].y, f->pts[i][j].z); + } + } // if renderColorPts else +} + +void render( History *history, CvPoint3D32f *camPts, int trPtsCnt + , CvMat *rot, CvMat *trn + , CvPoint3D32f **camPts3D, char **pts3DStatus + , int reprojected, bool poseEstimated) { + + if(glfwGetKey((int)'W') == GLFW_PRESS) roty += 10.0; + if(glfwGetKey((int)'S') == GLFW_PRESS) roty -= 10.0; + if(glfwGetKey((int)'A') == GLFW_PRESS) rotx -= 10.0; + if(glfwGetKey((int)'D') == GLFW_PRESS) rotx += 10.0; + if(glfwGetKey((int)'Q') == GLFW_PRESS) rotz -= 10.0; + if(glfwGetKey((int)'E') == GLFW_PRESS) rotz += 10.0; + if(glfwGetKey((int)'R') == GLFW_PRESS) scale -= 0.1; + if(glfwGetKey((int)'F') == GLFW_PRESS) scale += 0.1; + if(glfwGetKey((int)'1') == GLFW_PRESS) renderCoords = !renderCoords; + if(glfwGetKey((int)'2') == GLFW_PRESS) renderCams = !renderCams; + if(glfwGetKey((int)'3') == GLFW_PRESS) renderColorPts = !renderColorPts; + if(glfwGetKey((int)'4') == GLFW_PRESS) renderLines = !renderLines; + if(glfwGetKey((int)'5') == GLFW_PRESS) renderTracked = !renderTracked; + if(glfwGetKey((int)'6') == GLFW_PRESS) renderHistory = !renderHistory; + if(glfwGetKey((int)'C') == GLFW_PRESS) centerCloud = !centerCloud; + + int width, height; + GLUquadric *quadric; + + glfwGetWindowSize(&width, &height); + height = height < 1 ? 1 : height; + + glViewport(0, 0, width, height); + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glClearColor(0.0f, 0.0f, 0.0f, 0.0f); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(70, (double)width/(double)height, 0.01, 100.0); + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + int cj = history->frame->sz.width / 2; + int ci = history->frame->sz.height / 2; + CvPoint3D32f **pts = history->frame->pts; + // gluLookAt(scale, scale, scale, + // pts[ci][cj].x, pts[ci][cj].y, -pts[ci][cj].z, 0.0, 1.0, 0.0); + gluLookAt(scale, scale, scale, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); + + glScalef(1.0, 1.0, -1.0); // convert opengl coord system to left handed + + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LEQUAL); + + /* text rendering */ + char info[30]; + FTPoint pt(5.0, 5.0); + sprintf(info, "2D pts: %i", trPtsCnt); + font->Render(info, -1, pt); + + pt.Y(35.0); + sprintf(info, "3D pts: %i", reprojected); + font->Render(info, -1, pt); + + pt.Y(65.0); + sprintf(info, "Align error: %f", history->frame->alignError); + font->Render(info, -1, pt); + + glRotatef(rotz, 0.0, 0.0, 1.0); + glRotatef(roty, 0.0, 1.0, 0.0); + glRotatef(rotx, 1.0, 0.0, 0.0); + + if(centerCloud) glTranslatef(-pts[ci][cj].x, -pts[ci][cj].y, -pts[ci][cj].z); + + + /***** xyz axes *****/ + if(renderCoords) { + glBegin(GL_LINES); + glColor3f(1.0, 0.0, 0.0); + glVertex3f(0.0, 0.0, 0.0); + glVertex3f(1.0, 0.0, 0.0); + glColor3f(0.0, 1.0, 0.0); + glVertex3f(0.0, 0.0, 0.0); + glVertex3f(0.0, 1.0, 0.0); + glColor3f(0.0, 0.0, 1.0); + glVertex3f(0.0, 0.0, 0.0); + glVertex3f(0.0, 0.0, 1.0); + glEnd(); + } + + /***** render PMD-points PMD-cam in the O *****/ + fflush(stdout); + renderFrame(history->frame); + fflush(stdout); + + if(renderHistory) { + fflush(stdout); + History *histI = history; + glPushMatrix(); + + //for(int i = 0; histI->prev; i++) { + while(histI->prev) { + glColor3f(1.0, 0.0, 0.0);//, (100.0-i)/100); + assert(histI); + + //if(histI->frame->alignError < 0.03) { //FIXME_ hardcoded thrs + glRotatef(-CV_MAT_ELEM(*histI->frame->rot, float, 0, 0), 1.0f, 0.0f, 0.0f); + glRotatef(-CV_MAT_ELEM(*histI->frame->rot, float, 1, 0), 0.0f, 1.0f, 0.0f); + glRotatef(-CV_MAT_ELEM(*histI->frame->rot, float, 2, 0), 0.0f, 0.0f, 1.0f); + glTranslatef( -CV_MAT_ELEM(*histI->frame->trn, float, 0, 0) + , -CV_MAT_ELEM(*histI->frame->trn, float, 1, 0) + , -CV_MAT_ELEM(*histI->frame->trn, float, 2, 0)); + //} + histI = histI->prev; + renderFrame(histI->frame); + } + glPopMatrix(); + } + + quadric = gluNewQuadric(); + + + if(renderCams) { + glColor3f(0.5, 0.5, 0.0); + glPushMatrix(); + glScalef(1.0, 1.0, -1.0); // rotated cylinder + gluCylinder(quadric, 0.05, 0.0, 0.05, 10, 10); + glPopMatrix(); + } + + /***** render Cam and features according to essential matrix [R|t] *****/ + if(renderTracked) { + glBegin(GL_LINES); + for(int i = 0; i < trPtsCnt; i++) { + if(pts3DStatus[1][i] && + pts3DStatus[0][i]) { + glColor3f(1.0, 0.0, 0.0); + glVertex3f(camPts3D[0][i].x, camPts3D[0][i].y, camPts3D[0][i].z); + glVertex3f(camPts3D[1][i].x, camPts3D[1][i].y, camPts3D[1][i].z); + glColor3f(0.0, 0.0, 0.5); + glPushMatrix(); + glTranslatef(camPts3D[0][i].x, camPts3D[0][i].y, camPts3D[0][i].z); + gluSphere(quadric, 0.005f, 10, 10); + glPopMatrix(); + glColor3f(0.5, 0.0, 1.0); + glPushMatrix(); + glTranslatef(camPts3D[1][i].x, camPts3D[1][i].y, camPts3D[1][i].z); + gluSphere(quadric, 0.01f, 10, 10); + glPopMatrix(); + } + } + glEnd(); + } + glBegin(GL_LINES); + for(int i = 0; i < trPtsCnt; i++) + if(pts3DStatus[1][i] && + pts3DStatus[0][i]) { + glColor3f(1.0, 0.0, 0.0); + glVertex3f(camPts3D[0][i].x, camPts3D[0][i].y, camPts3D[0][i].z); + glVertex3f(camPts3D[1][i].x, camPts3D[1][i].y, camPts3D[1][i].z); + } + glEnd(); + + + glPushMatrix(); + glTranslatef( -CV_MAT_ELEM(*trn, float, 0, 0) + , -CV_MAT_ELEM(*trn, float, 1, 0) + , -CV_MAT_ELEM(*trn, float, 2, 0)); + glRotatef(-CV_MAT_ELEM(*rot, float, 0, 0), 1.0f, 0.0f, 0.0f); + glRotatef(-CV_MAT_ELEM(*rot, float, 1, 0), 0.0f, 1.0f, 0.0f); + glRotatef(-CV_MAT_ELEM(*rot, float, 2, 0), 0.0f, 0.0f, 1.0f); + if(renderCams) { + glColor3f(0.0, 0.5, 0.5); + glPushMatrix(); + glScalef(1.0, 1.0, -1.0); + gluCylinder(quadric, 0.05, 0.0, 0.05, 10, 10); + glPopMatrix(); + } + + if(renderLines) { + glColor3f(1.0, 0.0, 1.0); + glBegin(GL_LINES); + for(int i = 0; i < trPtsCnt; i++) { + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(-camPts[i].x, camPts[i].y, camPts[i].z); //FIXME: revise coordinates! why -camPts?! + } + glEnd(); + } + glPopMatrix(); + + glPushMatrix(); + glRotatef(CV_MAT_ELEM(*history->frame->rot, float, 2, 0), 0.0f, 0.0f, 1.0f); + glRotatef(CV_MAT_ELEM(*history->frame->rot, float, 1, 0), 0.0f, 1.0f, 0.0f); + glRotatef(CV_MAT_ELEM(*history->frame->rot, float, 0, 0), 1.0f, 0.0f, 0.0f); + glTranslatef( CV_MAT_ELEM(*history->frame->trn, float, 0, 0) + , CV_MAT_ELEM(*history->frame->trn, float, 1, 0) + , CV_MAT_ELEM(*history->frame->trn, float, 2, 0)); + if(poseEstimated) glColor3f(0.0, 0.0, 1.0); + else glColor3f(0.0, 0.0, 0.3); + //render pose and pmd cam + glScalef(1.0, 1.0, -1.0); + gluCylinder(quadric, 0.05, 0.0, 0.05, 10, 10); + glColor4f(1.0, 0.0, 0.0, 0.5); + gluSphere(quadric, history->frame->alignError, 20, 20); + glPopMatrix(); + + gluDeleteQuadric(quadric); + glfwSwapBuffers(); +} + diff --git a/.svn/pristine/7a/7ae14d3810c563366f49f514547bb42f2b39c4c0.svn-base b/.svn/pristine/7a/7ae14d3810c563366f49f514547bb42f2b39c4c0.svn-base new file mode 100644 index 0000000..413878e --- /dev/null +++ b/.svn/pristine/7a/7ae14d3810c563366f49f514547bb42f2b39c4c0.svn-base @@ -0,0 +1,93 @@ +/* + * extrinsic implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + + +int main(int argc, char **argv) { + + if(argc < 3) { + printf("extrinic: x y img1 img2\n"); + exit(1); + } + // extrinic params: rotation and translation + CvMat *rotCam = cvCreateMat(1, 3, CV_32FC1); + CvMat *transCam = cvCreateMat(1, 3, CV_32FC1); + CvMat *rotPMD = cvCreateMat(1, 3, CV_32FC1); + CvMat *transPMD = cvCreateMat(1, 3, CV_32FC1); + + CvSize boardSz = cvSize(atoi(argv[1]), atoi(argv[2])); + int totalPoints = boardSz.width*boardSz.height; + double boardSide = 0.04; + CvMat *objPts = cvCreateMat(totalPoints, 3, CV_32FC1); + CvPoint2D32f *cornersCam = (CvPoint2D32f*)cvAlloc(totalPoints * sizeof(CvPoint2D32f)); + CvMat imgPtsCam = cvMat(totalPoints, 1, CV_32FC2, cornersCam); + CvPoint2D32f *cornersPMD = (CvPoint2D32f*)cvAlloc(totalPoints * sizeof(CvPoint2D32f)); + CvMat imgPtsPMD = cvMat(totalPoints, 1, CV_32FC2, cornersPMD); + + for(int i = 0; i < totalPoints; i++) { + CV_MAT_ELEM(*objPts, float, i, 0) = boardSide * (i / boardSz.width); + CV_MAT_ELEM(*objPts, float, i, 1) = boardSide * (i % boardSz.width); + CV_MAT_ELEM(*objPts, float, i, 2) = 0.0f; + } + + IplImage *imgPMD = cvLoadImage(argv[3], CV_LOAD_IMAGE_GRAYSCALE); + IplImage *imgPMDU = cvCreateImage(cvGetSize(imgPMD), 8, 1); + IplImage *imgCam = cvLoadImage(argv[4], CV_LOAD_IMAGE_GRAYSCALE); + IplImage *imgCamU = cvCreateImage(cvGetSize(imgCam), 8, 1); + + CvMat *intrinsicsCam = (CvMat*)cvLoad("../intrinsic-cam-6x4.xml"); + CvMat *intrinsicsPMD = (CvMat*)cvLoad("../intrinsic-pmd-6x4.xml"); + CvMat *distortionCam = (CvMat*)cvLoad("../distortion-cam-6x4.xml"); + CvMat *distortionPMD = (CvMat*)cvLoad("../distortion-pmd-6x4.xml"); + + cvUndistort2(imgPMD, imgPMDU, intrinsicsPMD, distortionPMD); + cvUndistort2(imgCam, imgCamU, intrinsicsCam, distortionCam); + + int cornersCountCam, cornersCountPMD; + int foundPMD = cvFindChessboardCorners(imgPMDU, boardSz, cornersPMD, + &cornersCountPMD, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + if(foundPMD) { + cvFindCornerSubPix(imgPMDU, cornersPMD, cornersCountPMD, cvSize(2, 2), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + printf("foundPMD\n"); + } + int foundCam = cvFindChessboardCorners(imgCamU, boardSz, cornersCam, + &cornersCountCam, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + if(foundPMD) { + cvFindCornerSubPix(imgCamU, cornersCam, cornersCountCam, cvSize(11, 11), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + printf("foundCam\n"); + } + + cvNamedWindow("c", 0); + cvNamedWindow("p", 0); + + cvShowImage("c", imgCamU); + cvShowImage("p", imgPMDU); + cvWaitKey(0); + + + if(foundCam) cvFindExtrinsicCameraParams2(objPts, &imgPtsCam, intrinsicsCam, distortionCam, rotCam, transCam); + if(foundPMD) cvFindExtrinsicCameraParams2(objPts, &imgPtsPMD, intrinsicsPMD, distortionPMD, rotPMD, transPMD); + + cvSave("./rotcam.xml", rotCam); + cvSave("./rotpmd.xml", rotPMD); + cvSave("./transcam.xml", transCam); + cvSave("./transpmd.xml", transPMD); +} diff --git a/.svn/pristine/7d/7d6d4ee3aa05f62b4482f203bddf3b35e1c050a6.svn-base b/.svn/pristine/7d/7d6d4ee3aa05f62b4482f203bddf3b35e1c050a6.svn-base new file mode 100644 index 0000000..cf30654 --- /dev/null +++ b/.svn/pristine/7d/7d6d4ee3aa05f62b4482f203bddf3b35e1c050a6.svn-base @@ -0,0 +1,81 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "cvblob.h" + +namespace cvb +{ + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=geometry1 + + double cvDotProductPoints(CvPoint const &a, CvPoint const &b, CvPoint const &c) + { + double abx = b.x-a.x; + double aby = b.y-a.y; + double bcx = c.x-b.x; + double bcy = c.y-b.y; + + return abx*bcx + aby*bcy; + } + + double cvCrossProductPoints(CvPoint const &a, CvPoint const &b, CvPoint const &c) + { + double abx = b.x-a.x; + double aby = b.y-a.y; + double acx = c.x-a.x; + double acy = c.y-a.y; + + return abx*acy - aby*acx; + } + + double cvDistancePointPoint(CvPoint const &a, CvPoint const &b) + { + double abx = a.x-b.x; + double aby = a.y-b.y; + + return sqrt(abx*abx + aby*aby); + } + + double cvDistanceLinePoint(CvPoint const &a, CvPoint const &b, CvPoint const &c, bool isSegment) + { + if (isSegment) + { + double dot1 = cvDotProductPoints(a, b, c); + if (dot1>0) return cvDistancePointPoint(b, c); + + double dot2 = cvDotProductPoints(b, a, c); + if(dot2>0) return cvDistancePointPoint(a, c); + } + + return fabs(cvCrossProductPoints(a,b,c)/cvDistancePointPoint(a,b)); + } + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +} diff --git a/.svn/pristine/83/830ae6600453a26492b31d814e7f1c156975fa13.svn-base b/.svn/pristine/83/830ae6600453a26492b31d814e7f1c156975fa13.svn-base new file mode 100644 index 0000000..8561a85 --- /dev/null +++ b/.svn/pristine/83/830ae6600453a26492b31d814e7f1c156975fa13.svn-base @@ -0,0 +1,775 @@ +/* + * panorama implementation + * + * Copyright (C) HamidReza Houshiar + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/fbr/panorama.h" + +using namespace std; + +namespace fbr{ + + void panorama::init(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){ + iWidth = width; + iHeight = height; + pMethod = method; + nImages = numberOfImages; + pParam = param; + if(mMethod == FARTHEST){ + iMap.create(iHeight, iWidth, CV_32FC(3)); + iMap = cv::Scalar::all(0); + } + else if(mMethod == EXTENDED){ + extendedIMap.resize(iHeight); + for (unsigned int i = 0; i < iHeight; i++) + extendedIMap[i].resize(iWidth); + } + iReflectance.create(iHeight, iWidth, CV_8U); + iReflectance = cv::Scalar::all(0); + iRange.create(iHeight, iWidth, CV_32FC(1)); + iRange = cv::Scalar::all(0); + mapMethod = mMethod; + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){ + init(width, height, method, numberOfImages, param, mMethod); + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param){ + init(width, height, method, numberOfImages, param, FARTHEST); + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages){ + double param = 0; + if(method == PANNINI) + param = 1; + else if (method == STEREOGRAPHIC) + param = 2; + + init(width, height, method, numberOfImages, param, FARTHEST); + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method){ + double param = 0; + unsigned int numberOfImages = 1; + if(method == RECTILINEAR) + numberOfImages = 3; + else if(method == PANNINI){ + numberOfImages = 3; + param = 1; + } else if (method == STEREOGRAPHIC){ + numberOfImages = 3; + param = 2; + } + + init(width, height, method, numberOfImages, param, FARTHEST); + } + + void panorama::map(int x, int y, cv::MatIterator_ it, double range){ + iReflectance.at(y,x) = (*it)[3]*255;//reflectance + iRange.at(y,x) = range;//range + if(mapMethod == FARTHEST){ + //adding the point with max distance + if( iRange.at(y,x) < range ){ + iMap.at(y,x)[0] = (*it)[0];//x + iMap.at(y,x)[1] = (*it)[1];//y + iMap.at(y,x)[2] = (*it)[2];//z + } + }else if(mapMethod == EXTENDED){ + //adding all the points + cv::Vec3f point; + point[0] = (*it)[0];//x + point[1] = (*it)[1];//y + point[2] = (*it)[2];//z + extendedIMap[y][x].push_back(point); + } + } + + void panorama::createPanorama(cv::Mat scan){ + + //EQUIRECTANGULAR projection + if(pMethod == EQUIRECTANGULAR){ + //adding the longitude to x axis and latitude to y axis + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + double yFactor = (double) iHeight / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI); + //shift all the valuse to positive points on image + double heightLow =(0 - MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + int y = (int) ( yFactor * (theta + heightLow) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //CONIC projection + if(pMethod == CONIC){ + // set up maximum latitude and longitude angles of the robot + double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0, + MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI; + // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html + double Lat0 = 0., Long0 = 0.; + double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0; + double n = (sin(Phi1) + sin(Phi2)) / 2.; + double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1); + double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n; + // set up max values for x and y and add the longitude to x axis and latitude to y axis + double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0)); + double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0)); + double xFactor = (double) iWidth / ( xmax - xmin ); + int widthMax = iWidth - 1; + double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 )); + double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 )); + double yFactor = (double) iHeight / ( ymax - ymin ); + //shift all the values to positive points on image + int heightMax = iHeight - 1; + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //phi == longitude == horizantal angle of view of [0:360] + phi = 180.0 - phi; + phi *= M_PI / 180.0; + //theta == latitude == vertical angle of view of [-40:60] + theta -= 90; + theta *= -1; + theta *= M_PI / 180.0; + + // add minimum x position as an offset + int x = (int) ( xFactor * (sqrt(C - 2 * n * sin( theta) ) / n * sin(n * (phi - Long0)) + fabs(xmin) ) ); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + + // add minimum y position as an offset + int y = (int) ( yFactor * (Rho0 - (1/n * sqrt(C - 2 * n * sin( theta) ) ) * cos(n * (phi - Long0)) + fabs( ymin ) ) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //CYLINDRICAL projection + if(pMethod == CYLINDRICAL){ + //adding the longitude to x and tan(latitude) to y + //find the x and y range + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + double yFactor = (double) iHeight / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI)); + double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + int y = (int) ((double) yFactor * (tan(theta) - tan(heightLow))); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //Mercator Projection + if( pMethod == MERCATOR){ + //find the x and y range + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + double yFactor = (double) iHeight / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) ); + double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI))); + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + int y = (int) ( yFactor * (log(tan(theta) + (1/cos(theta))) - heightLow) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //RECTILINEAR projection + if(pMethod == RECTILINEAR){ + //default value for nImages + if(nImages == 0) nImages = 3; + cout<<"Number of images per scan is: "< it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + for(unsigned int j = 0 ; j < nImages ; j++){ + iMinx = j * interval; + iMaxx = (j + 1) * interval; + //check for point in interval + if(phi < iMaxx && phi > iMinx){ + double max, min, coscRectilinear; + //the longitude of projection center + l0 = iMinx + interval / 2; + //finding the min and max of the x direction + coscRectilinear = sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0); + max = (cos(iMaxy) * sin(iMaxx - l0) / coscRectilinear); + coscRectilinear = sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0); + min = (cos(iMiny) * sin(iMinx - l0) / coscRectilinear); + double xFactor = (double) (iWidth / nImages) / (max - min); + double xlow = min; + int widthMax = (iWidth / nImages) - 1; + //finding the min and max of y direction + coscRectilinear = sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0); + max = ( (cos(p1) * sin(iMaxy) - sin(p1) * cos(iMaxy) * cos(iMaxx - l0) )/ coscRectilinear); + coscRectilinear = sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0); + min = ( (cos(p1) * sin(iMiny) - sin(p1) * cos(iMiny) * cos(iMinx - l0) )/ coscRectilinear); + double yFactor = (double) iHeight / (max - min); + double heightLow = min; + int heightMax = iHeight - 1; + //project the points and add them to image + coscRectilinear = sin(p1) * sin(theta) + cos(p1) * cos(theta) * cos(phi - l0); + int x = (int)(xFactor) * ((cos(theta) * sin(phi - l0) / coscRectilinear) - xlow); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + x = x + (j * iWidth / nImages); + int y = (int) (yFactor) * (( (cos(p1) * sin(theta) - sin(p1) * cos(theta) * cos(phi - l0)) / coscRectilinear) - heightLow); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + } + + //PANNINI projection + if(pMethod == PANNINI){ + //default values for nImages and dPannini==pParam + if(pParam == 0) pParam = 1; + if(nImages == 0) nImages = 3; + cout << "Parameter d is:" << pParam <<", Number of images per scan is:" << nImages << endl; + double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval; + interval = 2 * M_PI / nImages; + iMiny = -M_PI/9; + iMaxy = 2*M_PI/9; + //latitude of projection center + p1 = 0; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + for(unsigned int j = 0 ; j < nImages ; j++){ + iMinx = j * interval; + iMaxx = (j + 1) * interval; + //check for point in interval + if(phi < (iMaxx) && phi > (iMinx)){ + double max, min, sPannini; + //the longitude of projection center + l0 = iMinx + interval / 2; + //use the S variable of pannini projection mentioned in the thesis + //finding the min and max of the x direction + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMaxy) + cos(p1) * cos(iMaxx - l0)); + max = sPannini * (sin(iMaxx - l0)); + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMiny) + cos(p1) * cos(iMinx - l0)); + min = sPannini * (sin(iMinx - l0)); + double xFactor = (double) (iWidth / nImages) / (max - min); + double xlow = min; + int widthMax = (iWidth / nImages) - 1; + //finding the min and max of y direction + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMaxy) + cos(p1) * cos(iMaxx - l0)); + max = sPannini * (tan(iMaxy) * (cos(p1) - sin(p1) * 1/tan(iMaxy) * cos(iMaxx - l0))); + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMiny) + cos(p1) * cos(iMinx - l0)); + min = sPannini * (tan(iMiny) * (cos(p1) - sin(p1) * 1/tan(iMiny) * cos(iMinx - l0))); + double yFactor = (double) iHeight / (max - min); + double heightLow = min; + int heightMax = iHeight - 1; + //project the points and add them to image + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(theta) + cos(p1) * cos(phi - l0)); + int x = (int)(xFactor) * (sPannini * sin(phi - l0) - xlow); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + x = x + (j * iWidth / nImages); + int y = (int) (yFactor) * ( (sPannini * tan(theta) * (cos(p1) - sin(p1) * (1/tan(theta)) * cos(phi - l0) ) ) - heightLow ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + } + + //STEREOGRAPHIC projection + if(pMethod == STEREOGRAPHIC){ + //default values for nImages and rStereographic==pParam + if(pParam == 0) pParam = 2; + if(nImages == 0) nImages = 3; + cout << "Paremeter R is:" << pParam << ", Number of images per scan is:" << nImages << endl; + // l0 and p1 are the center of projection iminx, imaxx, iminy, imaxy are the bounderis of intervals + double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval; + interval = 2 * M_PI / nImages; + iMiny = -M_PI/9; + iMaxy = 2*M_PI/9; + //latitude of projection center + p1 = 0; + + //go through all points + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + for (unsigned int j = 0 ; j < nImages ; j++){ + iMinx = j * interval; + iMaxx = (j + 1) * interval; + //check for point in intervals + if(phi < (iMaxx) && phi > (iMinx)){ + double max, min, k; + //longitude of projection center + l0 = iMinx + interval / 2; + //use the R variable of stereographic projection mentioned in the thesis + //finding the min and max of x direction + k = (2 * pParam) / (1 + sin(p1) * sin(p1) + cos(p1) * cos(p1) * cos(iMaxx - l0)); + max = k * cos(p1) * sin (iMaxx - l0); + k = (2 * pParam) / (1 + sin (p1) * sin(p1) + cos(p1) * cos(p1) * cos(iMinx -l0)); + min = k * cos(p1) * sin (iMinx -l0); + double xFactor = (double) (iWidth / nImages) / (max - min); + double xlow = min; + int widthMax = (iWidth / nImages) - 1; + //finding the min and max of y direction + k = (2 * pParam) / (1 + sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0)); + max = k * (cos(p1) * sin(iMaxy) - sin(p1) * cos(iMaxy) * cos(iMaxx - l0)); + k = (2 * pParam) / (1 + sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0)); + min = k * (cos(p1) * sin(iMiny) - sin(p1) * cos(iMiny) * cos(iMinx - l0)); + double yFactor = (double) iHeight / (max - min); + double heightLow = min; + int heightMax = iHeight - 1; + //project the points and add them to image + k = (2 * pParam) / (1 + sin(p1) * sin(theta) + cos(p1) * cos(theta) * cos(phi - l0)); + int x = (int) (xFactor) * (k * cos(theta) * sin(phi - l0) - xlow); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + x = x + (j * iWidth / nImages); + int y = (int) (yFactor) * (k * ( cos(p1) * sin(theta) - sin(p1) * cos(theta) * cos(phi - l0) ) - heightLow); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + } + + //ZAXIS projection + if(pMethod == ZAXIS){ + double zmin = -200; + double zmax = 4000; + //adding the longitude to x axis and latitude to y axis + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + cout << "ZMAX= " << zmax << " ZMIN= "<< zmin << endl; + double yFactor = (double) iHeight / (zmax - zmin); + //shift all the valuse to positive points on image + double heightLow = zmin; + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + ///////////////////check this + int y = (int) ( yFactor * ((*it)[1] - heightLow) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + + void panorama::recoverPointCloud(const cv::Mat& range_image, + cv::Mat& reflectance_image, vector &reduced_points) { + if (range_image.cols != reflectance_image.cols + || range_image.rows != reflectance_image.rows) { + cerr << "range image and reflectance image have different geometries - using empty range image" << endl; + reflectance_image.create(range_image.size(), CV_8U); + reflectance_image = cv::Scalar::all(0); + } + + //recover from EQUIRECTANGULAR projection + if(pMethod == EQUIRECTANGULAR) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + //int widthMax = range_image.size().width - 1; + double yFactor = (double) range_image.size().height / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI); + double heightLow = (0 - MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; + float theta = (heightMax - row) / yFactor - heightLow; + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); + } + } + } + + //recover from CYLINDRICAL projection + if(pMethod == CYLINDRICAL) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + //int widthMax = range_image.size().width - 1; + double yFactor = (double) range_image.size().height / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI)); + double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI; + //int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; + float theta = atan2(row + yFactor * tan(heightLow), yFactor); + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); + } + } + } + + //recover from MERCATOR projection + if(pMethod == MERCATOR) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + double yFactor = (double) range_image.size().height / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) ); + double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI))); + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; + float theta = 2 * atan2(exp((heightMax - row) / yFactor + heightLow), 1.) - M_PI_2; + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 180.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); + } + } + } + + //recover from CONIC projection + if(pMethod == CONIC) { + // set up maximum latitude and longitude angles of the robot + double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0, + MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI; + // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html + double Lat0 = 0., Long0 = 0.; + double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0; + double n = (sin(Phi1) + sin(Phi2)) / 2.; + double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1); + double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n; + // set up max values for x and y and add the longitude to x axis and latitude to y axis + double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0)); + double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0)); + double xFactor = (double) range_image.size().width / ( xmax - xmin ); + double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 )); + double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 )); + double yFactor = (double) range_image.size().height / ( ymax - ymin ); + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; + float x = col * 1. / xFactor - fabs(xmin); + float y = (heightMax - row) * 1. / yFactor - fabs(ymin); + float theta = asin((C - (x*x + (Rho0 - y) * (Rho0 - y)) * n * n) / (2 * n)); + float phi = Long0 + (1./n) * ::atan2(x, Rho0 - y); + + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + //if ( std::isnan(cartesian[0]) || std::isnan(cartesian[1]) || std::isnan(cartesian[2]) ) continue; + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); + } + } + } + } + + unsigned int panorama::getImageWidth(){ + return iWidth; + } + + unsigned int panorama::getImageHeight(){ + return iHeight; + } + + projection_method panorama::getProjectionMethod(){ + return pMethod; + } + + unsigned int panorama::getNumberOfImages(){ + return nImages; + } + + double panorama::getProjectionParam(){ + return pParam; + } + + cv::Mat panorama::getReflectanceImage(){ + return iReflectance; + } + + cv::Mat panorama::getMap(){ + return iMap; + } + + cv::Mat panorama::getRangeImage(){ + return iRange; + } + + vector > > panorama::getExtendedMap(){ + return extendedIMap; + } + + panorama_map_method panorama::getMapMethod(){ + return mapMethod; + } + + void panorama::getDescription(){ + cout << "panorama created with width: " << iWidth << ", and height: " + << iHeight << ", and projection method: " << projectionMethodToString(pMethod) + << ", number of images: " << nImages << ", projection param: " << pParam << "." + << endl; + cout << endl; + } +} + diff --git a/.svn/pristine/a6/a6e2af71c156e39fa800c635778f62716795e538.svn-base b/.svn/pristine/a6/a6e2af71c156e39fa800c635778f62716795e538.svn-base new file mode 100644 index 0000000..26f2e55 --- /dev/null +++ b/.svn/pristine/a6/a6e2af71c156e39fa800c635778f62716795e538.svn-base @@ -0,0 +1,1065 @@ +/* + * show_common implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#ifdef WITH_GLEE +#include +#endif + +#include "show/show.h" +#include "show/show_Boctree.h" +#include "show/compacttree.h" +#include "show/NurbsPath.h" +#include "show/vertexarray.h" +#ifndef DYNAMIC_OBJECT_REMOVAL +#include "slam6d/scan.h" +#include "slam6d/managedScan.h" +#else +#include "veloslam/veloscan.h" +#endif +#include "glui/glui.h" /* Header File For The glui functions */ +#include +using std::ifstream; +#include +using std::exception; +#include + + +#ifdef _MSC_VER +#include "XGetopt.h" +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#include "slam6d/point_type.h" +#include "slam6d/io_utils.h" +#include "show/display.h" + +/** + * This vector contains the pointer to a vertex array for + * all colors (inner vector) and all scans (outer vector) + */ +vector< vector > vvertexArrayList; + +vector< ::SDisplay*> displays; +/** + * the octrees that store the points for each scan + */ +//Show_BOctTree **octpts; +vector octpts; +/** + * Storing the base directory + */ +string scan_dir; + +/** + * Storing the ID of the main windows + */ +int window_id; + +/** + * Size of points + */ +GLfloat pointsize = 1.0; +int anim_delay = 5; + +/** + * Select Color Buffer + */ +GLenum buffermode = GL_BACK; + +/** + * Indicator whether and how the drawing window + * has to be updated. + * + * haveToUpdate == 1 redisplay + * haveToUpdate == 2 reshape + * haveToUpdate == 3 animation scan matching + * haveToUpdate == 4 stop animation scan matching + * haveToUpdate == 6 path animation + * haveToUpdate == 7 force redisplay with all points + */ +int haveToUpdate = 0; + +/** + * Flag for invert the scene + */ +bool invert = true; + +/** + * Flag for indicating brid eyes view + */ +bool showTopView = false; + +/** + * Flag for idicating camera add mode + */ +bool addCameraView = false; //Is the view in add box mode? + +/** + * Storing the apex angle of the camera + */ +GLfloat cangle = 60.0; // Current camera opening mode +GLfloat cangle_old = cangle; + +/** + * Current rotation axis of the scene as quaternion + */ +GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; +GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; + +/** + * Current translation of the scene + */ +GLdouble X = 0.0, Y = 0.0, Z = 0.0; +GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; + +/** + * Center of Mass coordinates + */ +GLdouble CoM[3] = { 0.0, 0.0, 0.0 }; + +/** + * parallel zoom (similar to apex angle) for parallel projection + */ +GLfloat pzoom = 2000.0; +GLfloat pzoom_old = pzoom; + + +/** + * Mode of the fog (exp, exp2, linear) + */ +GLint fogMode = GL_EXP; + +/** + * Indicates if fog should be shown + */ +int show_fog = 1; + +/** + * Indicates if the points should be shown + */ +int show_points = 1; // Show data points in the viewer? + +/** + * Indicates if camera boxes should be shown + */ +int show_cameras = 1; // Show the camera boxes in the viewer? + +/** + * Indicates if camera path or robot path should be shown + */ +int show_path = 1; // Show the camera movement path ? + +/** + * Camera navigation by mouse or by panel + */ +int cameraNavMouseMode = 1; + +int mouseNavX, mouseNavY; +int mouseNavButton = -1; + +double mouseRotX = 0.0; +double mouseRotY = 0.0; +double mouseRotZ = 0.0; + +bool keymap[256]; + +//@@@ +//int animate_both = 0; // Animate both scan matchin and path? + +int frameNr = 0; + +/** + * Storing of all transformation (frames for animation) of all scans + */ +vector < vector > MetaMatrix; + +/** + * Storing of AlgoType for all frames + */ +vector < vector > MetaAlgoType; + +/** + * Window position + */ +int START_X = 0; +int START_Y = 0; +int START_WIDTH = 960; +int START_HEIGHT = 540; +GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio +bool advanced_controls = false; + +bool fullscreen = false; +int current_width = START_WIDTH; +int current_height = START_HEIGHT; + + +// the following values are scale dependant, i.e. all values are in m +float neardistance = 0.10; +double oldneardistance = 0.10; +float maxfardistance = 400.0; +double fardistance = 400.0; +double oldfardistance = 40000.0; +double movementSpeed = 0.1; +double defaultZoom = 20.0; +GLfloat fogDensity = 0.1; +double voxelSize = 0.20; + + +float adaption_rate = 1.0; +float LevelOfDetail = 0.0001; + + +// Defines for Point Semantic +#define TYPE_UNKNOWN 0x0000 +#define TYPE_OBJECT 0x0001 +#define TYPE_GROUND 0x0002 +#define TYPE_CEILING 0x0003 + +unsigned int cam_choice = 0; + +static unsigned int path_iterator = 0; +static int oldcamNavMode = 0; + +/** + * Animation sould be saved to file + */ +int save_animation = 0; +/** + * If true, interpolation for camera path is based on distance, else always + * the same number of images between two cameras. + */ +int inter_by_dist = 1; + +/**some variables for the camera path**/ +vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; +vector cams; +vector lookats; +vector ups; + +NurbsPath cam_nurbs_path; +char *path_file_name; +char *pose_file_name; + +/** Factor for saved image size */ +int factor = 1; + +/** + * program tries to have this framerate + */ +float idealfps = 20.0; +/** + * value of the listBox fo Color Value and Colormap + */ +int listboxColorVal = 0; +int listboxColorMapVal = 0; +int colorScanVal = 0; +ScanColorManager *cm; +float mincolor_value = 0.0; +float maxcolor_value = 0.0; +//unsigned int types = Point::USE_HEIGHT; +PointType pointtype; + +/** + * Contains the selected points for each scan + */ +set *selected_points; +/** + * Select single points? + */ +int select_voxels = 0; +/** + * Select or unselect points ? + */ +int selectOrunselect = 1; +/** octree depth for selecting groups of points */ +int selection_depth = 1; +int brush_size = 0; +char *selection_file_name; + +int current_frame = 0; +#include "show_menu.cc" +#include "show_animate.cc" +#include "show_gl.cc" + +/** + * Explains the usage of this program's command line parameters + * @param prog name of the program + */ +void usage(char* prog) +{ +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl + << " will attempt to display points with a framerate of NR" << endl + << endl + << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << + "FILE" << endl + << " load objects specified in " << endl + << endl + << endl + << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl + << " neglegt all data points with a distance larger than NR 'units'" << endl + << endl + << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl + << " neglegt all data points with a distance smaller than NR 'units'" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl + << " sets the starting and reset position to: " << endl + << " 0 = the origin of the coordinate system (default)" << endl + << " 1 = the position of the first scan (default if --origin is in argument list)" << endl + << " 2 = the center of the first scan" << endl + << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -C" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl + << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl + << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl + << " " << endl + << endl + + << bold << " -R, --reflectance, --reflectivity" << normal << endl + << " use reflectivity values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -D, --temperature, --degree" << normal << endl + << " use temperature values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -a, --amplitude" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -d, --deviation" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -h, --height" << endl << normal + << " use y-values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -T, --type" << endl << normal + << " use type values for coloring point clouds" << endl + << " only works when using octree display" << endl + << bold << " -c, --color" << endl << normal + << " use color RGB values for coloring point clouds" << endl + << bold << " -b" << normal << " NR, " << bold << "--sphere=" << normal << "NR" << endl + << " map all measurements on a sphere (of radius NRcm)" << endl + << bold << " --saveOct" << endl << normal + << " stores all used scans as octrees in the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl + << " only works when using octree display" << endl + << bold << " --loadOct" << endl << normal + << " only reads octrees from the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are read from file." << endl + << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl + << " only works when using octree display" << endl + << endl << endl; + + exit(1); +} + +/** + * A function that parses the command-line arguments and sets the respective flags. + * + * @param argc the number of arguments + * @param argv the arguments + * @param dir parsing result - the directory + * @param start parsing result - starting at scan number 'start' + * @param end parsing result - stopping at scan number 'end' + * @param maxDist parsing result - maximal distance + * @param minDist parsing result - minimal distance + * @param readInitial parsing result - read a file containing a initial transformation matrix + * @param type parsing result - file format to be read + * @return 0, if the parsing was successful, 1 otherwise + */ +int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, + double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, + bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver, + double& sphereMode) +{ + unsigned int types = PointType::USE_NONE; + start = 0; + end = -1; // -1 indicates no limitation + maxDist = -1; // -1 indicates no limitation + int c; + // from unistd.h + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + cout << endl; + static struct option longopts[] = { + { "origin", optional_argument, 0, 'o' }, + { "format", required_argument, 0, 'f' }, + { "fps", required_argument, 0, 'F' }, + { "scale", required_argument, 0, 'C' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "octree", optional_argument, 0, 'O' }, + { "reflectance", no_argument, 0, 'R' }, + { "reflectivity", no_argument, 0, 'R' }, + { "temperature", no_argument, 0, 'D' }, + { "degree", no_argument, 0, 'D' }, + { "amplitude", no_argument, 0, 'a' }, + { "deviation", no_argument, 0, 'd' }, + { "height", no_argument, 0, 'h' }, + { "type", no_argument, 0, 'T' }, + { "color", no_argument, 0, 'c' }, + { "loadObj", required_argument, 0, 'l' }, + { "saveOct", no_argument, 0, '0' }, + { "loadOct", no_argument, 0, '1' }, + { "advanced", no_argument, 0, '2' }, + { "scanserver", no_argument, 0, 'S' }, + { "sphere", required_argument, 0, 'b' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRDadhTcb", longopts, NULL)) != -1) { + switch (c) { + case 's': + w_start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 't': + readInitial = true; + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case '?': + usage(argv[0]); + return 1; + case 'R': + types |= PointType::USE_REFLECTANCE; + break; + case 'D': + types |= PointType::USE_TEMPERATURE; + break; + case 'a': + types |= PointType::USE_AMPLITUDE; + break; + case 'd': + types |= PointType::USE_DEVIATION; + break; + case 'h': + types |= PointType::USE_HEIGHT; + break; + case 'T': + types |= PointType::USE_TYPE; + break; + case 'c': + types |= PointType::USE_COLOR; + break; + case 'F': + fps = atof(optarg); + break; + case 'C': + scale = atof(optarg); + break; + case 'S': + scanserver = true; + break; + case 'o': + if (optarg) { + origin = atoi(optarg); + } else { + origin = 1; + } + break; + case '0': + saveOct = true; + break; + case '1': + loadOct = true; + break; + case 'l': + loadObj = optarg; + break; + case '2': + advanced_controls = true; + break; + case 'b': + sphereMode = atof(optarg); + break; + default: + abort (); + } + } + + if (optind != argc-1) { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + parseFormatFile(dir, w_type, w_start, w_end); + + ptype = PointType(types); + return 0; +} + +void setResetView(int origin) { + if (origin == 1) { + // set origin to the pose of the first scan + double *transmat = MetaMatrix[0].back(); + cout << transmat << endl; + + RVX = -transmat[12]; + RVY = -transmat[13]; + RVZ = -transmat[14]; + Matrix4ToQuat(transmat, Rquat); + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + } else if (origin == 2) { + // set origin to the center of the first octree + double center[3], center_transformed[3]; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[0])->getCenter(center); +#else + ((Show_BOctTree*)octpts[0])->getCenter(center); +#endif + VTrans(MetaMatrix[0].back(), center, center_transformed); + RVX = -center_transformed[0]; + RVY = -center_transformed[1]; + RVZ = -center_transformed[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } else if (origin == 3) { + // set origin to the center of mass of all scans + for (size_t i = 0; i < octpts.size(); ++i) { + vector points; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[i])->AllPoints( points ); +#else + BOctTree* cur_tree = ((Show_BOctTree*)octpts[i])->getTree(); + cur_tree->AllPoints( points ); +#endif + + cout << "Scan " << i << " size: " << points.size() << endl; + double centroid[3] = {0., 0., 0.}; + double centroid_transformed[3];; + for (size_t j = 0; j < points.size(); ++j) { + for (unsigned int k = 0; k < 3; ++k) + centroid[k] += points[j][k]; + } + for (unsigned int k = 0; k < 3; ++k) { + centroid[k] /= (double)points.size(); + } + VTrans(MetaMatrix[i].back(), centroid, centroid_transformed); + for (unsigned int k = 0; k < 3; ++k) { + CoM[k] += centroid_transformed[k]; + } + } + for (unsigned int k = 0; k < 3; ++k) + CoM[k] /= octpts.size() * 1.; + + cout << "Center of Mass at: " << CoM[0] << ", " << CoM[1] << ", " << CoM[2] << endl; + + RVX = -CoM[0]; + RVY = -CoM[1]; + RVZ = -CoM[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } +} + +/* + * A function that read the .frame files created by slam6D + * + * @param dir the directory + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param read a file containing a initial transformation matrix and apply it + */ +int readFrames(string dir, int start, int end, bool readInitial, IOType &type) +{ + + // convert to OpenGL coordinate system + double mirror[16]; + M4identity(mirror); + mirror[10] = -1.0; + + double initialTransform[16]; + if (readInitial) { + cout << "Initial Transform:" << endl; + string initialTransformFileName = dir + "initital.frame"; + ifstream initial_in(initialTransformFileName.c_str()); + if (!initial_in.good()) { + cout << "Error opening " << initialTransformFileName << endl; + exit(-1); + } + initial_in >> initialTransform; + cout << initialTransform << endl; + + // update the mirror to apply the initial frame for all frames + double tempxf[16]; + MMult(mirror, initialTransform, tempxf); + memcpy(mirror, tempxf, sizeof(tempxf)); + } + + for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + const double* transformation; + Scan::AlgoType algoType; + vector Matrices; + vector algoTypes; + + // iterate over frames (stop if none were created) and pull/convert the frames into local containers + unsigned int frame_count; + try { + frame_count = (*it)->readFrames(); + } catch(std::ios_base::failure& e) { + break; + } + for(unsigned int i = 0; i < frame_count; ++i) { + (*it)->getFrame(i, transformation, algoType); + double* transMatOpenGL = new double[16]; + + // apply mirror to convert (and initial frame if requested) the frame and save in opengl + MMult(mirror, transformation, transMatOpenGL); + + Matrices.push_back(transMatOpenGL); + algoTypes.push_back(algoType); + } + + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + + if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && it == Scan::allScans.begin()) { + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + } + current_frame = MetaMatrix.back().size() - 1; + } + + if (MetaMatrix.size() == 0) { + cerr << "*****************************************" << endl; + cerr << "** ERROR: No .frames could be found! **" << endl; + cerr << "*****************************************" << endl; + cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; + return -1; + } + return 0; +} + +void generateFrames(int start, int end, bool identity) { + if (identity) { + cout << "using Identity for frames " << endl; + } else { + cout << "using pose information for frames " << endl; + } + int fileCounter = start; + int index = 0; + for (;;) { + if (fileCounter > end) break; // 'nuf read + fileCounter++; + + vector Matrices; + vector algoTypes; + + for (int i = 0; i < 3; i++) { + double *transMat = new double[16]; + + if (identity) { + M4identity(transMat); + transMat[10] = -1.0; + } else { + EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); + } + + Matrices.push_back(transMat); + algoTypes.push_back(Scan::ICP); + + } + index++; + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + current_frame = MetaMatrix.back().size() - 1; + } +} + +void cycleLOD() { + LevelOfDetail = 0.00001; + for (unsigned int i = 0; i < octpts.size(); i++) + octpts[i]->cycleLOD(); +} + + +void initShow(int argc, char **argv){ + + /***************/ + /* init OpenGL */ + /***************/ + glutInit(&argc,argv); + + cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; + + if(argc <= 1){ + usage(argv[0]); + } + + double red = -1.0; + int start = 0, end = -1, maxDist = -1, minDist = -1; + string dir; + bool readInitial = false; + IOType type = UOS; + int octree = 0; + bool loadOct = false; + bool saveOct = false; + string loadObj; + int origin = 0; + double scale = 0.01; // in m + bool scanserver = false; + double sphereMode = 0.0; + + pose_file_name = new char[1024]; + path_file_name = new char[1024]; + selection_file_name = new char[1024]; + + strncpy(pose_file_name, "pose.dat", 1024); + strncpy(path_file_name, "path.dat", 1024); + strncpy(selection_file_name, "selected.3d", 1024); + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, + octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, + type, scanserver, sphereMode); + + // modify all scale dependant variables + scale = 1.0 / scale; + movementSpeed *= scale; + neardistance *= scale; + oldneardistance *= scale; + maxfardistance *= scale; + fardistance *= scale; + fogDensity /= scale; + defaultZoom *= scale; + voxelSize *= scale; +// oldfardistance *= scale; + + //////////////////////// + SDisplay::readDisplays(loadObj, displays); + //////////////////// + + if (type == OCT) { + loadOct = true; + } + + // if we want to load display file get pointtypes from the files first + if(loadOct) { + string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; + cout << "Getting point information from " << scanFileName << endl; + cout << "Attention! All subsequent oct-files must be of the same type!" << endl; + + pointtype = BOctTree::readType(scanFileName); + } + scan_dir = dir; + + // init and create display + M4identity(view_rotate_button); + obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; + + // Loading scans, reducing, loading frames and generation if neccessary + + // load all available scans + Scan::openDirectory(scanserver, dir, type, start, end); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + scan->setRangeFilter(maxDist, minDist); + if (sphereMode > 0.0) scan->setRangeMutation(sphereMode); + if (red > 0) { + // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time + if(scanserver) { + dynamic_cast(scan)->setShowReductionParameter(red, octree); + } else { + scan->setReductionParameter(red, octree); + } + } + } + cm = new ScanColorManager(4096, pointtype); + +#ifdef USE_COMPACT_TREE + cout << "Creating compact display octrees.." << endl; +#else + cout << "Creating display octrees.." << endl; +#endif + + if(loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; + + // for managed scans the input phase needs to know how much it can handle + std::size_t free_mem = 0; + if(scanserver) + free_mem = ManagedScan::getMemorySize(); + + for(unsigned int i = 0; i < Scan::allScans.size(); ++i) { + Scan* scan = Scan::allScans[i]; + + // create data structures +#ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed + compactTree* tree; + try { + if (red > 0) { // with reduction, only xyz points + DataXYZ xyz_r(scan->get("xyz reduced show")); + tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); + } else { // without reduction, xyz + attribute points + sfloat** pts = pointtype.createPointArray(scan); + unsigned int nrpts = scan->size("xyz"); + tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + } + } catch(...) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } +#else // FIXME: remove the case above + scan->setOcttreeParameter(red, voxelSize, pointtype, loadOct, saveOct); + + DataOcttree* data_oct; + try { + data_oct = new DataOcttree(scan->get("octtree")); + } catch(runtime_error& e) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } + BOctTree* btree = &(data_oct->get()); + unsigned int tree_size = btree->getMemorySize(); + + if(scanserver) { + // check if the octtree would actually fit with all the others + if(tree_size > free_mem) { + delete data_oct; + cout << "Stopping at scan " << i << ", no more octtrees could fit in memory." << endl; + break; + } else { + // subtract available memory + free_mem -= tree_size; + } + } +#endif //FIXME: COMPACT_TREE + +#if !defined USE_COMPACT_TREE + // show structures + // associate show octtree with the scan and hand over octtree pointer ownership + + Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); + + // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points + tree->unlockCachedTree(); +#endif + + // octtrees have been created successfully + octpts.push_back(tree); + + // print something +#ifdef USE_COMPACT_TREE // TODO: change compact tree for memory footprint output, remove this case + cout << "Scan " << i << " octree finished." << endl; +#else + cout << "Scan " << i << " octree finished ("; + bool space = false; + if(tree_size/1024/1024 > 0) { + cout << tree_size/1024/1024 << "M"; + space = true; + } + if((tree_size/1024)%1024 > 0) { + if(space) cout << " "; + cout << (tree_size/1024)%1024 << "K"; + space = true; + } + if(tree_size%1024 > 0) { + if(space) cout << " "; + cout << tree_size%1024 << "B"; + } + cout << ")." << endl; +#endif + } + +/* +TODO: to maximize space for octtrees, implement a heuristic to remove all +CacheObjects sequentially from the start and pack octtrees one after another +until it reaches the maximum size allowed, resetting the index, but causing the +first to be locked again and stopping by catching the exception +set heuristic, do locking, catch exception, reset heuristic to default or old +*/ +#if !defined USE_COMPACT_TREE + if(scanserver) { + // activate all octtrees until they don't fit anymore + cout << "Locking managed octtrees in memory " << flush; + bool stop = false; + unsigned int loaded = 0; + unsigned int dots = (octpts.size() / 20); + if(dots == 0) dots = 1; + vector::iterator it_remove_first = octpts.end(); + for(vector::iterator it = octpts.begin(); it != octpts.end(); ++it) { + if(!stop) { + // try to lock the octtree in cache + try { + Show_BOctTree* stree = dynamic_cast*>(*it); + stree->lockCachedTree(); + loaded++; + if(loaded % dots == 0) cout << '.' << flush; + } catch(runtime_error& e) { + stop = true; + it_remove_first = it; + } + } + if(stop) { + // delete the octtree, resize after iteration + delete *it; + } + } + // now remove iterators for deleted octtrees + if(stop) octpts.erase(it_remove_first, octpts.end()); + cout << ' ' << loaded << " octtrees loaded." << endl; + } + +#endif // !COMPACT_TREE + + + // load frames now that we know how many scans we actually loaded + unsigned int real_end = min((unsigned int)(end), + (unsigned int)(start + octpts.size() - 1)); + if(readFrames(dir, start, real_end, readInitial, type)) + generateFrames(start, real_end, true); + + cm->setCurrentType(PointType::USE_HEIGHT); + //ColorMap cmap; + //cm->setColorMap(cmap); + resetMinMax(0); + + selected_points = new set[octpts.size()]; + + // sets (and computes if necessary) the pose that is used for the reset button + setResetView(origin); + + for (unsigned int i = 0; i < 256; i++) { + keymap[i] = false; + } +} + +void deinitShow() +{ + static volatile bool done = false; + if(done) return; + done = true; + + cout << "Cleaning up octtrees and scans." << endl; + if(octpts.size()) { + // delete octtrees to release the cache locks within + for(vector::iterator it = octpts.begin(); it!= octpts.end(); ++it) { + delete *it; + } + } + + Scan::closeDirectory(); +} + +/** + * Global program scope destructor workaround to clean up data regardless of + * the way of program exit. + */ +struct Deinit { ~Deinit() { deinitShow(); } } deinit; diff --git a/.svn/pristine/a7/a7f6c0e3f30c3b3c06cbed3dce3dca6c80b6e545.svn-base b/.svn/pristine/a7/a7f6c0e3f30c3b3c06cbed3dce3dca6c80b6e545.svn-base new file mode 100644 index 0000000..bafb673 --- /dev/null +++ b/.svn/pristine/a7/a7f6c0e3f30c3b3c06cbed3dce3dca6c80b6e545.svn-base @@ -0,0 +1,2011 @@ +/* + * show_gl implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#include +#include +#include "show/viewcull.h" +#include "show/scancolormanager.h" + +using namespace show; +bool fullydisplayed = true; // true if all points have been drawn to the screen +bool mousemoving = false; // true iff a mouse button has been pressed inside a window, + // but hs not been released +bool keypressed = false; // true iff a key button has been pressed inside a window, + // but hs not been released +double ptstodisplay = 100000; +double lastfps = idealfps; // last frame rate +int pointmode = -1; + +bool smallfont = true; +bool label = true; + +/** + * Displays all data (i.e., points) that are to be displayed + * @param mode spezification for drawing to screen or in selection mode + */ +void DrawPoints(GLenum mode, bool interruptable) +{ + long time = GetCurrentTimeInMilliSec(); + double min = 0.000000001; + double max = 1.0; + LevelOfDetail *= 1.0 + adaption_rate*(lastfps - idealfps)/idealfps; + if (LevelOfDetail > max) LevelOfDetail = max; + else if (LevelOfDetail < min) LevelOfDetail = min; + + // In case of animation + if(frameNr != 0) { + cm->setMode(ScanColorManager::MODE_ANIMATION); + + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + // ignore scans that don't have any frames associated with them + if((unsigned int)iterator >= MetaMatrix.size()) continue; + // set usable frame + double* frame; + Scan::AlgoType type; + if((unsigned int)frameNr >= MetaMatrix[iterator].size()) { + // use last possible frame + frame = MetaMatrix[iterator].back(); + type = MetaAlgoType[iterator].back(); + } else { + frame = MetaMatrix[iterator][frameNr]; + type = MetaAlgoType[iterator][frameNr]; + } + if(type == Scan::INVALID) continue; + cm->selectColors(type); + glPushMatrix(); + glMultMatrixd(frame); + + + glPointSize(pointsize); + ExtractFrustum(pointsize); + cm->selectColors(type); + if (pointmode == 1 ) { + octpts[iterator]->display(); + } else { + octpts[iterator]->displayLOD(LevelOfDetail); + } + glPopMatrix(); + } + + setScansColored(0); + } else { + + if(mode == GL_SELECT){ + // select points mode + // ------------------ + GLuint name = 0; + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + glPushMatrix(); + glMultMatrixd(MetaMatrix[iterator].back()); + + glColor4f(1.0, 0.0, 0.0,1.0); + glPointSize(pointsize + 2.0); + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + glLoadName(name++); + glBegin(GL_POINTS); + glVertex3d((*it)[0], (*it)[1], (*it)[2]); + glEnd(); + } + glPointSize(pointsize); + + glFlush(); + glPopMatrix(); + } + + } else { + + // draw point is normal mode + // ------------------------- + + if (interruptable) { + glDrawBuffer (GL_FRONT); + } + glPointSize(pointsize); + + vector sequence; + calcPointSequence(sequence, current_frame); + for(unsigned int i = 0; i < sequence.size(); i++) { + int iterator = sequence[i]; + // ignore scans that don't have any frames associated with them + if((unsigned int)iterator >= MetaMatrix.size()) continue; + // set usable frame + double* frame; + Scan::AlgoType type; + if((unsigned int)current_frame >= MetaMatrix[iterator].size()) { + // use last possible frame + frame = MetaMatrix[iterator].back(); + type = MetaAlgoType[iterator].back(); + } else { + frame = MetaMatrix[iterator][current_frame]; + type = MetaAlgoType[iterator][current_frame]; + } + if (type == Scan::INVALID) continue; + glPushMatrix(); + if (invert) // default: white points on black background + glColor4d(1.0, 1.0, 1.0, 0.0); + else // black points on white background + glColor4d(0.0, 0.0, 0.0, 0.0); + + // glMultMatrixd(MetaMatrix[iterator].back()); + if (current_frame != (int)MetaMatrix.back().size() - 1) { + cm->setMode(ScanColorManager::MODE_ANIMATION); + cm->selectColors(type); + } + glMultMatrixd(frame); + + ExtractFrustum(pointsize); + if (pointmode == 1 ) { + octpts[iterator]->display(); + } else if (interruptable) { + checkForInterrupt(); + glFlush(); + glFinish(); + if (isInterrupted()) { + glPopMatrix(); + return; + } + octpts[iterator]->display(); + } else { + octpts[iterator]->displayLOD(LevelOfDetail); + } + if (!selected_points[iterator].empty()) { + glColor4f(1.0, 0.0, 0.0, 1.0); + glPointSize(pointsize + 2.0); + glBegin(GL_POINTS); + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + glVertex3d((*it)[0], (*it)[1], (*it)[2]); + } + glEnd(); + glPointSize(pointsize); + } + + glPopMatrix(); + } + } + } + + + if (pointmode == 1 ) { + fullydisplayed = true; + } else { + unsigned long td = (GetCurrentTimeInMilliSec() - time); + if (td > 0) + lastfps = 1000.0/td; + else + lastfps = 1000.0; + fullydisplayed = false; + } + if (interruptable) + fullydisplayed = true; +} + + +void DrawObjects(GLenum mode) { + for (unsigned int i = 0; i < displays.size(); i++) + displays[i]->displayAll(); + +} + +/** + * Draw a smooth path passing from all the camera points. + * + */ +void DrawPath() +{ + + glLineWidth(10.0); + // draw path + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < path_vectorX.size(); j++){ + // set the color + glColor4f(0.0, 1.0, 0.0, 1.0); + // set the points + glVertex3f(path_vectorX.at(j).x,path_vectorX.at(j).y,path_vectorZ.at(j).y); + } + glEnd(); + + // draw lookat path + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < lookat_vectorX.size(); j++){ + //set the color + glColor4d(1.0, 1.0, 0.0, 1.0); + //set the points + glVertex3f(lookat_vectorX.at(j).x,lookat_vectorX.at(j).y,lookat_vectorZ.at(j).y); + } + glEnd(); + + // draw up path + /* + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < ups_vectorX.size(); j++){ + //set the color + glColor4d(0.0, 1.0, 0.0, 0.7); + //set the points + glVertex3f(ups_vectorX.at(j).x,ups_vectorX.at(j).y,ups_vectorZ.at(j).y); + } + glEnd(); + */ +} + +/** + * Draw the camera boxes in the viewer + * + */ +void DrawCameras(void) +{ + for (unsigned int i = 0; i < cams.size(); i++) { + glPushMatrix(); + + // TODO improve upon this primitive camera + Point p = cams[i]; + Point l = Point::norm( lookats[i] - p ); // forward vector + Point u = Point::norm( ups[i] - p ); // up vector + Point r = Point::cross(l,u); // right vector + l = 5 * l; + r = 5 * r; + u = 5 * u; + + Point cube[8]; + cube[0] = p + l - r - u; + cube[1] = p + l + r - u; + cube[2] = p - l + r - u; + cube[3] = p - l - r - u; + cube[4] = p + l - r + u; + cube[5] = p + l + r + u; + cube[6] = p - l + r + u; + cube[7] = p - l - r + u; + + int sides[6][4] = {{0,1,2,3}, {4,5,6,7}, {0,1,5,4}, + {3,2,6,7}, {1,2,6,5}, {0,3,7,4}}; + + if (i+1 == cam_choice) { + glColor4f(1, 0, 1, 1); + } else { + glColor4f(0, 1, 0, 1); + } + // camera cube + glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glBegin(GL_QUADS); + for (int j = 0; j < 6; j++) { + if (j == 2) continue; + for (int k = 0; k < 4; k++) { + int index = sides[j][k]; + glVertex3d(cube[index].x, cube[index].y, cube[index].z); + } + } + glEnd(); + + r = 5 * r; + u = 5 * u; + + glColor4f(1, 1, 0, 1); + if (i+1 == cam_choice) { + glPointSize(10); + } else { + glPointSize(5); + } + glBegin(GL_POINTS); + glVertex3d( lookats[i].x, lookats[i].y, lookats[i].z); + glEnd(); + + Point fcube[8]; + fcube[0] = cube[4]; + fcube[1] = cube[5]; + fcube[2] = cube[1]; + fcube[3] = cube[0]; + fcube[4] = lookats[i] - r + u; + fcube[5] = lookats[i] + r + u; + fcube[6] = lookats[i] + r - u; + fcube[7] = lookats[i] - r - u; + + glLineWidth(2.0); + glLineStipple(1, 0x0C0F); + glEnable(GL_LINE_STIPPLE); + glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); + // camera FOV + glBegin(GL_QUADS); + for (int j = 0; j < 6; j++) { + for (int k = 0; k < 4; k++) { + int index = sides[j][k]; + glVertex3d(fcube[index].x, fcube[index].y, fcube[index].z); + } + } + glEnd(); + glDisable(GL_LINE_STIPPLE); + + + /* + if (i+1 == cam_choice) { + glColor3f(1, 1, 0); + glPointSize(20); + } else { + glColor3f(0, 0, 1); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d(p.x, p.y, p.z); + glEnd(); + + if (i+1 == cam_choice) { + glColor3f(0, 1, 1); + glPointSize(20); + } else { + glColor3f(1, 0, 0); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d( l.x, l.y, l.z); + glEnd(); + + if (i+1 == cam_choice) { + glColor3f(1, 0, 1); + glPointSize(20); + } else { + glColor3f(0, 1, 0); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d( u.x, u.y, u.z); + glEnd(); +*/ + + glPopMatrix(); + } +} + +//----------------------------------------------------------------------------------- + + +/** + * Display function + */ +void DisplayItFunc(GLenum mode, bool interruptable) +{ + /** + * Color of the fog + */ + + GLfloat fogColor[4]; + + if(!invert) { + glEnable(GL_COLOR_LOGIC_OP); + glLogicOp(GL_COPY_INVERTED); + } + + // set the clear color buffer in case of + // both invert and non invert mode + if (invert) + glClearColor(0.0, 0.0, 0.0, 0.0); + else + glClearColor(1.0, 1.0, 1.0, 1.0); + + // clear the color and depth buffer bit + if (!interruptable) { // single buffer mode, we need the depth buffer + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + } + + glPushMatrix(); + + // set the matrix mode + glMatrixMode(GL_MODELVIEW); + + // init modelview matrix + glLoadIdentity(); + + // set the polygon mode + glPolygonMode(GL_FRONT/*_AND_BACK*/, GL_LINE); + + // do the model-transformation + if (haveToUpdate == 6 && path_iterator < path_vectorX.size() ) { + gluLookAt(path_vectorX.at(path_iterator).x, + path_vectorX.at(path_iterator).y, + path_vectorZ.at(path_iterator).y, + lookat_vectorX.at(path_iterator).x, + lookat_vectorX.at(path_iterator).y, + lookat_vectorZ.at(path_iterator).y, + ups_vectorX.at(path_iterator).x - path_vectorX.at(path_iterator).x, + ups_vectorX.at(path_iterator).y - path_vectorX.at(path_iterator).y, + ups_vectorZ.at(path_iterator).y - path_vectorZ.at(path_iterator).y); + } else { + if (cameraNavMouseMode == 1) { + glRotated( mouseRotX, 1, 0, 0); + glRotated( mouseRotY, 0, 1, 0); + glRotated( mouseRotZ, 0, 0, 1); + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + update_view_rotate(0); + } else { + double t[3] = {0,0,0}; + double mat[16]; + QuatToMatrix4(quat, t, mat); + glMultMatrixd(mat); + + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + //glRotated(angle, axis[0], axis[1], axis[2]); // rotate the camera + double rPT[3]; + Matrix4ToEuler(mat, rPT); + mouseRotX = deg(rPT[0]); + mouseRotY = deg(rPT[1]); + mouseRotZ = deg(rPT[2]); + + } + updateControls(); + + glTranslated(X, Y, Z); // move camera + } + +// cout << "Position :" << X << " " << Y << " " << Z << endl; +// cout << "Quaternion:" << quat[0] << " " << quat[1] << " " << quat[2] << " " << quat[3] << endl; +// cout << "Axis/Angle:" << axis[0] << " " << axis[1] << " " << axis[2] << " " << angle << endl; +// cout << "Apex angle:" << cangle << endl; +// cout << endl; + + // process fog + if (show_fog > 0) { + if (show_fog > 3) + fogColor[0] = fogColor[1] = fogColor[2] = fogColor[3] = 1.0; + else + fogColor[0] = fogColor[1] = fogColor[2] = fogColor[3] = 0.0; + glEnable(GL_FOG); + { + // ln(1/2^8) = -5.54517744 -> threshold at which the last color bit is gone due to fog + if (show_fog==1) {fogMode = GL_EXP; fardistance = min(5.54517744 / fogDensity, (double)maxfardistance);} + else if (show_fog==2) {fogMode = GL_EXP2; fardistance = min(sqrt(5.54517744) / fogDensity, (double)maxfardistance);} + else if (show_fog==3) {fogMode = GL_LINEAR; fardistance = 32000.0;} + else if (show_fog==4) {fogMode = GL_EXP; fardistance = (double)maxfardistance; } + else if (show_fog==5) {fogMode = GL_EXP2; fardistance = (double)maxfardistance; } + else if (show_fog==6) {fogMode = GL_LINEAR; fardistance = (double)maxfardistance;} + glFogi(GL_FOG_MODE, fogMode); + glFogfv(GL_FOG_COLOR, fogColor); + glFogf(GL_FOG_DENSITY, fogDensity); + glHint(GL_FOG_HINT, GL_FASTEST); + glFogf(GL_FOG_START, neardistance); + glFogf(GL_FOG_END, maxfardistance); + } + } else { + glDisable(GL_FOG); + fardistance = maxfardistance; + } + if (fardistance > maxfardistance) fardistance = maxfardistance; + if ( fabs(oldfardistance - fardistance) > 0.00001 || fabs(oldneardistance - neardistance) > 0.00001 ) { + oldfardistance = fardistance; + oldneardistance = neardistance; + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + } + + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // + // show the objects __after__ the model-transformation + // for all status variables we show the appropiated thing + // using the drawing functions + // + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + if (show_path == 1) { + double *pose; + glColor4d(1.0, 0.0, 0.0, 1.0); + glLineWidth(5); + glBegin(GL_LINE_STRIP); + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + // set usable type + Scan::AlgoType type; + if((unsigned int)frameNr >= MetaMatrix[i].size()) { + type = MetaAlgoType[i].back(); + } else { + type = MetaAlgoType[i][frameNr]; + } + if(frameNr >= 1 && frameNr < (int)MetaMatrix[i].size()) { + if(type == Scan::INVALID) continue; + // avoid incomplete frames in a scan + if((unsigned int)frameNr >= MetaMatrix[i].size()) + pose = MetaMatrix[i].back(); + else + pose = MetaMatrix[i][frameNr]; + } else { + //pose = MetaMatrix[i].back(); + // avoid incomplete frames in a scan + if((unsigned int)current_frame >= MetaMatrix[i].size()) + pose = MetaMatrix[i].back(); + else + pose = MetaMatrix[i][current_frame]; + } + if(showTopView) { + glVertex3f(pose[12], 2000, pose[14]); + } else { + glVertex3f(pose[12], pose[13], pose[14]); + } + } + glEnd(); + } + + // if show camera is true then draw cameras. + if (show_cameras == 1) { + DrawCameras(); + } + + // if show path is true the draw path. + if (show_path == 1) { + DrawPath(); + } + DrawObjects(mode); + + if (label) DrawUrl(); + + // if show points is true the draw points + if (show_points == 1) DrawPoints(mode, interruptable); + + + glPopMatrix(); + + if (!invert) { + glDisable(GL_COLOR_LOGIC_OP); + } + + // force draw the scene + glFlush(); + glFinish(); +} + +void DrawUrl() { + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + glLoadIdentity(); + glMatrixMode(GL_PROJECTION); + + // Save the current projection matrix + glPushMatrix(); + // Make the current matrix the identity matrix + glLoadIdentity(); + + // Set the projection (to 2D orthographic) + glOrtho(0.0,100.0,0.0,100.0,-1.5,1.5); + + glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // TODO + + glColor4d(0.0,0.0,0.0,0.7); + + glBegin(GL_QUADS); + glVertex3f(0,0,1.49); + glVertex3f(0,6,1.49); + if(smallfont) { + glVertex3f(22,6,1.49); + glVertex3f(22,0,1.49); + } else { + glVertex3f(25,6,1.49); + glVertex3f(25,0,1.49); + } + glEnd(); + + glBlendFunc(GL_ONE, GL_ZERO); + glColor3f(1,1,1); + if(smallfont) { + glRasterPos3f(1,3.5,1.5); + _glutBitmapString(GLUT_BITMAP_8_BY_13, "created by 3DTK"); + glRasterPos3f(1,1,1.5); + _glutBitmapString(GLUT_BITMAP_8_BY_13, "http://threedtk.de"); + } else { + glRasterPos3f(1,3.5,1.5); + _glutBitmapString(GLUT_BITMAP_9_BY_15, "created by 3DTK"); + glRasterPos3f(1,1,1.5); + _glutBitmapString(GLUT_BITMAP_9_BY_15, "http://threedtk.de"); + } + + // Restore the original projection matrix + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + glPopMatrix(); + +} +/** + * Function topview. Set the screen for top view. + */ +void topView() +{ + static GLdouble save_qx, save_qy, save_qz, save_qangle, save_X, save_Y, save_Z; + static GLdouble saveMouseRotX, saveMouseRotY, saveMouseRotZ; + + if (!showTopView) // set to top view + { + showTopView = true; + // save current pose + save_X = X; + save_Y = Y; + save_Z = Z; + save_qx = quat[0]; + save_qy = quat[1]; + save_qz = quat[2]; + save_qangle = quat[3]; + saveMouseRotX = mouseRotX; + saveMouseRotY = mouseRotY; + saveMouseRotZ = mouseRotZ; + + Y = Y - 350.0; + Z = Z + 500.0; + quat[0] = quat[1] = sqrt(0.5); + quat[2] = quat[3] = 0.0; + mouseRotX = 90; + mouseRotY = 0; + mouseRotZ = 0; + + haveToUpdate = 2; + } else { + showTopView = false; + + // restore old settings + X = save_X; + Y = save_Y; + Z = save_Z; + quat[0] = save_qx; + quat[1] = save_qy; + quat[2] = save_qz; + quat[3] = save_qangle; + mouseRotX = saveMouseRotX; + mouseRotY = saveMouseRotY; + mouseRotZ = saveMouseRotZ; + + haveToUpdate = 2; + } +} + +//--------------------------------------------------------------------------- +/** + * This function is called when the user wants to + * delete a camera. + */ + +void callDeleteCamera(int dummy){ + + //iterator for the position of camera + //in the camera list + vector::iterator position; + vector::iterator positionL; + vector::iterator positionU; + + //calculate the position of the camera. we are referring + //to the selected camera + position = cams.begin()+ (cam_choice-1); + positionL = lookats.begin()+ (cam_choice-1); + positionU = ups.begin()+ (cam_choice-1); + + //if no camera present then return + if(cam_choice == 0) + return; + + //if the list is not empty then + if(!cams.empty()){ + //delete the camera from the position + cams.erase(position); + lookats.erase(positionL); + ups.erase(positionU); + //reset the cam_choice spinner values + } + + updateCamera(); +} + + +//--------------------------------------------------------------------------- +/** + * Function to reset the viewer window. + */ + +void resetView(int dummy) +{ + cangle = 60.0; + pzoom = defaultZoom; + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + haveToUpdate = 2; + mouseRotX = 0; + mouseRotY = 0; + mouseRotZ = 0; + + resetRotationButton(); +} + +/** + * Function to set the viewer window back to a previously saved state. + */ + +void setView(double pos[3], double new_quat[4], + double newMouseRotX, double newMouseRotY, double newMouseRotZ, + double newCangle, + bool sTV, bool cNMM, double pzoom_new, + bool s_points, bool s_path, bool s_cameras, double ps, int + sf, double fD, bool inv) +{ + X = pos[0]; + Y = pos[1]; + Z = pos[2]; + for(int i = 0; i < 4; i++) { + quat[i] = new_quat[i]; + } + cangle = newCangle; + mouseRotX = newMouseRotX; + mouseRotY = newMouseRotY; + mouseRotZ = newMouseRotZ; + showTopView = sTV, + cameraNavMouseMode = cNMM; + pzoom = pzoom_new; + updateTopViewControls(); + + show_points = s_points; + show_path = s_path; + show_cameras = s_cameras; + pointsize = ps; + show_fog = sf; + fogDensity = fD; + invert = inv; + + haveToUpdate = 2; +} + + +/** + * This function is called when the viewer is created. This + * acts as the display function. + */ +void CallBackDisplayFunc() +{ + if ((cangle_spinner != 0 && (fabs(cangle_old - cangle) > 0.5)) || + (pzoom_spinner != 0 && (fabs(pzoom_old - pzoom) > 0.5))) { + + cangle_old = cangle; + pzoom_old = pzoom; + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); +#ifdef _MSC_VER + Sleep(25); +#else + usleep(250000); +#endif + } + + glDrawBuffer(buffermode); + // delete framebuffer and z-buffer + + //Call the display function + DisplayItFunc(GL_RENDER ); + + // show the rednered scene + glutSwapBuffers(); + +} + +/** + * This function is called when there is nothing to be done + * in the screen. + */ +void CallBackIdleFunc(void) +{ + +#ifdef _MSC_VER + Sleep(1); +#else + usleep(1000); +#endif + + if(glutGetWindow() != window_id) + glutSetWindow(window_id); + + // return as nothing has to be updated + if (haveToUpdate == 0) { + if (!fullydisplayed && !mousemoving && !keypressed && pointmode == 0 + ) { + glDrawBuffer(buffermode); + //Call the display function + DisplayItFunc(GL_RENDER, true); + } + return; + } + + // case: display is invalid - update it + if (haveToUpdate == 1) { + glutPostRedisplay(); + haveToUpdate = 0; + return; + } + // case: display is invalid - update it with all points +/* if (haveToUpdate == 7) { + showall = true; + glutPostRedisplay(); + haveToUpdate = 0; + return; + }*/ + + // case: camera angle is changed - instead of repeating code call Reshape, + // since these OpenGL commands are the same + if (haveToUpdate == 2) { + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + glutPostRedisplay(); + haveToUpdate = 0; + return; + } + + // case: animation + if(haveToUpdate == 3 ){ + frameNr += 1; + if(!(MetaMatrix.size() > 1 && frameNr < (int) MetaMatrix[1].size())){ + frameNr = 0; + haveToUpdate = 4; + return; + } + glutPostRedisplay(); + + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(frameNr,5) + ".ppm"; + cout << "write " << filename << endl; + int tmpUpdate = haveToUpdate; + glWriteImagePPM(filename.c_str(), factor, 0); + haveToUpdate = tmpUpdate; + + string jpgname = scan_dir + "animframe" + to_string(frameNr,5) + ".jpg"; + string systemcall = "convert -quality 100 -type TrueColor " + filename + " " + jpgname; + // cout << systemcall << endl; + system(systemcall.c_str()); + systemcall = "rm " + filename; + system(systemcall.c_str()); + // cout << systemcall << endl; + // for f in *ppm ; do convert -quality 100 -type TrueColor $f `basename $f ppm`jpg; done + } + + } +#ifdef _MSC_VER + Sleep(300); + Sleep(anim_delay); +#else + usleep(anim_delay * 10000); +#endif + + if (haveToUpdate == 4) { // stop animation + frameNr = 0; // delete these lines if you want a 'continue' functionality. + haveToUpdate = 1; + } + + // case: path animation + if(haveToUpdate == 6){ + + if (path_iterator == 0) { + oldcamNavMode = cameraNavMouseMode; // remember state of old mousenav + cameraNavMouseMode = 0; + } + + // check if the user wants to animate both + // scan matching and the path at the same + //time + + // cout << "path_iterator: " << path_iterator << endl; + if(path_iterator < path_vectorX.size()){ // standard animation case + + // call the path animation function + // hide both the cameras and the path + show_cameras = 0; + show_path = 0; + // increase the iteration count + + path_iterator += 1; + // repaint the screen + glutPostRedisplay(); + + // save the animation + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(path_iterator,5) + ".ppm"; + string jpgname = scan_dir + "animframe" + to_string(path_iterator,5) + ".jpg"; + cout << "written " << filename << " of " << path_vectorX.size() << " files" << endl; + glWriteImagePPM(filename.c_str(), factor, 0); + string systemcall = "convert -quality 100 " + filename + " " + jpgname; + system(systemcall.c_str()); + systemcall = "rm " + filename; + system(systemcall.c_str()); + haveToUpdate = 6; + + } + }else{ // animation has just ended + cameraNavMouseMode = oldcamNavMode; + show_cameras = 1; + show_path = 1; + haveToUpdate = 0; + } + } + +} + + +/** + * This function handles the rotation of the view + */ + +void update_view_rotate(int t) +{ + double view_rotate_button_quat[4]; + + // convert the rotate button matrix to quaternion + //Matrix4ToQuaternion(view_rotate_button, view_rotate_button_quat); + double mat[16]; + for (int i = 0; i < 16; i++) + mat[i] = view_rotate_button[i]; + Matrix4ToQuat(mat, view_rotate_button_quat); + + // normalize the quartenion + QuatNormalize(view_rotate_button_quat); + + // copy it to the global quartenion quat + memcpy(quat, view_rotate_button_quat, sizeof(quat)); +} + +/** + * This function handles the translation of view. + */ +void update_view_translation(int t) +{ + double obj_pos_button1[3]; + + for (int i = 0; i < 3; i++) { + if (fabs(obj_pos_button_old[i] - obj_pos_button[i]) > COMPARE_EPSILON) { + obj_pos_button1[i] = obj_pos_button[i] - obj_pos_button_old[i]; + obj_pos_button_old[i] = obj_pos_button[i]; + } else obj_pos_button1[i] = 0.0; + } + + X = X + obj_pos_button1[0] * view_rotate_button[0] + obj_pos_button1[1] * view_rotate_button[1] + obj_pos_button1[2] * view_rotate_button[2]; + Y = Y + obj_pos_button1[0] * view_rotate_button[4] + obj_pos_button1[1] * view_rotate_button[5] + obj_pos_button1[2] * view_rotate_button[6]; + Z = Z + obj_pos_button1[0] * view_rotate_button[8] + obj_pos_button1[1] * view_rotate_button[9] + obj_pos_button1[2] * view_rotate_button[10]; + +} + + +/** + * handles the animation button + * @param dummy not needed necessary for glui + */ +void startAnimation(int dummy) +{ + if (MetaMatrix.size() > 0) { + if (haveToUpdate != 3) { + haveToUpdate = 3; + } + else // stop animation + haveToUpdate = 4; + } +} + +/** + * calls the resetView function + * @param dummy not needed necessary for glui + */ +void callResetView(int dummy) +{ + if (showTopView) callTopView(dummy); + resetView(0); +} + +/** + * calls the resetView function + * @param dummy not needed necessary for glui + */ +void invertView(int dummy) +{ + invert = !invert; +} + +/** + * calls the topView function + * @param dummy not needed necessary for glui + */ +void callTopView(int dummy) +{ + topView(); + if (showTopView) { + rotButton->disable(); + cangle_spinner->disable(); + pzoom_spinner->enable(); + } else { + rotButton->enable(); + cangle_spinner->enable(); + pzoom_spinner->disable(); + } +} + +/** + * calls the cameraView function + * @param dummy not needed necessary for glui + */ +void callAddCamera(int dummy) +{ + Point campos(-X, -Y, -Z); + + // calculate lookat point + Point lookat; + Point up(0, 0, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = view_rotate_button[i]; + + lookat.x = -50*tmat[2] -X; + lookat.y = -50*tmat[6] -Y; + lookat.z = -50*tmat[10] -Z; + + up.x = 50*tmat[1] -X; + up.y = 50*tmat[5] -Y; + up.z = 50*tmat[9] -Z; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + + updateCamera(); + + // signal to repaint screen + haveToUpdate = 1; +} + +void selectPoints(int x, int y) { + + GLuint selectBuf[BUFSIZE]; + GLint hits; + GLint viewport[4]; + if (selectOrunselect) { + // set the matrix mode + glMatrixMode(GL_MODELVIEW); + // init modelview matrix + glLoadIdentity(); + + // do the model-transformation + if (cameraNavMouseMode == 1) { + glRotated( mouseRotX, 1, 0, 0); + glRotated( mouseRotY, 0, 1, 0); + glRotated( mouseRotZ, 0, 0, 1); + } else { + double t[3] = {0,0,0}; + double mat[16]; + QuatToMatrix4(quat, t, mat); + glMultMatrixd(mat); + + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + double rPT[3]; + Matrix4ToEuler(mat, rPT); + mouseRotX = deg(rPT[0]); + mouseRotY = deg(rPT[1]); + mouseRotZ = deg(rPT[2]); + } + updateControls(); + glTranslated(X, Y, Z); // move camera + + static sfloat *sp2 = 0; +/* for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + if (!selected_points[iterator].empty()) sp2 = *selected_points[iterator].begin(); + + // selected_points[iterator].clear(); + }*/ + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + glPushMatrix(); + glMultMatrixd(MetaMatrix[iterator].back()); + calcRay(x, y, 1.0, 40000.0); + if (select_voxels) { + octpts[iterator]->selectRay(selected_points[iterator], selection_depth); + } else if (brush_size == 0) { + sfloat *sp = 0; + octpts[iterator]->selectRay(sp); + if (sp != 0) { + cout << "Selected point: " << sp[0] << " " << sp[1] << " " << sp[2] << endl; + + if (sp2 != 0) { + cout << "Distance to last point: " + << sqrt( sqr(sp2[0] - sp[0]) + sqr(sp2[1] - sp[1]) + sqr(sp2[2] - sp[2]) ) << endl; + } + sp2 = sp; + + selected_points[iterator].insert(sp); + } + } else { // select multiple points with a given brushsize + octpts[iterator]->selectRayBrushSize(selected_points[iterator], brush_size); + } + + glPopMatrix(); + } + + } else { + // unselect points + glGetIntegerv(GL_VIEWPORT, viewport); + + glSelectBuffer(BUFSIZE, selectBuf); + (void) glRenderMode(GL_SELECT); + + glInitNames(); + glPushName(0); + + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + +// gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), 10.0, 10.0, viewport); + gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), brush_size*2, brush_size*2, viewport); + gluPerspective(cangle, aspect, neardistance, fardistance); + glMatrixMode(GL_MODELVIEW); + DisplayItFunc(GL_SELECT); + + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + + hits = glRenderMode(GL_RENDER); // get hits + ProcessHitsFunc(hits, selectBuf); + } + glPopMatrix(); + glutPostRedisplay(); +} + +void CallBackMouseFuncMoving(int button, int state, int x, int y) +{ + + if( state == GLUT_DOWN) { + mousemoving = true; + } else { + mousemoving = false; + } +} + + +/** + * This function is called after a mousebutton has been pressed. + */ +void CallBackMouseFunc(int button, int state, int x, int y) +{ + // Are we selecting points or moving the camera? + if(cameraNavMouseMode != 1) { // selecting points + if (state == GLUT_DOWN && (button == GLUT_LEFT_BUTTON || button == GLUT_RIGHT_BUTTON)) { + selectPoints(x,y); + } + } else { + if( state == GLUT_DOWN) { + + mouseNavX = x; + mouseNavY = y; + mouseNavButton = button; + + mousemoving = true; + } else { + mouseNavButton = -1; + mousemoving = false; + } + } +} + + +void moveCamera(double x, double y, double z, double rotx, double roty, double rotz) { + interruptDrawing(); + double mat[9]; + + double xr = M_PI * mouseRotX / 180; + double yr = M_PI * mouseRotY / 180; + double zr = M_PI * mouseRotZ / 180; + double c1,c2,c3,s1,s2,s3; + s1 = sin(xr); c1 = cos(xr); + s2 = sin(yr); c2 = cos(yr); + s3 = sin(zr); c3 = cos(zr); + mat[0] = c2*c3; + mat[1] = -c2*s3; + mat[2] = s2; + mat[3] = c1*s3+c3*s1*s2; + mat[4] = c1*c3-s1*s2*s3; + mat[5] = -c2*s1; + mat[6] = s1*s3-c1*c3*s2; + mat[7] = c1*s2*s3+c3*s1; + mat[8] = c1*c2; + + double transX, transY, transZ; + transX = transY = transZ = 0.0; + + mouseRotX += rotx; + mouseRotY -= roty; + mouseRotZ -= rotz; + + if (mouseRotX < -90) mouseRotX=-90; + else if (mouseRotX > 90) mouseRotX=90; + if (mouseRotY > 360) mouseRotY-=360; + else if (mouseRotY < 0) mouseRotY+=360; + if (mouseRotZ > 360) mouseRotZ-=360; + else if (mouseRotZ < 0) mouseRotZ+=360; + + transX += x * mat[0] + y * mat[3] + z * mat[6]; + transY += x * mat[1] + y * mat[4] + z * mat[7]; + transZ += x * mat[2] + y * mat[5] + z * mat[8]; + + + X += transX; + Y += transY; + Z += transZ; + haveToUpdate = 1; + +} + +void KeyboardFunc(int key, bool control, bool alt, bool shift) { + double stepsize = movementSpeed; + if (shift) stepsize *= 10.0; + if (control) stepsize *= 0.1; + + double rotsize = 0.2 * stepsize; + + switch (key) { + case 'w': + case 'W': + moveCamera(0,0,stepsize,0,0,0); + break; + case 'a': + case 'A': + moveCamera(stepsize,0,0,0,0,0); + break; + case 's': + case 'S': + moveCamera(0,0,-stepsize,0,0,0); + break; + case 'd': + case 'D': + moveCamera(-stepsize,0,0,0,0,0); + break; + case 'c': + case 'C': + moveCamera(0,stepsize,0,0,0,0); + break; + case 32: // WXK_SPACE + moveCamera(0,-stepsize,0,0,0,0); + break; + case 314: // WXK_LEFT + moveCamera(0,0,0,0,rotsize,0); + break; + case 315: // WXK_UP + moveCamera(0,0,0,rotsize,0,0); + break; + case 316: // WXK_RIGHT + moveCamera(0,0,0,0,-rotsize,0); + break; + case 317: // WXK_DOWN + moveCamera(0,0,0,-rotsize,0,0); + break; + case 'q': + case 'Q': + case 366: // WXK_PAGEUP + moveCamera(0,0,0,0,0,rotsize); + break; + case 'e': + case 'E': + case 367: // WXK_PAGEDOWN + moveCamera(0,0,0,0,0,-rotsize); + break; + case 'f': + if (!fullscreen) { + fullscreen = true; + glutFullScreen(); + } else { + fullscreen = false; + glutReshapeWindow(current_width, current_height); + } + break; + default: + break; + } +} + +void CallBackMouseMotionFunc(int x, int y) { + double deltaMouseX = x - mouseNavX; + double deltaMouseY = mouseNavY - y; + mouseNavX = x; + mouseNavY = y; + + if(cameraNavMouseMode == 1) { + if( mouseNavButton == GLUT_RIGHT_BUTTON){ + if ( showTopView ) { + deltaMouseX *= 5; + deltaMouseY *= 5; + } + deltaMouseX *= movementSpeed/10.0; // moving 10 pixels is equivalent to one key stroke + deltaMouseY *= movementSpeed/10.0; + moveCamera(deltaMouseX, deltaMouseY, 0, 0,0,0); + } else if( mouseNavButton == GLUT_MIDDLE_BUTTON ){ + if ( !showTopView ) { + deltaMouseY *= -5; + } + deltaMouseX *= movementSpeed/10.0; // moving 10 pixels is equivalent to one key stroke + deltaMouseY *= movementSpeed/10.0; + moveCamera(deltaMouseX, 0, deltaMouseY, 0,0,0); + } else if ( mouseNavButton == GLUT_LEFT_BUTTON ){ + moveCamera(0, 0, 0, deltaMouseY,deltaMouseX,0); + } else { + return; + } + } else { + selectPoints(x,y); + } +} + + + +void initScreenWindow() +{ + // init display + glutInitDisplayMode(GLUT_DEPTH | GLUT_RGBA | GLUT_DOUBLE); + + // define the window position and size + glutInitWindowPosition(START_X, START_Y); + glutInitWindowSize( START_WIDTH, START_HEIGHT ); + + // create window and append callback functions + window_id = glutCreateWindow("3D_Viewer"); + + glutDisplayFunc( CallBackDisplayFunc ); + glutReshapeFunc( CallBackReshapeFunc ); + + glutMouseFunc ( CallBackMouseFunc ); + glutKeyboardFunc ( CallBackKeyboardFunc); + glutKeyboardUpFunc ( CallBackKeyboardUpFunc); + glutMotionFunc ( CallBackMouseMotionFunc); + glutSpecialFunc ( CallBackSpecialFunc); + // glutEntryFunc ( CallBackEntryFunc); + GLUI_Master.set_glutReshapeFunc( CallBackReshapeFunc ); + GLUI_Master.set_glutIdleFunc( CallBackIdleFunc ); + + update_view_rotate(0); + glClearColor(0.0, 0.0, 0.0, 0.0); + // glClearColor(1.0, 1.0, 1.0, 1.0); +} + + +/* +++++++++-------------++++++++++++ + * NAME + * glDumpWindowPPM + * DESCRIPTION + * writes an ppm file of the window + * content + * PARAMETERS + * filename + * RESULT + * writes the framebuffer content + * to a ppm file ++++++++++-------------++++++++++++ */ +void glDumpWindowPPM(const char *filename, GLenum mode) +{ + int win_height, win_width; + int i,j,k,l; // Counter variables + GLubyte *buffer; // The GL Frame Buffer + unsigned char *ibuffer; // The PPM Output Buffer + ofstream fp; // The PPM File + + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + win_height = viewport[3]; + win_width = viewport[2]; + + // Allocate memory for the the frame buffer and output buffer + buffer = new GLubyte[win_width * win_height * RGBA]; + ibuffer = new unsigned char[win_width * win_height * RGB]; + + // Read window contents from GL frame buffer with glReadPixels + glFinish(); + glReadBuffer(buffermode); + glReadPixels(0, 0, win_width, win_height, + GL_RGBA, GL_UNSIGNED_BYTE, buffer); + + // Open the output file + fp.open(filename, ios::out); + + // Write a proper P6 PPM header + fp << "P6" << endl << "# CREATOR: 3D_Viewer by Andreas Nuechter, University of Osnabrueck" + << endl << win_width << " " << win_height << " " << UCHAR_MAX << endl; + + // Loop through the frame buffer data, writing to the PPM file. Be careful + // to account for the frame buffer having 4 bytes per pixel while the + // output file has 3 bytes per pixel + l = 0; + for (i = 0; i < win_height; i++) { // For each row + for (j = 0; j < win_width; j++) { // For each column + for (k = 0; k < RGB; k++) { // For each RGB component + //cout << (RGBA*((win_height-1-i)*win_width+j)+k) << endl; + ibuffer[l++] = (unsigned char) + *(buffer + (RGBA*((win_height-1-i)*win_width+j)+k)); + } // end RGB + } // end column + } // end row + + // to make a video do: + // for f in *ppm ; do convert -quality 100 $f `basename $f ppm`jpg; done + // mencoder "mf://*.jpg" -mf fps=10 -o test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800 + // Write output buffer to the file */ + fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * win_width * win_height)); + fp.close(); + fp.clear(); + delete [] buffer; + delete [] ibuffer; +} + +/* +++++++++-------------++++++++++++ + * NAME + * glDumpWindowPPM + * DESCRIPTION + * writes an ppm file of the window + * content + * size is scale times the window size + * PARAMETERS + * filename + * RESULT + * writes the framebuffer content + * to a ppm file ++++++++++-------------++++++++++++ */ +void glWriteImagePPM(const char *filename, int scale, GLenum mode) +{ + //if(!showTopView) { + int m,o,k; // Counter variables + // Get viewport parameters + double left, right, top, bottom; + double tmp = 1.0/tan(rad(cangle)/2.0); + + // Save camera parameters + GLdouble savedMatrix[16]; + glGetDoublev(GL_PROJECTION_MATRIX,savedMatrix); + GLdouble savedModel[16]; + glGetDoublev(GL_MODELVIEW_MATRIX,savedModel); + //glMatrixMode(GL_PROJECTION); + //glPushMatrix(); + //glMatrixMode(GL_MODELVIEW); + //glPushMatrix(); + top = 1.0/tmp; + bottom = -top; + right = (aspect)/tmp; + left = -right; + + double part_height, part_width; + if(!showTopView) { + part_width = (right - left)/(double)scale; + part_height = (top - bottom)/(double)scale; + } else { + part_height = (2*pzoom)/scale; + part_width = (2*pzoom*aspect)/scale; + cout << part_width << " " << part_height << endl; + } + // Calculate part parameters + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + int win_width = viewport[2]; + int win_height = viewport[3]; + int image_width = scale * win_width; + int image_height = scale * win_height; + + // Allocate memory for the the frame buffer and output buffer + GLubyte *buffer; // The GL Frame Buffer + unsigned char *ibuffer; // The PPM Output Buffer + buffer = new GLubyte[win_width * win_height * RGBA]; + ibuffer = new unsigned char[image_width * image_height * RGB]; + + smallfont = (scale==1); + double height; + if(!showTopView) { + height = bottom; + } else { + height = -pzoom; + } + for(int i = 0; i < scale; i++) { + double width; + if(!showTopView) { + width = left; + } else { + width = -pzoom*aspect; + } + for(int j = 0; j < scale; j++) { + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + label = false; + if(!showTopView) { + glFrustum(neardistance*width, neardistance*(width + part_width), + neardistance*(height), + neardistance*(height + part_height), + neardistance, fardistance); + glMatrixMode(GL_MODELVIEW); + if(i==0 && j==0) { + label = true; + } + DisplayItFunc(mode); + } else { + glOrtho( width, width + part_width, + height, height + part_height, + 1.0, 32000.0 ); + glMatrixMode(GL_MODELVIEW); + if(i==0 && j==0) { + label = true; + } + DisplayItFunc(mode); + } + + // Read window contents from GL frame buffer with glReadPixels + glFinish(); + glReadBuffer(buffermode); + glReadPixels(0, 0, win_width, win_height, GL_RGBA, GL_UNSIGNED_BYTE, buffer); + + // Loop through the frame buffer data, writing to the PPM file. Be careful + // to account for the frame buffer having 4 bytes per pixel while the + // output file has 3 bytes per pixel + // end row + for (m = 0; m < win_height; m++) { // For each row + for (o = 0; o < win_width; o++) { // For each column + for (k = 0; k < RGB; k++) { // For each RGB component + int l = (k+RGB*(image_width*((scale - 1 - i)*win_height + m) + j*win_width + o)); + ibuffer[l] = (unsigned char) *(buffer + (RGBA*((win_height-1-m)*win_width+o)+k)); + } // end RGB + } // end column + } + width += part_width; + } + height += part_height; + } + + // show the starting scene + + // Restore the original projection matrix + glMatrixMode(GL_PROJECTION); + glLoadMatrixd(savedMatrix); + glMatrixMode(GL_MODELVIEW); + glLoadMatrixd(savedModel); + // show the rednered scene + label = true; + smallfont = true; + haveToUpdate=2; + DisplayItFunc(mode); + + ofstream fp; // The PPM File + + // Open the output file + fp.open(filename, ios::out); + + // Write a proper P6 PPM header + fp << "P6" << endl << "# CREATOR: 3D_Viewer by Dorit Borrmann, Jacobs University Bremen gGmbH" + << endl << image_width << " " << image_height << " " << UCHAR_MAX << endl; + + // Write output buffer to the file + fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * image_width * image_height)); + fp.close(); + fp.clear(); + delete [] buffer; + delete [] ibuffer; +} + +/** Reshape Function + * TODO: have to describe it. + * + */ +void CallBackReshapeFunc(int width, int height) +{ + if (!fullscreen) { + current_height = height; + current_width = width; + } + aspect = (double)width/(double)height; + if (!showTopView) { + // usage of the vsize of a structiewport + glViewport(0, 0, (GLint)width, (GLint)height); + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + + // angle, aspect, near clip, far clip + // get matrix + gluPerspective(cangle, aspect, neardistance, fardistance); + + // now use modelview-matrix as current matrix + glMatrixMode(GL_MODELVIEW); + + haveToUpdate = 1; + + } else { + + // usage of the viewport + glViewport ( 0, 0, width, height); + + glMatrixMode ( GL_PROJECTION ); + glLoadIdentity (); + + // get matrix + glOrtho ( -aspect * pzoom, aspect * pzoom, + -1 * pzoom, pzoom, + 1.0, 32000.0 ); + + // now use modelview-matrix as current matrix + glMatrixMode(GL_MODELVIEW); + + haveToUpdate = 1; + + } + // glDepthMask(false); + glEnable(GL_BLEND); // TODO + glBlendFunc(GL_ONE, GL_ZERO); // TODO + // glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // TODO + // glBlendFunc(GL_ONE, GL_ONE); // TODO + // glBlendFunc(GL_SRC_COLOR, GL_DST_COLOR); + glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); + // TODO glDepthFunc(GL_LEQUAL); + glDepthFunc(GL_LESS); //TODO + glEnable(GL_DEPTH_TEST); + glEnable (GL_POINT_SMOOTH); +} + +/** + * Prints out which points were clicked on + */ +void ProcessHitsFunc(GLint hits, GLuint buffer[]) +{ + //cout << "SIZE " << selected_points[0].size() << endl; + //cout << "processing " << endl; + set names; + set unsel_points; + + GLuint *ptr, nr_names; + + ptr = (GLuint *)buffer; + + for(int i = 0 ; i < hits ; i++) { + nr_names = *ptr; + ptr+=3; // skip 2 z values + for(unsigned int j = 0;j < nr_names ; j++){ // iterate over all names + names.insert(*ptr); + + ptr++; + } + } + //cout << "number of names " << names.size() << endl; + if (names.empty()) return; + + int index = 0; + set::iterator nit = names.begin(); + // find the respective name + + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + // iterate over the selected points as in DrawPoints + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + if (index == *nit) { // if the current index is the current name + unsel_points.insert(*it); + nit++; + } + if (nit == names.end()) goto Done; // break out of the loop + index++; + } + } + + Done: + + cout << "Erasing " << endl; + for (set::iterator it = unsel_points.begin(); + it != unsel_points.end(); it++) { // iterate to the index as indicated by the name *ptr + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { // erase for all scans + selected_points[iterator].erase(*it); + } + } + + + cout << "processing done" << endl; + +} + + +//------------------------------------------------------------------ +/** + * This function deals with all our keyboard activities + */ + +void InterfaceFunc(unsigned char key){ + + strncpy(path_file_name, path_filename_edit->get_text(), 1024); + strncpy(pose_file_name, pose_filename_edit->get_text(), 1024); + return; +} + + +void CallBackSpecialFunc(int key , int x, int y) { + //KeyboardFunc(key + 214, false, false, false); + // return; +} + +/** + * Function drawRobotPath + * \brief This functions draws the path where the + * robot has travelled along while taking the scans + */ +void drawRobotPath(int dummy){ + // clear the camera list as we are going to add the cameras + // in the path where the robot travelled. + + // lets loop through the entire frame files to extract the + // total number of places where the robot has taken the scans from + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + //temp variable + double *temp; + // Now, lets go to the last of each frame file to + // extract the transformation matrix obtained + // after scan matching has been done. + glMultMatrixd(MetaMatrix[i].back()); + + // temp is final transformation matrix + temp = MetaMatrix[i].back(); + + Point campos(temp[12], temp[13] + 100, temp[14]); + + // calculate lookat point + Point lookat(0, 0, 50); + Point up(0, 50, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = temp[i]; + lookat.transform(tmat); + lookat.x = lookat.x ; + lookat.y = lookat.y + 100; + lookat.z = lookat.z ; + + up.transform(tmat); + up.x = up.x ; + up.y = up.y + 100; + up.z = up.z ; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + } + updateCamera(); + + // signal for the update of scene + haveToUpdate = 1; +} + +/** + * Calculates the positions of the interpolated camera path positions on the + * Nurbs path. There will be an equal number of intermediate positions between + * neighboring cameras. + */ +void calcInterpolatedCameras(vector vec1, vector vec2) { + NurbsPath::camRatio.clear(); + double distance = 0.0; + double dx, dy, dz; + for(unsigned int i=0;i vec1, vector vec2) +{ + double distance = 0.0; + double dx, dy, dz; + for(unsigned int i=0;i= 'A' && key <= 'Z') { + keymap[key+ ('a'-'A')] = false; + } + if (key >= 'a' && key <= 'z') { + keymap[key+ ('A'-'a')] = false; + } + + for (unsigned int i = 0; i < 256; i++) { + if (keymap[i]) { + keypressed = true; + return; + } + } + keypressed = false; +} + +void CallBackKeyboardFunc(unsigned char key, int x, int y) { + keymap[key] = true; + keypressed = true; + bool cmd,alt,shift; + cmd = glutGetModifiers() & GLUT_ACTIVE_CTRL; + alt = glutGetModifiers() & GLUT_ACTIVE_ALT; + shift = glutGetModifiers() & GLUT_ACTIVE_SHIFT; + if (cmd) { + key += 96; + } + KeyboardFunc(key, cmd, alt, shift); +} + +void mapColorToValue(int dummy) { + switch (listboxColorVal) { + case 0: + cm->setCurrentType(PointType::USE_HEIGHT); + break; + case 1: + cm->setCurrentType(PointType::USE_REFLECTANCE); + break; + case 2: + cm->setCurrentType(PointType::USE_TEMPERATURE); + break; + case 3: + cm->setCurrentType(PointType::USE_AMPLITUDE); + break; + case 4: + cm->setCurrentType(PointType::USE_DEVIATION); + break; + case 5: + cm->setCurrentType(PointType::USE_TYPE); + break; + case 6: + cm->setCurrentType(PointType::USE_COLOR); + break; + default: + break; + }; + resetMinMax(0); +} + +void changeColorMap(int dummy) { + ColorMap c; + GreyMap gm; + HSVMap hsv; + SHSVMap shsv; + JetMap jm; + HotMap hot; + DiffMap diff; + TempMap temp; + + switch (listboxColorMapVal) { + case 0: + // TODO implement no color map + cm->setColorMap(c); + break; + case 1: + cm->setColorMap(gm); + break; + case 2: + cm->setColorMap(hsv); + break; + case 3: + cm->setColorMap(jm); + break; + case 4: + cm->setColorMap(hot); + break; + case 5: + cm->setColorMap(diff); + break; + case 6: + cm->setColorMap(shsv); + break; + case 7: + cm->setColorMap(temp); + break; + default: + break; + } +} + +void minmaxChanged(int dummy) { + cm->setMinMax(mincolor_value, maxcolor_value); +} + +void resetMinMax(int dummy) { + mincolor_value = cm->getMin(); + maxcolor_value = cm->getMax(); + minmaxChanged(0); +} + +void setScansColored(int dummy) { + switch(colorScanVal) { + case 0: + cm->setMode(ScanColorManager::MODE_STATIC); + break; + case 1: + cm->setMode(ScanColorManager::MODE_COLOR_SCAN); + break; + case 2: + cm->setMode(ScanColorManager::MODE_POINT_COLOR); + break; + default: + break; + } +} + + +void changePointMode(int dummy) { + if (dummy == 0) { // always display + if (pointmode != 1) { // standard mode + pointmode = 1; + //never_box->set_int_val(0); + } else { + pointmode = 0; + } + } else if (dummy == 1) { // never display + if (pointmode != -1) { // standard mode + pointmode = -1; + //always_box->set_int_val(0); + } else { + pointmode = 0; + } + } + updatePointModeControls(); +} + + +void callCameraUpdate(int dummy) { + updateCamera(); +} + +void calcPointSequence(vector &sequence, int frameNr) { + sequence.clear(); + vector > dists; + double x,y,z; + + for (unsigned int i = 0; i < octpts.size(); i++) { + // stop at scans that don't have any frames associated with them + if(i >= MetaMatrix.size()) break; + // set usable frame + double* frame; + if((unsigned int)frameNr >= MetaMatrix[i].size()) { + // use last possible frame + frame = MetaMatrix[i].back(); + } else { + frame = MetaMatrix[i][frameNr]; + } + x = frame[12]; + y = frame[13]; + z = frame[14]; + dists.push_back( pair(sqr(X + x) + sqr(Y + y) + sqr(Z + z), i) ); + } + + sort( dists.begin(), dists.end()); + + for (unsigned int i = 0; i < dists.size(); i++) { + sequence.push_back( dists[i].second); + } +} diff --git a/.svn/pristine/ac/ac71fb21115f17bb9771757a649c2713dc26499d.svn-base b/.svn/pristine/ac/ac71fb21115f17bb9771757a649c2713dc26499d.svn-base new file mode 100644 index 0000000..10803a1 --- /dev/null +++ b/.svn/pristine/ac/ac71fb21115f17bb9771757a649c2713dc26499d.svn-base @@ -0,0 +1,438 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +#include + +#define _USE_MATH_DEFINES +#include + +#include +#include +#include +#include +using namespace std; + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "cvblob.h" + +#ifdef M_PI +const double pi = M_PI; +#else +const double pi = std::atan(1.)*4.; +#endif // M_PI + +namespace cvb +{ + + void cvRenderContourChainCode(CvContourChainCode const *contour, IplImage const *img, CvScalar const &color) + { + CV_FUNCNAME("cvRenderContourChainCode"); + __CV_BEGIN__; + { + CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==3)); + + int stepDst = img->widthStep/(img->depth/8); + int img_width = img->width; + int img_height = img->height; + int img_offset = 0; + + if(img->roi) + { + img_width = img->roi->width; + img_height = img->roi->height; + img_offset = (img->nChannels * img->roi->xOffset) + (img->roi->yOffset * stepDst); + } + + unsigned char *imgData = (unsigned char *)img->imageData + img_offset; + + unsigned int x = contour->startingPoint.x; + unsigned int y = contour->startingPoint.y; + + for (CvChainCodes::const_iterator it=contour->chainCode.begin(); it!=contour->chainCode.end(); ++it) + { + imgData[img->nChannels*x+img->widthStep*y+0] = (unsigned char)(color.val[0]); // Blue + imgData[img->nChannels*x+img->widthStep*y+1] = (unsigned char)(color.val[1]); // Green + imgData[img->nChannels*x+img->widthStep*y+2] = (unsigned char)(color.val[2]); // Red + + x += cvChainCodeMoves[*it][0]; + y += cvChainCodeMoves[*it][1]; + } + } + __CV_END__; + } + + CvContourPolygon *cvConvertChainCodesToPolygon(CvContourChainCode const *cc) + { + CV_FUNCNAME("cvConvertChainCodesToPolygon"); + __CV_BEGIN__; + { + CV_ASSERT(cc!=NULL); + + CvContourPolygon *contour = new CvContourPolygon; + + unsigned int x = cc->startingPoint.x; + unsigned int y = cc->startingPoint.y; + contour->push_back(cvPoint(x, y)); + + if (cc->chainCode.size()) + { + CvChainCodes::const_iterator it=cc->chainCode.begin(); + CvChainCode lastCode = *it; + + x += cvChainCodeMoves[*it][0]; + y += cvChainCodeMoves[*it][1]; + + ++it; + + for (; it!=cc->chainCode.end(); ++it) + { + if (lastCode!=*it) + { + contour->push_back(cvPoint(x, y)); + lastCode=*it; + } + + x += cvChainCodeMoves[*it][0]; + y += cvChainCodeMoves[*it][1]; + } + } + + return contour; + } + __CV_END__; + } + + void cvRenderContourPolygon(CvContourPolygon const *contour, IplImage *img, CvScalar const &color) + { + CV_FUNCNAME("cvRenderContourPolygon"); + __CV_BEGIN__; + { + CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==3)); + + CvContourPolygon::const_iterator it=contour->begin(); + + if (it!=contour->end()) + { + unsigned int fx, x, fy, y; + fx = x = it->x; + fy = y = it->y; + + for (; it!=contour->end(); ++it) + { + cvLine(img, cvPoint(x, y), cvPoint(it->x, it->y), color, 1); + x = it->x; + y = it->y; + } + + cvLine(img, cvPoint(x, y), cvPoint(fx, fy), color, 1); + } + } + __CV_END__; + } + + double cvContourPolygonArea(CvContourPolygon const *p) + { + CV_FUNCNAME("cvContourPolygonArea"); + __CV_BEGIN__; + { + CV_ASSERT(p!=NULL); + + if (p->size()<=2) + return 1.; + + CvContourPolygon::const_iterator it=p->begin(); + CvPoint lastPoint = p->back(); + + double a = 0.; + + for (; it!=p->end(); ++it) + { + a += lastPoint.x*it->y - lastPoint.y*it->x; + lastPoint = *it; + } + + return a*0.5; + } + __CV_END__; + } + + double cvContourChainCodePerimeter(CvContourChainCode const *c) + { + CV_FUNCNAME("cvContourChainCodePerimeter"); + __CV_BEGIN__; + { + CV_ASSERT(c!=NULL); + + double perimeter = 0.; + + for(CvChainCodes::const_iterator it=c->chainCode.begin(); it!=c->chainCode.end(); ++it) + { + if ((*it)%2) + perimeter+=sqrt(1.+1.); + else + perimeter+=1.; + } + + return perimeter; + } + __CV_END__; + } + + double cvContourPolygonPerimeter(CvContourPolygon const *p) + { + CV_FUNCNAME("cvContourPolygonPerimeter"); + __CV_BEGIN__; + { + CV_ASSERT(p!=NULL); + + double perimeter = cvDistancePointPoint((*p)[p->size()-1], (*p)[0]); + + for (unsigned int i=0; isize()-1; i++) + perimeter+=cvDistancePointPoint((*p)[i], (*p)[i+1]); + + return perimeter; + } + __CV_END__; + } + + double cvContourPolygonCircularity(const CvContourPolygon *p) + { + CV_FUNCNAME("cvContourPolygonCircularity"); + __CV_BEGIN__; + { + CV_ASSERT(p!=NULL); + + double l = cvContourPolygonPerimeter(p); + double c = (l*l/cvContourPolygonArea(p)) - 4.*pi; + + if (c>=0.) + return c; + else // This could happen if the blob it's only a pixel: the perimeter will be 0. Another solution would be to force "cvContourPolygonPerimeter" to be 1 or greater. + return 0.; + } + __CV_END__; + } + + void simplifyPolygonRecursive(CvContourPolygon const *p, int const i1, int const i2, bool *pnUseFlag, double const delta) + { + CV_FUNCNAME("cvSimplifyPolygonRecursive"); + __CV_BEGIN__; + { + int endIndex = (i2<0)?p->size():i2; + + if (abs(i1-endIndex)<=1) + return; + + CvPoint firstPoint = (*p)[i1]; + CvPoint lastPoint = (i2<0)?p->front():(*p)[i2]; + + double furtherDistance=0.; + int furtherIndex=0; + + for (int i=i1+1; i=delta)&&(d>furtherDistance)) + { + furtherDistance=d; + furtherIndex=i; + } + } + + if (furtherIndex) + { + pnUseFlag[furtherIndex]=true; + + simplifyPolygonRecursive(p, i1, furtherIndex, pnUseFlag, delta); + simplifyPolygonRecursive(p, furtherIndex, i2, pnUseFlag, delta); + } + } + __CV_END__; + } + + CvContourPolygon *cvSimplifyPolygon(CvContourPolygon const *p, double const delta) + { + CV_FUNCNAME("cvSimplifyPolygon"); + __CV_BEGIN__; + { + CV_ASSERT(p!=NULL); + + double furtherDistance=0.; + unsigned int furtherIndex=0; + + CvContourPolygon::const_iterator it=p->begin(); + ++it; + for (unsigned int i=1; it!=p->end(); ++it, i++) + { + double d = cvDistancePointPoint(*it, p->front()); + + if (d>furtherDistance) + { + furtherDistance = d; + furtherIndex = i; + } + } + + if (furtherDistancepush_back(p->front()); + return result; + } + + bool *pnUseFlag = new bool[p->size()]; + for (unsigned int i=1; isize(); i++) pnUseFlag[i] = false; + + pnUseFlag[0] = pnUseFlag[furtherIndex] = true; + + simplifyPolygonRecursive(p, 0, furtherIndex, pnUseFlag, delta); + simplifyPolygonRecursive(p, furtherIndex, -1, pnUseFlag, delta); + + CvContourPolygon *result = new CvContourPolygon; + + for (unsigned int i=0; isize(); i++) + if (pnUseFlag[i]) + result->push_back((*p)[i]); + + delete[] pnUseFlag; + + return result; + } + __CV_END__; + } + + CvContourPolygon *cvPolygonContourConvexHull(CvContourPolygon const *p) + { + CV_FUNCNAME("cvPolygonContourConvexHull"); + __CV_BEGIN__; + { + CV_ASSERT(p!=NULL); + + if (p->size()<=3) + { + return new CvContourPolygon(p->begin(), p->end()); + } + + deque dq; + + if (cvCrossProductPoints((*p)[0], (*p)[1], (*p)[2])>0) + { + dq.push_back((*p)[0]); + dq.push_back((*p)[1]); + } + else + { + dq.push_back((*p)[1]); + dq.push_back((*p)[0]); + } + + dq.push_back((*p)[2]); + dq.push_front((*p)[2]); + + for (unsigned int i=3; isize(); i++) + { + int s = dq.size(); + + if ((cvCrossProductPoints((*p)[i], dq.at(0), dq.at(1))>=0) && (cvCrossProductPoints(dq.at(s-2), dq.at(s-1), (*p)[i])>=0)) + continue; // TODO Optimize. + + while (cvCrossProductPoints(dq.at(s-2), dq.at(s-1), (*p)[i])<0) + { + dq.pop_back(); + s = dq.size(); + } + + dq.push_back((*p)[i]); + + while (cvCrossProductPoints((*p)[i], dq.at(0), dq.at(1))<0) + dq.pop_front(); + + dq.push_front((*p)[i]); + } + + return new CvContourPolygon(dq.begin(), dq.end()); + } + __CV_END__; + } + + void cvWriteContourPolygonCSV(const CvContourPolygon& p, const string& filename) + { + ofstream f; + f.open(filename.c_str()); + + f << p << endl; + + f.close(); + } + + void cvWriteContourPolygonSVG(const CvContourPolygon& p, const string& filename, const CvScalar& stroke, const CvScalar& fill) + { + int minx=INT_MAX; + int miny=INT_MAX; + int maxx=INT_MIN; + int maxy=INT_MIN; + + stringstream buffer(""); + + for (CvContourPolygon::const_iterator it=p.begin(); it!=p.end(); ++it) + { + if (it->x>maxx) + maxx = it->x; + if (it->xx; + + if (it->y>maxy) + maxy = it->y; + if (it->yy; + + buffer << it->x << "," << it->y << " "; + } + + ofstream f; + f.open(filename.c_str()); + + f << "" << endl; + f << "" << endl; + f << "" << endl; + + f << "" << endl; + + f << "" << endl; + + f.close(); + } + +} + +ostream& operator<< (ostream& output, const cvb::CvContourPolygon& p) +{ + for (cvb::CvContourPolygon::const_iterator it=p.begin(); it!=p.end(); ++it) + output << it->x << ", " << it->y << endl; + + return output; +} diff --git a/.svn/pristine/b4/b4244ce440108187820f68e1aa5d2aa47fdea69b.svn-base b/.svn/pristine/b4/b4244ce440108187820f68e1aa5d2aa47fdea69b.svn-base new file mode 100644 index 0000000..d70c639 --- /dev/null +++ b/.svn/pristine/b4/b4244ce440108187820f68e1aa5d2aa47fdea69b.svn-base @@ -0,0 +1,48 @@ +if(WIN32) + add_library(pointfilter STATIC ../slam6d/pointfilter.cc) +else(WIN32) + add_library(pointfilter SHARED ../slam6d/pointfilter.cc) +endif(WIN32) + +set(SCANIO_LIBNAMES + uos uosr uos_rgb uos_rrgbt ks ks_rgb riegl_txt riegl_rgb rts velodyne +) + +if(WITH_RIVLIB) + set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} rxp) + if(LIBXML2_FOUND) + include_directories(${LIBXML2_INCLUDE_DIR}) +# set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} riegl_project) +# target_link_libraries(scan_io_riegl_project ${RIVLIB} scan_io_rxp ${LIBXML2_LIBRARIES}) + endif(LIBXML2_FOUND) +endif(WITH_RIVLIB) + + +foreach(libname ${SCANIO_LIBNAMES}) +if(WIN32) + #add_library(scan_io_${libname} STATIC scan_io_${libname}.cc) + add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) +else(WIN32) + add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) +endif(WIN32) + target_link_libraries(scan_io_${libname} pointfilter ${Boost_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) +endforeach(libname) + +if(WITH_RIVLIB) + target_link_libraries(scan_io_rxp ${RIVLIB}) + if(LIBXML2_FOUND) + target_link_libraries(scan_io_rxp ${LIBXML2_LIBRARIES}) #scan_io_riegl_project ${RIVLIB}) + endif(LIBXML2_FOUND) +endif(WITH_RIVLIB) + +if(WIN32) + add_library(scanio STATIC scan_io.cc ../slam6d/io_types.cc) +else(WIN32) + add_library(scanio SHARED scan_io.cc ../slam6d/io_types.cc) +endif(WIN32) + +if(UNIX) + target_link_libraries(scanio dl) +endif(UNIX) + + diff --git a/.svn/pristine/b6/b607ee34354a884369753887a602090ba29b54a1.svn-base b/.svn/pristine/b6/b607ee34354a884369753887a602090ba29b54a1.svn-base new file mode 100644 index 0000000..ca13036 --- /dev/null +++ b/.svn/pristine/b6/b607ee34354a884369753887a602090ba29b54a1.svn-base @@ -0,0 +1,528 @@ +/* + * scan_red implementation + * + * Copyright (C) Dorit Borrmann, Razvan-George Mihalyi, Remus Dumitru + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Main program for reducing 3D scans. + * + * Program to reduce scans for use with slam6d + * Usage: bin/scan_red -r 'dir', + * Use -r for octree based reduction (voxel size=) + * and 'dir' the directory of a set of scans + * Reduced scans will be written to 'dir/reduced' + * + * @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany. + */ +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#define WANT_STREAM ///< define the WANT stream :) +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::ofstream; +#include + +#include "slam6d/metaScan.h" +#include "slam6d/io_utils.h" +#include "slam6d/scan.h" +#include "slam6d/Boctree.h" +#include "slam6d/fbr/fbr_global.h" +#include "slam6d/fbr/panorama.h" +#include "slam6d/fbr/scan_cv.h" + +#include "scanserver/clientInterface.h" + +#include "slam6d/globals.icc" + +#ifdef _OPENMP +#include +#endif + + +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#include +#include +#else +#include +#include +#include +#include +#endif + +using namespace fbr; + +#include +namespace po = boost::program_options; + +enum reduction_method {OCTREE, RANGE, INTERPOLATE}; + +/* Function used to check that 'opt1' and 'opt2' are not specified + at the same time. */ +void conflicting_options(const po::variables_map & vm, + const char *opt1, const char *opt2) +{ + if (vm.count(opt1) && !vm[opt1].defaulted() + && vm.count(opt2) && !vm[opt2].defaulted()) + throw std::logic_error(string("Conflicting options '") + + opt1 + "' and '" + opt2 + "'."); +} + +/* Function used to check that if 'for_what' is specified, then + 'required_option' is specified too. */ +void option_dependency(const po::variables_map & vm, + const char *for_what, const char *required_option) +{ + if (vm.count(for_what) && !vm[for_what].defaulted()) + if (vm.count(required_option) == 0 + || vm[required_option].defaulted()) + throw std::logic_error(string("Option '") + for_what + + "' requires option '" + + required_option + "'."); +} + +/* + * validates panorama method specification + */ +namespace fbr { + void validate(boost::any& v, const std::vector& values, + projection_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "EQUIRECTANGULAR") == 0) v = EQUIRECTANGULAR; + else if(strcasecmp(arg.c_str(), "CYLINDRICAL") == 0) v = CYLINDRICAL; + else if(strcasecmp(arg.c_str(), "MERCATOR") == 0) v = MERCATOR; + else if(strcasecmp(arg.c_str(), "CONIC") == 0) v = CONIC; + else throw std::runtime_error(std::string("projection method ") + arg + std::string(" is unknown")); + } +} + +/* + * validates input type specification + */ +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +void reduction_option_dependency(const po::variables_map & vm, reduction_method stype, const char *option) +{ + if (vm.count("reduction") && vm["reduction"].as() == stype) { + if (!vm.count(option)) { + throw std::logic_error (string("this reduction option needs ")+option+" to be set"); + } + } +} + +void reduction_option_conflict(const po::variables_map & vm, reduction_method stype, const char *option) +{ + if (vm.count("reduction") && vm["reduction"].as() == stype) { + if (vm.count(option)) { + throw std::logic_error (string("this reduction option is incompatible with ")+option); + } + } +} + + +/* + * validates reduction method specification + */ +void validate(boost::any& v, const std::vector& values, + reduction_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "OCTREE") == 0) v = OCTREE; + else if(strcasecmp(arg.c_str(), "RANGE") == 0) v = RANGE; + else if(strcasecmp(arg.c_str(), "INTERPOLATE") == 0) v = INTERPOLATE; + else throw std::runtime_error(std::string("reduction method ") + arg + std::string(" is unknown")); +} + +void parse_options(int argc, char **argv, int &start, int &end, + bool &scanserver, int &width, int &height, + fbr::projection_method &ptype, string &dir, IOType &iotype, + int &maxDist, int &minDist, reduction_method &rtype, double &scale, + double &voxel, int &octree, bool &use_reflectance) +{ + po::options_description generic("Generic options"); + generic.add_options() + ("help,h", "output this help message"); + + po::options_description input("Input options"); + input.add_options() + ("start,s", po::value(&start)->default_value(0), + "start at scan (i.e., neglects the first scans) " + "[ATTENTION: counting naturally starts with 0]") + ("end,e", po::value(&end)->default_value(-1), + "end after scan ") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose F from {uos, uos_map, " + "uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, " + "riegl_txt, riegl_rgb, riegl_bin, zahn, ply})") + ("max,M", po::value(&maxDist)->default_value(-1), + "neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&minDist)->default_value(-1), + "neglegt all data points with a distance smaller than 'units") + ("scanserver,S", po::bool_switch(&scanserver), + "Use the scanserver as an input method and handling of scan data"); + + po::options_description reduction("Reduction options"); + reduction.add_options() + ("reduction,r", po::value(&rtype)->required(), + "choose reduction method (OCTREE, RANGE, INTERPOLATE)") + ("scale,S", po::value(&scale), + "scaling factor") + ("voxel,v", po::value(&voxel), + "voxel size") + ("projection,P", po::value(&ptype), + "projection method or panorama image") + ("octree,O", po::value(&octree), + "0 -> center\n1 -> random\nN>1 -> random N") + ("width,w", po::value(&width), + "width of panorama") + ("height,h", po::value(&height), + "height of panorama"); + + po::options_description output("Output options"); + output.add_options() + ("reflectance,R", po::bool_switch(&use_reflectance), + "Use reflectance when reducing points and save scan files in UOSR format"); + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + // all options + po::options_description all; + all.add(generic).add(input).add(reduction).add(output).add(hidden); + + // options visible with --help + po::options_description cmdline_options; + cmdline_options.add(generic).add(input).add(reduction).add(output); + + // positional argument + po::positional_options_description pd; + pd.add("input-dir", 1); + + // process options + po::variables_map vm; + po::store(po::command_line_parser(argc, argv). + options(all).positional(pd).run(), vm); + + // display help + if (vm.count("help")) { + cout << cmdline_options; + cout << endl + << "Example usage:" << endl + << "\t./bin/scan_red -s 0 -e 0 -f uos --reduction OCTREE --voxel 10 --octree 0 dat" << endl + << "\t./bin/scan_red -s 0 -e 0 -f uos --reduction RANGE --scale 0.5 --projection EQUIRECTANGULAR --width 3600 --height 1000 dat" << endl + << "\t./bin/scan_red -s 0 -e 0 -f uos --reduction INTERPOLATE --scale 0.2 --projection EQUIRECTANGULAR --width 3600 --height 1000 dat" << endl; + exit(0); + } + + po::notify(vm); + + reduction_option_dependency(vm, OCTREE, "voxel"); + reduction_option_dependency(vm, OCTREE, "octree"); + reduction_option_conflict(vm, OCTREE, "scale"); + reduction_option_conflict(vm, OCTREE, "projection"); + reduction_option_conflict(vm, OCTREE, "width"); + reduction_option_conflict(vm, OCTREE, "height"); + + reduction_option_conflict(vm, RANGE, "voxel"); + reduction_option_conflict(vm, RANGE, "octree"); + reduction_option_dependency(vm, RANGE, "scale"); + reduction_option_dependency(vm, RANGE, "projection"); + reduction_option_dependency(vm, RANGE, "width"); + reduction_option_dependency(vm, RANGE, "height"); + + reduction_option_conflict(vm, INTERPOLATE, "voxel"); + reduction_option_conflict(vm, INTERPOLATE, "octree"); + reduction_option_dependency(vm, INTERPOLATE, "scale"); + reduction_option_dependency(vm, INTERPOLATE, "projection"); + reduction_option_dependency(vm, INTERPOLATE, "width"); + reduction_option_dependency(vm, INTERPOLATE, "height"); + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif +} + +void createdirectory(string dir) +{ + int success = mkdir(dir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); + + if (success == 0 || errno == EEXIST) { + cout << "Writing to " << dir << endl; + } else { + cerr << "Creating directory " << dir << " failed" << endl; + exit(1); + } +} + +void scan2mat(Scan *source, cv::Mat &mat) +{ + DataXYZ xyz = source->get("xyz"); + DataReflectance xyz_reflectance = (((DataReflectance)source->get("reflectance")).size() == 0) ? + source->create("reflectance", sizeof(float)*xyz.size()) + : source->get("reflectance"); + if(((DataReflectance)source->get("reflectance")).size() == 0){ + for(unsigned int i = 0; i < xyz.size(); i++) + xyz_reflectance[i] = 255; + } + unsigned int nPoints = xyz.size(); + mat.create(nPoints,1,CV_32FC(4)); + mat = cv::Scalar::all(0); + cv::MatIterator_ it = mat.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float reflectance = xyz_reflectance[i]; + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; + + (*it)[0] = xyz[i][0]; + (*it)[1] = xyz[i][1]; + (*it)[2] = xyz[i][2]; + (*it)[3] = reflectance; + ++it; + } +} + +void reduce_octree(Scan *scan, vector &reduced_points, int octree, + int red, bool use_reflectance) +{ + if (use_reflectance) { + unsigned int types = PointType::USE_REFLECTANCE; + PointType pointtype(types); + scan->setReductionParameter(red, octree, pointtype); + scan->calcReducedPoints(); + + DataXYZ xyz_reduced(scan->get("xyz reduced")); + DataReflectance reflectance_reduced(scan->get("reflectance reduced")); + + if (xyz_reduced.size() != reflectance_reduced.size()) { + cerr << "xyz_reduced size different than reflectance_reduced size" << endl; + return; + } + + for(unsigned int j = 0; j < xyz_reduced.size(); j++) { + reduced_points.push_back(cv::Vec4f(xyz_reduced[j][0], xyz_reduced[j][1], xyz_reduced[j][2], reflectance_reduced[j])); + } + } + else { + scan->setReductionParameter(red, octree); + scan->calcReducedPoints(); + + DataXYZ xyz_reduced(scan->get("xyz reduced")); + for(unsigned int j = 0; j < xyz_reduced.size(); j++) { + reduced_points.push_back(cv::Vec4f(xyz_reduced[j][0], xyz_reduced[j][1], xyz_reduced[j][2], 0.0)); + } + } +} + +void reduce_range(Scan *scan, vector &reduced_points, int width, + int height, fbr::projection_method ptype, double scale, + bool use_reflectance) +{ + panorama image(width, height, ptype); + cv::Mat mat; + scan2mat(scan, mat); + image.createPanorama(mat); + image.getDescription(); + + cv::Mat range_image_resized; + cv::Mat reflectance_image_resized; + resize(image.getRangeImage(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + if (use_reflectance) { + resize(image.getReflectanceImage(), reflectance_image_resized, + cv::Size(), scale, scale, cv::INTER_NEAREST); + } else { + reflectance_image_resized.create(range_image_resized.size(), CV_8U); + reflectance_image_resized = cv::Scalar::all(0); + } + image.recoverPointCloud(range_image_resized, reflectance_image_resized, reduced_points); +} + +void reduce_interpolation(Scan *scan, vector &reduced_points, + int width, int height, fbr::projection_method ptype, double scale, + bool use_reflectance) +{ + panorama image(width, height, ptype); + cv::Mat mat; + scan2mat(scan, mat); + image.createPanorama(mat); + image.getDescription(); + + cv::Mat range_image_resized; + cv::Mat reflectance_image_resized; + resize(image.getMap(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + if (use_reflectance) { + resize(image.getReflectanceImage(), reflectance_image_resized, + cv::Size(), scale, scale, cv::INTER_NEAREST); + } + for(int i = 0; i < range_image_resized.rows; i++) { + for(int j = 0; j < range_image_resized.cols; j++) { + cv::Vec3f vec = range_image_resized.at(i, j); + if (use_reflectance) { + reduced_points.push_back(cv::Vec4f( + vec[0], vec[1], vec[2], + reflectance_image_resized.at(i, j)/255.0)); + } else { + reduced_points.push_back(cv::Vec4f(vec[0], vec[1], vec[2], 0.0)); + } + } + } +} + +/* + * given a vector of 3d points, write them out as uos files + */ +void write_uos(vector &points, string &dir, string id) +{ + ofstream outfile(dir + "/scan" + id + ".3d"); + + outfile << "# header is ignored" << endl; + + for (vector::iterator it=points.begin(); it < points.end(); it++) { + outfile << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << endl; + } + + outfile.close(); +} + +/* + * given a vector of 3d points, write them out as uosr files + */ +void write_uosr(vector &points, string &dir, string id) +{ + ofstream outfile(dir + "/scan" + id + ".3d"); + + outfile << "# header is ignored" << endl; + + for (vector::iterator it=points.begin(); it < points.end(); it++) { + outfile << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << " " << (*it)[3] << endl; + } + + outfile.close(); +} + +// write .pose files +// .frames files can later be generated from them using ./bin/pose2frames +void writeposefile(string &dir, const double* rPos, const double* rPosTheta, string id) +{ + ofstream posefile(dir + "/scan" + id + ".pose"); + posefile << rPos[0] << " " << rPos[1] << " " << rPos[2] << endl; + posefile << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + posefile.close(); +} + +/** + * Main program for reducing scans. + * Usage: bin/scan_red -r 'dir', + * Use -r for octree based reduction (voxel size=) + * and 'dir' the directory of a set of scans + * Reduced scans will be written to 'dir/reduced' + * + */ +int main(int argc, char **argv) +{ + int start, end; + bool scanserver; + int width, height; + int maxDist, minDist; + fbr::projection_method ptype; + string dir; + IOType iotype; + reduction_method rtype; + double scale, voxel; + int octree; + bool use_reflectance; + + parse_options(argc, argv, start, end, scanserver, width, height, ptype, + dir, iotype, maxDist, minDist, rtype, scale, voxel, octree, + use_reflectance); + + for (int iter = start; iter <= end; iter++) { + + Scan::openDirectory(scanserver, dir, iotype, iter, iter); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + scan->setRangeFilter(maxDist, minDist); + + vector reduced_points; + + string reddir = dir + "reduced"; + createdirectory(reddir); + + switch (rtype) { + case OCTREE: + reduce_octree(scan, reduced_points, octree, voxel, use_reflectance); + break; + case RANGE: + reduce_range(scan, reduced_points, width, height, ptype, scale, use_reflectance); + break; + case INTERPOLATE: + reduce_interpolation(scan, reduced_points, width, height, ptype, scale, use_reflectance); + break; + default: + cerr << "unknown method" << endl; + return 1; + break; + } + + if (use_reflectance) + write_uosr(reduced_points, reddir, scan->getIdentifier()); + else + write_uos(reduced_points, reddir, scan->getIdentifier()); + writeposefile(reddir, scan->get_rPos(), scan->get_rPosTheta(), scan->getIdentifier()); + } + Scan::closeDirectory(); + } +} diff --git a/.svn/pristine/c3/c3a30556c1a43f1d79aef6f34ed7604414218428.svn-base b/.svn/pristine/c3/c3a30556c1a43f1d79aef6f34ed7604414218428.svn-base new file mode 100644 index 0000000..47e2b81 --- /dev/null +++ b/.svn/pristine/c3/c3a30556c1a43f1d79aef6f34ed7604414218428.svn-base @@ -0,0 +1,44 @@ +#ifndef __THERMO_H__ +#define __THERMO_H__ + +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#include +#else +#include +#endif +//#include +#include +#include +using namespace std; +//typedef vector > Float2D[1200][1600]; +typedef vector > Float2D[2592][3888]; + +void calcBoard(double point_array[][2], int board_n, double &x, double &y, double &cx, double &cy, bool pc); +void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, bool quiet); +IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]); +void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color=false); +IplImage* resizeImage(IplImage *source, int scale); +IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale=1); +void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1); +void writeCalibParam(int images, int corner_exp, int board_w, CvMat* image_points, CvSize size, string dir); + +void loadIntrinsicCalibration(CvMat * intrinsic, CvMat * distortion, string dir, bool optical=false) ; +void loadExtrinsicCalibration(CvMat * Translation, CvMat * Rotation, string dir, int method, bool optical=false) ; +void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, +IOType type, int scale, double rot_angle, double minDist, double maxDist, +bool correction, int neighborhood, int method=0); + +bool readPoints(string filename, CvPoint3D32f *corners, int size) ; +void sortElementByElement(CvMat * vectors, int nr_elems, int nr_vectors); +void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat * +points3D, CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, CvMat +* distortions, CvMat * instrinsics, int corners, int successes, string dir, bool quiet=true, string substring = "") ; +void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, int successes, string dir, bool quiet=true, string substring = "") ; +void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical); +void clusterSearch(float ** points, int size, double thresh1, double thresh2, fstream &outfile, bool optical); +void sortDistances(float ** points, int size); +void ExtrCalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1); + + +#endif diff --git a/.svn/pristine/c4/c4451a4e8e457b86f54692055b6f379ba2ad0084.svn-base b/.svn/pristine/c4/c4451a4e8e457b86f54692055b6f379ba2ad0084.svn-base new file mode 100644 index 0000000..34af0a2 --- /dev/null +++ b/.svn/pristine/c4/c4451a4e8e457b86f54692055b6f379ba2ad0084.svn-base @@ -0,0 +1,111 @@ +/* + * history implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#include "history.h" +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +Frame *allocFrame3DData(CvSize pmdSz) { + Frame *f = (Frame*)cvAlloc(sizeof(Frame)); + + f->sz = pmdSz; + f->img = 0; + + f->trn = cvCreateMat(3, 1, CV_32FC1); + f->rot = cvCreateMat(3, 1, CV_32FC1); + + f->status = (CvPoint**) cvAlloc(pmdSz.height * sizeof(CvPoint*)); + f->pts = (CvPoint3D32f**) cvAlloc(pmdSz.height * sizeof(CvPoint3D32f*)); + for(int i = 0; i < pmdSz.height; i++) { + f->status[i] = (CvPoint*) cvAlloc(pmdSz.width * sizeof(CvPoint)); + f->pts[i] = (CvPoint3D32f*) cvAlloc(pmdSz.width * sizeof(CvPoint3D32f)); + } + + return f; +} + +void fillFrame(Frame *f, IplImage *img, CvSize pmdSz, CvPoint3D32f **pts, CvPoint **status + , CvMat *rot, CvMat *trn, double alignError) { + + assert(pmdSz.width == f->sz.width || pmdSz.height == f->sz.height); + + if(f->img) cvCopy(img, f->img, NULL); + else f->img = cvCloneImage(img); + f->alignError = alignError; + + for(int j = 0; j < pmdSz.width; j++) + for(int i = 0; i < pmdSz.height; i++) { + f->status[i][j] = status[i][j]; + f->pts[i][j] = pts[i][j]; + } + + cvCopy(rot, f->rot, NULL); + cvCopy(trn, f->trn, NULL); +} + +void releaseFrame(Frame **f) { + Frame *t = *f; + cvReleaseImage(&t->img); + cvReleaseMat(&t->rot); + cvReleaseMat(&t->trn); + for(int i = 0; i < t->sz.height; i++) { + cvFree(&t->pts[i]); + cvFree(&t->status[i]); + } + cvFree(&t->pts); + cvFree(&t->status); + *f = NULL; +} + + +History *createHistory() { + History *h = (History*)cvAlloc(sizeof(History)); + h->frame = 0; + h->prev = 0; // essential!!! + return h; +} + +History *addFrame(History *h, Frame *f) { + if(!h->frame) { // first frame, FIXME: should be out here + h->prev = 0; // ensure + h->frame = f; + return h; + } + // else + History *n = (History*)cvAlloc(sizeof(History)); + n->prev = h; + n->frame = f; + return n; +} + +void releaseTail(History *h) { + History *tmp = h->prev; + History *prev = 0; + h->prev = 0; + while(tmp) { + prev = tmp->prev; + releaseFrame(&tmp->frame); + cvFree(&tmp); + tmp = prev; + } +} + +void checkHistoryLen(History *h, int maxLen) { + History *histI = h; + for(int i = 0; histI->prev; i++) + if(i > maxLen-2) // -2 because I release *prev* elemnents + releaseTail(histI); + else histI = histI->prev; +} + diff --git a/.svn/pristine/c8/c841b05e4a2414fad6c60fb0fa9638aedbe01e8a.svn-base b/.svn/pristine/c8/c841b05e4a2414fad6c60fb0fa9638aedbe01e8a.svn-base new file mode 100644 index 0000000..a14b237 --- /dev/null +++ b/.svn/pristine/c8/c841b05e4a2414fad6c60fb0fa9638aedbe01e8a.svn-base @@ -0,0 +1,421 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +#include +#include +#include +using namespace std; + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "cvblob.h" + +namespace cvb +{ + + double distantBlobTrack(CvBlob const *b, CvTrack const *t) + { + double d1; + if (b->centroid.xminx) + { + if (b->centroid.yminy) + d1 = MAX(t->minx - b->centroid.x, t->miny - b->centroid.y); + else if (b->centroid.y>t->maxy) + d1 = MAX(t->minx - b->centroid.x, b->centroid.y - t->maxy); + else // if (t->miny < b->centroid.y)&&(b->centroid.y < t->maxy) + d1 = t->minx - b->centroid.x; + } + else if (b->centroid.x>t->maxx) + { + if (b->centroid.yminy) + d1 = MAX(b->centroid.x - t->maxx, t->miny - b->centroid.y); + else if (b->centroid.y>t->maxy) + d1 = MAX(b->centroid.x - t->maxx, b->centroid.y - t->maxy); + else + d1 = b->centroid.x - t->maxx; + } + else // if (t->minx =< b->centroid.x) && (b->centroid.x =< t->maxx) + { + if (b->centroid.yminy) + d1 = t->miny - b->centroid.y; + else if (b->centroid.y>t->maxy) + d1 = b->centroid.y - t->maxy; + else + return 0.; + } + + double d2; + if (t->centroid.xminx) + { + if (t->centroid.yminy) + d2 = MAX(b->minx - t->centroid.x, b->miny - t->centroid.y); + else if (t->centroid.y>b->maxy) + d2 = MAX(b->minx - t->centroid.x, t->centroid.y - b->maxy); + else // if (b->miny < t->centroid.y)&&(t->centroid.y < b->maxy) + d2 = b->minx - t->centroid.x; + } + else if (t->centroid.x>b->maxx) + { + if (t->centroid.yminy) + d2 = MAX(t->centroid.x - b->maxx, b->miny - t->centroid.y); + else if (t->centroid.y>b->maxy) + d2 = MAX(t->centroid.x - b->maxx, t->centroid.y - b->maxy); + else + d2 = t->centroid.x - b->maxx; + } + else // if (b->minx =< t->centroid.x) && (t->centroid.x =< b->maxx) + { + if (t->centroid.yminy) + d2 = b->miny - t->centroid.y; + else if (t->centroid.y>b->maxy) + d2 = t->centroid.y - b->maxy; + else + return 0.; + } + + return MIN(d1, d2); + } + + // Access to matrix +#define C(blob, track) close[((blob) + (track)*(nBlobs+2))] + // Access to accumulators +#define AB(label) C((label), (nTracks)) +#define AT(id) C((nBlobs), (id)) + // Access to identifications +#define IB(label) C((label), (nTracks)+1) +#define IT(id) C((nBlobs)+1, (id)) + // Access to registers +#define B(label) blobs.find(IB(label))->second +#define T(id) tracks.find(IT(id))->second + + void getClusterForTrack(unsigned int trackPos, CvID *close, unsigned int nBlobs, unsigned int nTracks, CvBlobs const &blobs, CvTracks const &tracks, list &bb, list &tt); + + void getClusterForBlob(unsigned int blobPos, CvID *close, unsigned int nBlobs, unsigned int nTracks, CvBlobs const &blobs, CvTracks const &tracks, list &bb, list &tt) + { + for (unsigned int j=0; j1) + { + getClusterForTrack(j, close, nBlobs, nTracks, blobs, tracks, bb, tt); + } + } + } + } + + void getClusterForTrack(unsigned int trackPos, CvID *close, unsigned int nBlobs, unsigned int nTracks, CvBlobs const &blobs, CvTracks const &tracks, list &bb, list &tt) + { + for (unsigned int i=0; i1) + { + getClusterForBlob(i, close, nBlobs, nTracks, blobs, tracks, bb, tt); + } + } + } + } + + void cvUpdateTracks(CvBlobs const &blobs, CvTracks &tracks, const double thDistance, const unsigned int thInactive, const unsigned int thActive) + { + CV_FUNCNAME("cvUpdateTracks"); + __CV_BEGIN__; + + unsigned int nBlobs = blobs.size(); + unsigned int nTracks = tracks.size(); + + // Proximity matrix: + // Last row/column is for ID/label. + // Last-1 "/" is for accumulation. + CvID *close = new unsigned int[(nBlobs+2)*(nTracks+2)]; // XXX Must be same type than CvLabel. + + try + { + // Inicialization: + unsigned int i=0; + for (CvBlobs::const_iterator it = blobs.begin(); it!=blobs.end(); ++it, i++) + { + AB(i) = 0; + IB(i) = it->second->label; + } + + CvID maxTrackID = 0; + + unsigned int j=0; + for (CvTracks::const_iterator jt = tracks.begin(); jt!=tracks.end(); ++jt, j++) + { + AT(j) = 0; + IT(j) = jt->second->id; + if (jt->second->id > maxTrackID) + maxTrackID = jt->second->id; + } + + // Proximity matrix calculation and "used blob" list inicialization: + for (i=0; iinactive++; + track->label = 0; + } + } + + // Detect new tracks + for (i=0; iid = maxTrackID; + track->label = blob->label; + track->minx = blob->minx; + track->miny = blob->miny; + track->maxx = blob->maxx; + track->maxy = blob->maxy; + track->centroid = blob->centroid; + track->lifetime = 0; + track->active = 0; + track->inactive = 0; + tracks.insert(CvIDTrack(maxTrackID, track)); + } + } + + // Clustering + for (j=0; j tt; tt.push_back(T(j)); + list bb; + + getClusterForTrack(j, close, nBlobs, nTracks, blobs, tracks, bb, tt); + + // Select track + CvTrack *track; + unsigned int area = 0; + for (list::const_iterator it=tt.begin(); it!=tt.end(); ++it) + { + CvTrack *t = *it; + + unsigned int a = (t->maxx-t->minx)*(t->maxy-t->miny); + if (a>area) + { + area = a; + track = t; + } + } + + // Select blob + CvBlob *blob; + area = 0; + //cout << "Matching blobs: "; + for (list::const_iterator it=bb.begin(); it!=bb.end(); ++it) + { + CvBlob *b = *it; + + //cout << b->label << " "; + + if (b->area>area) + { + area = b->area; + blob = b; + } + } + //cout << endl; + + // Update track + //cout << "Matching: track=" << track->id << ", blob=" << blob->label << endl; + track->label = blob->label; + track->centroid = blob->centroid; + track->minx = blob->minx; + track->miny = blob->miny; + track->maxx = blob->maxx; + track->maxy = blob->maxy; + if (track->inactive) + track->active = 0; + track->inactive = 0; + + // Others to inactive + for (list::const_iterator it=tt.begin(); it!=tt.end(); ++it) + { + CvTrack *t = *it; + + if (t!=track) + { + //cout << "Inactive: track=" << t->id << endl; + t->inactive++; + t->label = 0; + } + } + } + } + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + for (CvTracks::iterator jt=tracks.begin(); jt!=tracks.end();) + if ((jt->second->inactive>=thInactive)||((jt->second->inactive)&&(thActive)&&(jt->second->activesecond; + tracks.erase(jt++); + } + else + { + jt->second->lifetime++; + if (!jt->second->inactive) + jt->second->active++; + ++jt; + } + } + catch (...) + { + delete[] close; + throw; // TODO: OpenCV style. + } + + delete[] close; + + __CV_END__; + } + + CvFont *defaultFont = NULL; + + void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvFont *font) + { + CV_FUNCNAME("cvRenderTracks"); + __CV_BEGIN__; + + CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3)); + + if ((mode&CV_TRACK_RENDER_ID)&&(!font)) + { + if (!defaultFont) + { + font = defaultFont = new CvFont; + cvInitFont(font, CV_FONT_HERSHEY_DUPLEX, 0.5, 0.5, 0, 1); + // Other fonts: + // CV_FONT_HERSHEY_SIMPLEX, CV_FONT_HERSHEY_PLAIN, + // CV_FONT_HERSHEY_DUPLEX, CV_FONT_HERSHEY_COMPLEX, + // CV_FONT_HERSHEY_TRIPLEX, CV_FONT_HERSHEY_COMPLEX_SMALL, + // CV_FONT_HERSHEY_SCRIPT_SIMPLEX, CV_FONT_HERSHEY_SCRIPT_COMPLEX + } + else + font = defaultFont; + } + + if (mode) + { + for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it) + { + if (mode&CV_TRACK_RENDER_ID) + if (!it->second->inactive) + { + stringstream buffer; + buffer << it->first; + cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.,255.,0.)); + } + + if (mode&CV_TRACK_RENDER_BOUNDING_BOX) + if (it->second->inactive) + cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 50.)); + else + cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 255.)); + + if (mode&CV_TRACK_RENDER_TO_LOG) + { + clog << "Track " << it->second->id << endl; + if (it->second->inactive) + clog << " - Inactive for " << it->second->inactive << " frames" << endl; + else + clog << " - Associated with blob " << it->second->label << endl; + clog << " - Lifetime " << it->second->lifetime << endl; + clog << " - Active " << it->second->active << endl; + clog << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl; + clog << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl; + clog << endl; + } + + if (mode&CV_TRACK_RENDER_TO_STD) + { + cout << "Track " << it->second->id << endl; + if (it->second->inactive) + cout << " - Inactive for " << it->second->inactive << " frames" << endl; + else + cout << " - Associated with blobs " << it->second->label << endl; + cout << " - Lifetime " << it->second->lifetime << endl; + cout << " - Active " << it->second->active << endl; + cout << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl; + cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl; + cout << endl; + } + } + } + + __CV_END__; + } + +} diff --git a/.svn/pristine/c8/c8eb2a0439205605817ebc9bdf56a390b5b19ebd.svn-base b/.svn/pristine/c8/c8eb2a0439205605817ebc9bdf56a390b5b19ebd.svn-base new file mode 100644 index 0000000..0494f6d --- /dev/null +++ b/.svn/pristine/c8/c8eb2a0439205605817ebc9bdf56a390b5b19ebd.svn-base @@ -0,0 +1,42 @@ +Project admins + +Andreas Nuechter andreas@nuechti.de +Kai Lingemann kai.lingemann@gmx.de +Dorit Borrmann d.borrmann@jacobs-university.de + +List of contributors + +Andreas Nuechter andreas@nuechti.de +Kai Lingemann kai.lingemann@gmx.de +Dorit Borrmann d.borrmann@jacobs-university.de +Jan Elseberg j.elseberg@jacobs-university.de +Jochen Sprickerhof jochen@sprickerhof.de +HamidReza Houshiar h.houshiar@jacobs-university.de +Sven Albrecht sven.albrecht@uni-osnabrueck.de +Stan Serebryakov cfr.ssv@gmail.com +Thomas Escher tescher@uni-osnabrueck.de +Thomas Wiemann twiemann@uni-osnabrueck.de +Alexandru Tandrau alexandru@tandrau.com +Alexandru Eugen Ichim eugen@alexichim.com +Flavia Grosan me@flaviagrosan.com +Deyuan Qiu deyuan.qiu@googlemail.com +Darko Makreshanski d.makreshanski@jacobs-university.de +Mohammad Faisal Abdullah m.faisal@jacobs-university.de +Li Ming liming751218@whu.edu.cn +Li Wei xpaulee@gmail.com +Shams Feyzabadi sh.feyzabadi@gmail.co +Vladislav Perelmann v.perelman@jacobs-university.de +Chen Long lchen.whu@gmail.com +Remus Dumitru r.dumitru@jaocbs-university.de +Billy Okal okal.billy@googlemail.com +Razvan-George Mihalyi r.mihalyi@jacobs-university.de +Johannes Schauer j.schauer@jacobs-university.de +Corneliu-Claudiu Prodescu c.prodescu@jacobs-university.de +Vaibhav Kumar Mehta v.metha@jacobs-university.de + +Further contributors + +Uwe Hebbelmann, Sebastian Stock, Andre Schemschat +Hartmut Surmann +Amuz Tamrakars, Ulugbek Makhmudov +Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz diff --git a/.svn/pristine/cb/cb3cc584c42caf6489f18bfea716109231bce12e.svn-base b/.svn/pristine/cb/cb3cc584c42caf6489f18bfea716109231bce12e.svn-base new file mode 100644 index 0000000..1820544 --- /dev/null +++ b/.svn/pristine/cb/cb3cc584c42caf6489f18bfea716109231bce12e.svn-base @@ -0,0 +1,45 @@ +# Copyright (C) 2007 by Cristóbal Carnero Liñán +# grendel.ccl@gmail.com +# +# This file is part of cvBlob. +# +# cvBlob is free software: you can redistribute it and/or modify +# it under the terms of the Lesser GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# cvBlob is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# Lesser GNU General Public License for more details. +# +# You should have received a copy of the Lesser GNU General Public License +# along with cvBlob. If not, see . +# + +IF (WITH_THERMO) + +MESSAGE(STATUS "With cvblob") +find_package(OpenCV REQUIRED) + +set(CVBLOB_SRCS + cvblob.cpp + cvlabel.cpp + cvaux.cpp + cvcontour.cpp + cvtrack.cpp + cvcolor.cpp +) + +set_source_files_properties(${cvBlob_SRC} + PROPERTIES + COMPILE_FLAGS "-O3 ${ADDITIONAL_OPENCV_FLAGS}" +) + +add_library(cvblob STATIC ${CVBLOB_SRCS}) + +IF(EXPORT_SHARED_LIBS) +add_library(cvblob_s SHARED ${CVBLOB_SRCS}) +ENDIF(EXPORT_SHARED_LIBS) + +ENDIF (WITH_THERMO) diff --git a/.svn/pristine/d1/d1ce81a46af93232cc50fbcb32801fc111bdb345.svn-base b/.svn/pristine/d1/d1ce81a46af93232cc50fbcb32801fc111bdb345.svn-base new file mode 100644 index 0000000..e51c1b9 --- /dev/null +++ b/.svn/pristine/d1/d1ce81a46af93232cc50fbcb32801fc111bdb345.svn-base @@ -0,0 +1,296 @@ +/* + * PMDCam implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#include +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include + +#include "cvpmd.h" +#include "pmdWrap.h" + +#include "slam6d/icp6Dhelix.h" +#include "slam6d/icp6Dortho.h" +#include "slam6d/icp6Dquat.h" +#include "slam6d/icp6Dsvd.h" +#include "slam6d/icp6Dapx.h" + + +//TODO: releasePMDCam() + +//TODO: !!! check config parse error !!! TODO + +PMDCam *initPMDCam(const char *confPath) { + PMDCam *pmdc = (PMDCam*)malloc(sizeof(PMDCam)); + config_t *conf = (config_t*)malloc(sizeof(config_t)); + config_init(conf); + config_read_file(conf, confPath); + + config_lookup_bool(conf, "offline", &pmdc->_offlineMode); + if(!pmdc->_offlineMode) { + // init PMDCam + const char *ip; + const char *plugin; + config_lookup_string(conf, "ip", &ip); + config_lookup_string(conf, "plugin", &plugin); + printf("Starting PMD...\n"); + pmdc->_pmd = initPMD(plugin, ip); + //FIXME: pmdc initialization assert + + int camID; +#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \ + || (LIBCONFIG_VER_MAJOR > 1)) + config_lookup_int(conf, "cameraID", &camID); +#else //libconfig API changed in version 1.4b + config_lookup_int(conf, "cameraID", (long *)&camID); +#endif + pmdc->_capture = cvCaptureFromCAM(camID); + if(!pmdc->_capture) fprintf(stderr, "ERROR: Can't initialize capture, see camera id in pmdc.conf.\n"); + } else { + printf("Offline mode.\n"); + const char *camVid; + const char *pmdI; + const char *pmdA; + const char *pmd3D; + const char *headers; + config_lookup_string(conf, "camVideoFile", &camVid); + config_lookup_string(conf, "pmdIFile", &pmdI); + // config_lookup_string(conf, "pmdAFile", &pmdA); + config_lookup_string(conf, "pmd3DFile", &pmd3D); + config_lookup_string(conf, "headersFile", &headers); + //FIXME: check empty strings + printf( "DEBUG: Reading from files: %s, %s, %s.\n" + , camVid, pmdI, pmd3D); + pmdc->_capture = cvCaptureFromFile(camVid); + pmdc->_f.i = fopen(pmdI, "r"); + // pmdc->_f.a = fopen(pmdA, "r"); + pmdc->_f.p = fopen(pmd3D, "r"); + pmdc->_f.h = fopen(headers, "r"); + if( !pmdc->_capture + || !pmdc->_f.i + // || !pmdc->_f.a + || !pmdc->_f.p + || !pmdc->_f.h) fprintf(stderr, "ERROR: Can't open data file, see files settings in pmdc.conf.\n"); + pmdc->header = (ImageHeaderInformation*)cvAlloc(sizeof(ImageHeaderInformation)); + + //TODO: read pmd header somewhere + } + CvSize pmdSz; +#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \ + || (LIBCONFIG_VER_MAJOR > 1)) + config_lookup_int(conf, "pmdSize.width", &pmdSz.width); + config_lookup_int(conf, "pmdSize.height", &pmdSz.height); +#else //libconfig API changed in version 1.4b + config_lookup_int(conf, "pmdSize.width", (long *)&pmdSz.width); + config_lookup_int(conf, "pmdSize.height", (long *)&pmdSz.height); +#endif + printf("DEBUG: pmdSz: %i %i\n", pmdSz.width, pmdSz.height); + pmdc->_iPMDIU = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1); + pmdc->iPMDI = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1); + pmdc->_iPMDAU = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1); + pmdc->iPMDA = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1); + + pmdc->_iCamColorU = cvQueryFrame(pmdc->_capture); + CvSize camSz = cvGetSize(pmdc->_iCamColorU); + printf("DEBUG: camSz: %i %i\n", camSz.width, camSz.height); + + + config_lookup_bool(conf, "hybrid", &pmdc->hybrid); + if(pmdc->hybrid) pmdc->_iCamColorUBuffer = cvCreateImage(camSz, IPL_DEPTH_8U, 3); + + pmdc->iCamColor = cvCreateImage(camSz, IPL_DEPTH_8U, 3); + pmdc->iCam = cvCreateImage(camSz, IPL_DEPTH_8U, 1); + + pmdc->_mapXPMD = cvCreateImage(pmdSz, IPL_DEPTH_32F, 1); + pmdc->_mapYPMD = cvCreateImage(pmdSz, IPL_DEPTH_32F, 1); + pmdc->_mapXCam = cvCreateImage(camSz, IPL_DEPTH_32F, 1); + pmdc->_mapYCam = cvCreateImage(camSz, IPL_DEPTH_32F, 1); + printf("DEBUG: Created images...\n"); + + const char *inc; + const char *dsc; + const char *inp; + const char *dsp; + config_lookup_string(conf, "intrinsicCam", &inc); + config_lookup_string(conf, "distortionCam", &dsc); + config_lookup_string(conf, "intrinsicPMD", &inp); + config_lookup_string(conf, "distortionPMD", &dsp); + pmdc->intrinsicCam = (CvMat*)cvLoad(inc); + pmdc->distortionCam = (CvMat*)cvLoad(dsc); + pmdc->intrinsicPMD = (CvMat*)cvLoad(inp); + pmdc->distortionPMD = (CvMat*)cvLoad(dsp); + if(!pmdc->intrinsicCam || !pmdc->distortionCam || !pmdc->intrinsicPMD || !pmdc->distortionPMD) + fprintf(stderr, "ERROR: Cant load matrix file, see pmdc.conf matrix files settings.\n"); + + //TODO: essential mat + + cvInitUndistortMap(pmdc->intrinsicCam, pmdc->distortionCam, pmdc->_mapXCam, pmdc->_mapYCam); + cvInitUndistortMap(pmdc->intrinsicPMD, pmdc->distortionPMD, pmdc->_mapXPMD, pmdc->_mapYPMD); + printf("DEBUG: initialized undistortion maps...\n"); + + CvPoint3D32f **pmdPts = (CvPoint3D32f**) cvAlloc(pmdSz.height * sizeof(CvPoint3D32f*)); + for(int i = 0; i < pmdSz.height; i++) pmdPts[i] = (CvPoint3D32f*) cvAlloc(pmdSz.width * sizeof(CvPoint3D32f)); + pmdc->pts = pmdPts; + + + config_lookup_float(conf, "tracking.quality", &pmdc->_track.quality); +#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \ + || (LIBCONFIG_VER_MAJOR > 1)) + config_lookup_int(conf, "tracking.min", &pmdc->_track.minFeatures); + config_lookup_int(conf, "tracking.max", &pmdc->_track.maxFeatures); + config_lookup_int(conf, "tracking.minDist", &pmdc->_track.minDist); + config_lookup_int(conf, "tracking.winSize", &pmdc->_track.winSz); + config_lookup_int(conf, "historyLen", &pmdc->historyLen); +#else //libconfig API changed in version 1.4b + config_lookup_int(conf, "tracking.min", (long *)&pmdc->_track.minFeatures); + config_lookup_int(conf, "tracking.max", (long *)&pmdc->_track.maxFeatures); + config_lookup_int(conf, "tracking.minDist", (long *)&pmdc->_track.minDist); + config_lookup_int(conf, "tracking.winSize", (long *)&pmdc->_track.winSz); + config_lookup_int(conf, "historyLen", (long *)&pmdc->historyLen); +#endif + pmdc->_track.trackingFlags = 0; //FIXME, if you init pyrs here + +#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \ + || (LIBCONFIG_VER_MAJOR > 1)) + config_lookup_int(conf, "minPts", &pmdc->minPts4Pose); +#else //libconfig API changed in version 1.4b + config_lookup_int(conf, "minPts", (long *)&pmdc->minPts4Pose); +#endif + config_lookup_float(conf, "maxError", &pmdc->maxError); + + config_lookup_float(conf, "outliersRemoval.sigmaDepth", &pmdc->sigmaDepth); + config_lookup_float(conf, "outliersRemoval.sigmaColor", &pmdc->sigmaColor); + config_lookup_float(conf, "outliersRemoval.threshold", &pmdc->dpThreshold); + + config_lookup_bool(conf, "savePoses", &pmdc->savePoses); + config_lookup_bool(conf, "synchronous", &pmdc->synchronous); + + int icpVerbose; + config_lookup_bool(conf, "icp.verbose", &icpVerbose); + bool icpQuiet = !icpVerbose; + const char *icpMethod; + config_lookup_string(conf, "icp.method", &icpMethod); + printf("%s\n", icpMethod); + if(!strcmp(icpMethod, "helix")) pmdc->icp = new icp6D_HELIX(icpQuiet); + else if(!strcmp(icpMethod, "svd")) pmdc->icp = new icp6D_SVD(icpQuiet); + else if(!strcmp(icpMethod, "apx")) pmdc->icp = new icp6D_APX(icpQuiet); + else if(!strcmp(icpMethod, "quat")) pmdc->icp = new icp6D_QUAT(icpQuiet); + else if(!strcmp(icpMethod, "ortho")) pmdc->icp = new icp6D_ORTHO(icpQuiet); + else { + fprintf(stderr, "ERROR: Uknown ICP method: %s!\n", icpMethod); + exit(1); + } + + //TODO: init tracking? + //TODO: alloc pts + + config_destroy(conf); + return pmdc; +} + + +int grabData(PMDCam *pmdc) { + static unsigned int latestTimestamp = 0; + static unsigned int latestTimestampUsec = 0; + while(1) { // grab data until the new have come + if(!pmdc->_offlineMode) { + pmdUpdate(pmdc->_pmd->hnd); + + pmdc->_iCamColorUBuffer = cvQueryFrame(pmdc->_capture); + + if(pmdc->synchronous) { + pmdRetriveDistances(pmdc->_pmd); + pmdProjectArrayToCartesian(pmdc->_pmd, pmdc->intrinsicPMD, pmdc->pts); + // project array has to be called right after retrive distances + pmdQueryImageAsync(pmdc->_pmd, pmdc->_iPMDIU); + + } else { + pmdRetriveDistancesAsync(pmdc->_pmd); + pmdProjectArrayToCartesian(pmdc->_pmd, pmdc->intrinsicPMD, pmdc->pts); + // project array has to be called right after retrive distances + pmdQueryImageAsync(pmdc->_pmd, pmdc->_iPMDIU); +/* printf("time: %f, utime: %f, valid: %f\n" + , pmdc->header->Seconds + , pmdc->header->Useconds + , pmdc->header->ValidImage + );*/ + + } + + pmdc->header = retriveHeader(); + pmdc->timestamp = (unsigned int)pmdc->header->Seconds; + pmdc->timestampUsec = (unsigned int)pmdc->header->Useconds; + + if(pmdc->hybrid) { + if( pmdc->timestamp > latestTimestamp + || pmdc->timestampUsec > latestTimestampUsec) { + latestTimestamp = pmdc->timestamp; + latestTimestampUsec = pmdc->timestampUsec; + cvCopy(pmdc->_iCamColorUBuffer, pmdc->_iCamColorU); + break; + } else continue; + } else { + pmdc->_iCamColorU = pmdc->_iCamColorUBuffer; + break; + } + } else { // offliine mode + pmdc->_iCamColorUBuffer = cvQueryFrame(pmdc->_capture); + if(!pmdc->_iCamColorUBuffer) return 1; + + CvSize pmdSz = cvGetSize(pmdc->_iPMDIU); + if(fread(pmdc->_iPMDIU->imageData, sizeof(char), pmdSz.width*pmdSz.height, pmdc->_f.i) + < (unsigned)pmdSz.width*pmdSz.height) return 1; + //if(fread(pmdc->_iPMDAU->imageData, sizeof(char), pmdSz.width*pmdSz.height, pmdc->_f.a) + // < (unsigned)pmdSz.width*pmdSz.height) return 1; + for(int i = 0; i < pmdSz.height; i++) + fread(pmdc->pts[i], sizeof(CvPoint3D32f), pmdSz.width, pmdc->_f.p); + + fread(pmdc->header, sizeof(ImageHeaderInformation), 1, pmdc->_f.h); + // i hope CvPt32f contains no holes :) + pmdc->timestamp = (unsigned int)pmdc->header->Seconds; + pmdc->timestampUsec = (unsigned int)pmdc->header->Useconds; + + /*printf("%i %i %i\n", (unsigned int)pmdc->header->Seconds + , (unsigned int)pmdc->header->Useconds + , (unsigned int)pmdc->header->ValidImage);*/ + + + if(pmdc->hybrid) { + if( pmdc->timestamp > latestTimestamp + || pmdc->timestampUsec > latestTimestampUsec) { + latestTimestamp = pmdc->timestamp; + latestTimestampUsec = pmdc->timestampUsec; + cvCopy(pmdc->_iCamColorUBuffer, pmdc->_iCamColorU); + break; + } else continue; + } else { + pmdc->_iCamColorU = pmdc->_iCamColorUBuffer; + break; + } + + } + } + cvFlip(pmdc->_iPMDIU, 0, 1); // flip around x-axes + cvRemap(pmdc->_iPMDIU, pmdc->iPMDI, pmdc->_mapXPMD, pmdc->_mapYPMD); //undistortion + + cvFlip(pmdc->_iCamColorU, 0, -1); // flip around x and y axes + cvRemap(pmdc->_iCamColorU, pmdc->iCamColor, pmdc->_mapXCam, pmdc->_mapYCam); + cvCvtColor(pmdc->iCamColor, pmdc->iCam, CV_BGR2GRAY); + return 0; +} + diff --git a/.svn/pristine/d3/d353dde9df4d61135c94f27d7fcc513bd291b5d2.svn-base b/.svn/pristine/d3/d353dde9df4d61135c94f27d7fcc513bd291b5d2.svn-base new file mode 100644 index 0000000..4e43fdf --- /dev/null +++ b/.svn/pristine/d3/d353dde9df4d61135c94f27d7fcc513bd291b5d2.svn-base @@ -0,0 +1,33 @@ +SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES}) +IF(WIN32) + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt) + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/freeglut.lib XGetopt) + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) +ENDIF(WIN32) +IF (UNIX) + SET(SHOW_LIBS newmat dl ${SHOW_LIBS} ${GLUT_LIBRARIES}) +ENDIF(UNIX) + +IF(WITH_GLEE) + SET(SHOW_LIBS ${SHOW_LIBS} glee) +ENDIF(WITH_GLEE) + +SET(SHOW_SRCS NurbsPath.cc PathGraph.cc vertexarray.cc viewcull.cc colormanager.cc compacttree.cc scancolormanager.cc display.cc) + +IF (WITH_SHOW) + add_executable(show show.cc ${SHOW_SRCS}) + target_link_libraries(show ${SHOW_LIBS}) +ENDIF(WITH_SHOW) + +IF(WITH_WXSHOW) + add_executable(wxshow wxshow.cc selectionframe.cc ${SHOW_SRCS}) + target_link_libraries(wxshow ${wxWidgets_LIBRARIES} wxthings ${SHOW_LIBS}) +ENDIF(WITH_WXSHOW) + +### EXPORT SHARED LIBS +IF(EXPORT_SHARED_LIBS) +add_library(show_s SHARED ${SHOW_SRCS}) +target_link_libraries(show_s newmat_s) +ENDIF(EXPORT_SHARED_LIBS) diff --git a/.svn/pristine/d4/d4a88d17ba01eece10b6c951c1aee3e31db4c0f4.svn-base b/.svn/pristine/d4/d4a88d17ba01eece10b6c951c1aee3e31db4c0f4.svn-base new file mode 100644 index 0000000..b76fd0f --- /dev/null +++ b/.svn/pristine/d4/d4a88d17ba01eece10b6c951c1aee3e31db4c0f4.svn-base @@ -0,0 +1,128 @@ +/* + * grabVideoAnd3D implementation + * + * Copyright (C) Stanislav Serebryakov + * + * Released under the GPL version 3. + * + */ + +#include +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "pmdsdk2.h" +#include "cvpmd.h" + +int main(int argc, char **argv) +{ + int totalFrames = 0; + int ui = 1; + printf("%i\n", argc); + if(argc > 1) { + totalFrames = atoi(argv[1]); + ui = 0; + } + if(argc > 2) ui = 1; + + //FIXME: here is lots of old code, should be removed + + + PMD *pmd = initPMD("../o3d.L32.pcp", "192.168.0.69"); + + CvCapture *capture = cvCaptureFromCAM(1); //FIXME: should be passed via argc + + CvSize pmdSz = pmdGetSize(pmd); + printf("pmd sz: %i x %i\n", pmdSz.width, pmdSz.height); + IplImage *imgCamColor = cvQueryFrame(capture); + IplImage *imgPMD = cvCreateImage(pmdGetSize(pmd), 8, 1); + IplImage *imgPMDA = cvCreateImage(cvGetSize(imgPMD), 8, 1); + IplImage *imgCam = cvCreateImage(cvGetSize(imgCamColor), 8, 1); + CvPoint3D32f **pmdPts = (CvPoint3D32f**) cvAlloc(pmdSz.height * sizeof(CvPoint3D32f*)); + for(int i = 0; i < pmdSz.height; i++) pmdPts[i] = (CvPoint3D32f*) cvAlloc(pmdSz.width * sizeof(CvPoint3D32f)); + + CvMat *intrinsicCam = (CvMat*)cvLoad("../intrinsic-cam-6x4.xml"); + CvMat *distortionCam = (CvMat*)cvLoad("../distortion-cam-6x4.xml"); + CvMat *intrinsicPMD = (CvMat*)cvLoad("../intrinsic-pmd-6x4.xml"); + CvMat *distortionPMD = (CvMat*)cvLoad("../distortion-pmd-6x4.xml"); + if(!intrinsicCam || !distortionCam|| !intrinsicPMD || !distortionPMD) { + fprintf(stderr, "ERROR: can't load intrinsic and/or distortion xml files!\n"); + return 1; + } + + +// FILE *vPMDA = fopen("./s001.arv", "wb"); // ala dot Amplitudes Raw Video NOTE: i'm not sure it is amplitude :P +// FILE *vPMDAf = fopen("./s001.farv", "wb"); // same but Floating + FILE *vPMDI = fopen("./s001.irv", "wb"); // same but Intensities + FILE *vPMDIf = fopen("./s001.firv", "wb"); + FILE *headers = fopen("./s001.head", "wb"); + CvVideoWriter *vCam = cvCreateVideoWriter( "./s001.avi" + , CV_FOURCC('D', 'I', 'V', 'X') + , 25, cvGetSize(imgCam), 1); + + FILE *pmdPtsFile = fopen("./s001.3dp", "w"); + + if(ui) { + cvNamedWindow("Cam", 0); + cvNamedWindow("PMD", 1); + } + + printf("DEBUG: init done, going to grab %i frames.\n", totalFrames); + + int frames = 0; + while(1) { + frames++; + if(0 == frames % 100) printf("%i frames grabbed...\n", frames); + // Image retriving + pmdUpdate(pmd->hnd); + imgCamColor = cvQueryFrame(capture); + + pmdQueryImageAsync(pmd, imgPMD); + fwrite(pmdDataPtr(pmd), sizeof(float), pmdSz.width*pmdSz.height, vPMDIf); + + //pmdQueryAmplitudesAsync(pmd, imgPMDA); + //fwrite(pmdDataPtr(pmd), sizeof(float), pmdSz.width*pmdSz.height, vPMDAf); + + + pmdRetriveDistancesAsync(pmd); + pmdProjectArrayToCartesian(pmd, intrinsicPMD, pmdPts); + + ImageHeaderInformation *header = retriveHeader(); + + + if(ui) { + cvShowImage("Cam", imgCamColor); + cvShowImage("PMD", imgPMD); + } + + //FIXME: order col/str + for(int i = 0; i < pmdSz.height; i++) + fwrite(pmdPts[i], sizeof(CvPoint3D32f), pmdSz.width, pmdPtsFile); + + fwrite(imgPMD->imageData, sizeof(char), pmdSz.width*pmdSz.height, vPMDI); + //fwrite(imgPMDA->imageData, sizeof(char), pmdSz.width*pmdSz.height, vPMDA); + fwrite(header, sizeof(ImageHeaderInformation), 1, headers); + + cvWriteFrame(vCam, imgCamColor); + + if(totalFrames && (frames >= totalFrames)) break; + if(ui) if(27 == cvWaitKey(5)) break; + } +// fclose(vPMDA); +// fclose(vPMDAf); + fclose(vPMDI); + fclose(vPMDIf); + fclose(pmdPtsFile); + fclose(headers); + printf("grabbed %i frames.\n", frames); + printf("See s001* files (you want to rename them).\n"); +} + diff --git a/.svn/pristine/d9/d946d0746e8afad357b7c039e3e297cd2d38852a.svn-base b/.svn/pristine/d9/d946d0746e8afad357b7c039e3e297cd2d38852a.svn-base new file mode 100644 index 0000000..a9ede6d --- /dev/null +++ b/.svn/pristine/d9/d946d0746e8afad357b7c039e3e297cd2d38852a.svn-base @@ -0,0 +1,118 @@ +### TOOLS + +IF(WITH_TOOLS) + FIND_PACKAGE(OpenCV REQUIRED) + ### SCAN_RED + add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc) + + IF(UNIX) + target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS} ${Boost_LIBRARIES}) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(scan_red scan ANN XGetopt) + ENDIF(WIN32) + + ### SCAN_DIFF + add_executable(scan_diff scan_diff.cc) + # add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc) + + IF(UNIX) + target_link_libraries(scan_diff scan dl ANN) + # target_link_libraries(scan_diff2d scan dl ANN) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(scan_diff scan ANN XGetopt) + # target_link_libraries(scan_diff2d scan ANN XGetopt) + ENDIF(WIN32) + + add_executable(frame_to_graph frame_to_graph.cc) + add_executable(convergence convergence.cc) + add_executable(graph_balancer graph_balancer.cc) + add_executable(exportPoints exportPoints.cc) + add_executable(frames2riegl frames2riegl.cc) + add_executable(frames2pose frames2pose.cc) + add_executable(pose2frames pose2frames.cc) + add_executable(riegl2frames riegl2frames.cc) + add_executable(toGlobal toGlobal.cc) + + IF(UNIX) + target_link_libraries(graph_balancer scan ${Boost_GRAPH_LIBRARY} ${Boost_SERIALIZATION_LIBRARY} ${Boost_REGEX_LIBRARY}) + target_link_libraries(exportPoints scan dl ANN) + target_link_libraries(toGlobal scan) + ENDIF(UNIX) + + + IF (WIN32) + target_link_libraries(frame_to_graph XGetopt ${Boost_LIBRARIES}) + target_link_libraries(convergence XGetopt ${Boost_LIBRARIES}) + target_link_libraries(graph_balancer scan XGetopt ${Boost_LIBRARIES}) + target_link_libraries(exportPoints scan ANN XGetopt ${Boost_LIBRARIES}) + target_link_libraries(frames2pose XGetopt ${Boost_LIBRARIES}) + target_link_libraries(pose2frames XGetopt ${Boost_LIBRARIES}) + target_link_libraries(frames2riegl XGetopt ${Boost_LIBRARIES}) + target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES}) + target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES}) + ENDIF(WIN32) +ENDIF(WITH_TOOLS) + +### SCANLIB + +SET(SCANLIB_SRCS + kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc + graph.cc icp6Dapx.cc icp6D.cc icp6Dsvd.cc + icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc + icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc + ghelix6DQ2.cc gapx6D.cc graphToro.cc ann_kd.cc + graphHOG-Man.cc elch6D.cc elch6Dquat.cc elch6DunitQuat.cc + elch6Dslerp.cc elch6Deuler.cc loopToro.cc loopHOG-Man.cc + point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc + allocator.cc + scan.cc basicScan.cc managedScan.cc metaScan.cc + io_types.cc io_utils.cc pointfilter.cc + ) + +if(WITH_METRICS) + set(SCANLIB_SRCS ${SCANLIB_SRCS} metrics.cc) +endif(WITH_METRICS) + +add_library(scan STATIC ${SCANLIB_SRCS}) + +target_link_libraries(scan scanclient scanio) + +IF(UNIX) + target_link_libraries(scan dl) +ENDIF(UNIX) + +### EXPORT SHARED LIBS + +IF(EXPORT_SHARED_LIBS) +add_library(scan_s SHARED ${SCANLIB_SRCS}) +#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat) +target_link_libraries(scan_s newmat_s sparse_s ANN_s ) +ENDIF(EXPORT_SHARED_LIBS) + +### SLAM6D + +IF(WITH_CUDA) + CUDA_COMPILE(CUDA_FILES cuda/CIcpGpuCuda.cu ) + add_executable(slam6D slam6D.cc cuda/icp6Dcuda.cc ${CUDA_FILES}) + target_link_libraries(slam6D ${CUDA_LIBRARIES} ANN cudpp64) + CUDA_ADD_CUBLAS_TO_TARGET(slam6D) + CUDA_ADD_CUTIL_TO_TARGET(slam6D) +ELSE(WITH_CUDA) + add_executable(slam6D slam6D.cc) +ENDIF(WITH_CUDA) + +IF(UNIX) + target_link_libraries(slam6D scan newmat sparse ANN) +ENDIF(UNIX) + +IF(WIN32) + target_link_libraries(slam6D scan newmat sparse ANN XGetopt ${Boost_LIBRARIES}) +ENDIF(WIN32) + +#IF(MSVC) +# INSTALL(TARGETS slam6D RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/windows) +#ENDIF(MSVC) diff --git a/.svn/pristine/da/da4d3508c0d27871a3aaa14a21c06aee5adf99b2.svn-base b/.svn/pristine/da/da4d3508c0d27871a3aaa14a21c06aee5adf99b2.svn-base new file mode 100644 index 0000000..822a845 --- /dev/null +++ b/.svn/pristine/da/da4d3508c0d27871a3aaa14a21c06aee5adf99b2.svn-base @@ -0,0 +1,2466 @@ +#include +#include "thermo/thermo.h" +#include "newmat/newmatap.h" +using namespace NEWMAT; + +#include "cvblob.h" +using namespace cvb; + +#include + +#ifndef _MSC_VER +#include +#include +#else +#include "XGetopt.h" +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#include +#include +#endif + +#ifdef _EiC +#define WIN32 +#endif + +Float2D data1; +Float2D data2; + + +unsigned int BLOB_SIZE = 65; +double AVG_THRES = 0.8; +unsigned int GRAY_TH = 65; +/** + * Calculates the PCA of a two-dimensional point cloud + * @param x x coordinate of the axis + * @param y y coordinate of the axis + * @param pc true if the principal axis is wanted, false for the least dominant + * @param cx center x of the point cloud + * @param cy center y of the point cloud + */ + +void calcBoard(double point_array[][2], int board_n, double &x, double &y, double &cx, double &cy, bool pc) { + cx = cy = 0; + for (int a = 0; a < board_n; a++) { + cx += point_array[a][0]; + cy += point_array[a][1]; + } + + cx /= board_n; + cy /= board_n; + + SymmetricMatrix A(2); + A = 0; + + for(int a = 0; a < board_n; a++) { + A(1,1) += (point_array[a][0] - cx)*(point_array[a][0] - cx); + A(2,2) += (point_array[a][1] - cy)*(point_array[a][1] - cy); + A(1,2) += (point_array[a][0] - cx)*(point_array[a][1] - cy); + } + DiagonalMatrix D; + Matrix V; + try { + Jacobi(A,D,V); + } catch (ConvergenceException) { + cout << "couldn't find board..." << endl; + } + + int min, max; + + D.MaximumAbsoluteValue1(max); + D.MinimumAbsoluteValue1(min); + + // return eigenvector with highest eigenvalue + if(pc) { + x = V(1,max); + y = V(2,max); + // return eigenvector with lowest eigenvalue + } else { + x = V(1,min); + y = V(2,min); + } + +} + +/** + * Sorts the detected light bulbs on the board. + * @param point_array list of detected points + * @param board_n number of lightbulbs + * @param board_h number of rows + * @param board_w number of columns + * @param quiet if true, debug information is printed + */ +void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, bool quiet) { + double x, y, cx, cy; + // align board using PCA + calcBoard(point_array, board_n, x, y, cx, cy, board_h <= board_w); + double point_array2[board_n][2]; + double angle = -atan2(y,x); + for(int i = 0; i < board_n; i++) { + double tmpx = point_array[i][0] - cx; + double tmpy = point_array[i][1] - cy; + point_array2[i][0] = tmpx * cos(angle) - tmpy * sin(angle); + point_array2[i][1] = tmpx * sin(angle) + tmpy * cos(angle); + } + // sorting the points on the basis of y coordinate////// + int swapped1 = 0; + do { + swapped1 = 0; + + for (int a = 1; a <= board_n - 1; a++) { + if (point_array2[a][1] < point_array2[a - 1][1]) { + //rotated points + double tempx = point_array2[a][0]; + double tempy = point_array2[a][1]; + point_array2[a][0] = point_array2[a - 1][0]; + point_array2[a][1] = point_array2[a - 1][1]; + point_array2[a - 1][0] = tempx; + point_array2[a - 1][1] = tempy; + + //original points + double tmpx = point_array[a][0]; + double tmpy = point_array[a][1]; + point_array[a][0] = point_array[a - 1][0]; + point_array[a][1] = point_array[a - 1][1]; + point_array[a - 1][0] = tmpx; + point_array[a - 1][1] = tmpy; + swapped1 = 1; + } + } + } while (swapped1 == 1); + + if(!quiet) { + cout << "sorted array:" << endl; + for (int f = 0; f < board_n; f++) { + cout << point_array2[f][0] << " " << point_array2[f][1] << endl; + } + } + // sorting the array rows now + for (int x = 0; x < board_h; x++) { + double row_points[board_w][2]; + double row_points2[board_w][2]; + for (int y = 0; y < board_w; y++) { + row_points[y][0] = point_array[x * board_w + y][0]; + row_points[y][1] = point_array[x * board_w + y][1]; + row_points2[y][0] = point_array2[x * board_w + y][0]; + row_points2[y][1] = point_array2[x * board_w + y][1]; + if(!quiet) cout << row_points[y][0] << " " << row_points[y][1] << " "; + } + if(!quiet) cout << endl; + int swapped = 0; + do { + swapped = 0; + for (int a = 1; a <= board_w - 1; a++) { + if (row_points2[a][0] < row_points2[a - 1][0]) { + // original points + double tempx = row_points[a][0]; + double tempy = row_points[a][1]; + row_points[a][0] = row_points[a - 1][0]; + row_points[a][1] = row_points[a - 1][1]; + row_points[a - 1][0] = tempx; + row_points[a - 1][1] = tempy; + // rotated points + double tmpx = row_points2[a][0]; + double tmpy = row_points2[a][1]; + row_points2[a][0] = row_points2[a - 1][0]; + row_points2[a][1] = row_points2[a - 1][1]; + row_points2[a - 1][0] = tmpx; + row_points2[a - 1][1] = tmpy; + swapped = 1; + } + } + } while (swapped == 1); + if(!quiet) cout << "sorted:" << endl; + for (int z = 0; z < board_w; z++) { + point_array2[x * board_w + z][0] = row_points2[z][0]; + point_array2[x * board_w + z][1] = row_points2[z][1]; + if(!quiet) cout << point_array[x * board_w + z][0] << " " << point_array[x * board_w + z][1] << " "; + point_array[x * board_w + z][0] = row_points[z][0]; + point_array[x * board_w + z][1] = row_points[z][1]; + } + if(!quiet) cout << endl; + + } + +} + +/** + * Detects the blobs. + */ +IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]) { + + IplImage *gray_image = cvCloneImage(org_image); + cvThreshold(gray_image, gray_image, GRAY_TH, 255, CV_THRESH_BINARY); + IplImage *labelImg = cvCreateImage(cvGetSize(gray_image), IPL_DEPTH_LABEL, 1); + + // detect blobs + CvBlobs blobs; + cvLabel(gray_image, labelImg, blobs); + double average_size = 0; + int count = 0; + for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { + if (it->second->area < BLOB_SIZE) { + count++; + average_size += it->second->area; + } + } + if(!quiet) cout << "centroid:" << average_size << endl; + + // refine blobs + average_size = average_size / count; + double average_size_min = average_size * (1.0 - AVG_THRES); + double average_size_max = average_size * (1.0 + AVG_THRES); + int blob_count = 0; + + for (CvBlobs::const_iterator it2 = blobs.begin(); it2 != blobs.end(); ++it2) { + if (it2->second->area >= average_size_min && + it2->second->area <= average_size_max && + blob_count < corner_exp) { + + point_array2[blob_count][0] = it2->second->centroid.x; + point_array2[blob_count][1] = it2->second->centroid.y; + double sumx = 0.0; + double sumy = 0.0; + double sum = 0.0; + + /* + int step = 5; + int minx = ((int)it2->second->minx - step) > -1 ? (it2->second->minx - step) : 0; + int maxx = ((it2->second->maxx + step) < gray_image->width) ? (it2->second->maxx + step) : (gray_image->width - 1); + int miny = ((int)it2->second->miny - step) > -1 ? (it2->second->miny - step) : 0; + int maxy = ((it2->second->maxy + step) < gray_image->height) ? (it2->second->maxy + step) : (gray_image->height - 1); + */ + + int minx = it2->second->minx; + int miny = it2->second->miny; + int maxx = it2->second->maxx; + int maxy = it2->second->maxy; + + for(int x = minx; x <= maxx; x++) { + for(int y = miny; y <= maxy; y++) { + if(cvGet2D(gray_image, y, x).val[0] > 0) { + CvScalar c; + c = cvGet2D(org_image, y, x); + sum += c.val[0]; + sumx += c.val[0]*x; + sumy += c.val[0]*y; + } + } + } + + sumx /= sum; + sumy /= sum; + point_array2[blob_count][0] = sumx; + point_array2[blob_count][1] = sumy; + + blob_count++; + } + } + + if(!quiet) cout << "Refined number of blobs=" << blob_count << endl; + // sorting the points + sortBlobs(point_array2, corner_exp, board_h, board_w, true); + cvReleaseImage(&labelImg); + corner_exp = blob_count; + return gray_image; +} + +/** + * Connects the detected calibration features in the image with lines. + */ +void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color) { + for (int i = 0; i <= corner_exp - 2; i++) { + CvPoint pt1; + CvPoint pt2; + CvScalar s; + if(color) { + s = CV_RGB(255,0,0); + } else { + s.val[0] = 100; + } + double temp1 = point_array2[i][0] - floor(point_array2[i][0]); + if (temp1 < .5) { + pt1.x = floor(point_array2[i][0]); + } else { + pt1.x = floor(point_array2[i][0]) + 1; + } + double temp2 = point_array2[i][1] - floor(point_array2[i][1]); + if (temp2 < .5) { + pt1.y = floor(point_array2[i][1]); + } else { + pt1.y = floor(point_array2[i][1]) + 1; + } + double temp3 = point_array2[i + 1][0] - floor( + point_array2[i + 1][0]); + if (temp3 < .5) { + pt2.x = floor(point_array2[i + 1][0]); + } else { + pt2.x = floor(point_array2[i + 1][0]) + 1; + } + double temp4 = point_array2[i + 1][1] - floor( + point_array2[i + 1][1]); + if (temp4 < .5) { + pt2.y = floor(point_array2[i + 1][1]); + } else { + pt2.y = floor(point_array2[i + 1][1]) + 1; + } + cvLine(image, pt1, pt2, s, 3, 8); + } + cvShowImage("Final Result", image); + +} + +/** + * Resizes the image + */ +IplImage* resizeImage(IplImage *source, int scale) { + int width, height; + IplImage *image; + switch(scale) { + case 2: + width = 1200; + height = 900; + break; + case 3: + width = 800; + height = 600; + break; + case 4: + width = 400; + height = 300; + break; + case 5: + width = 160; + height = 120; + break; + case 1: + default: + return cvCloneImage(source); + } + + image = cvCreateImage(cvSize(width,height),source->depth,source->nChannels); + cvResize(source,image); + return image; +} + +/** + * Detects the corners of the chessboard pattern. + */ +IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale) { + + cout << "Scale: " << scale << endl; + IplImage *image = resizeImage(orgimage, scale); + CvSize size = cvGetSize(image); + CvPoint2D32f* corners = new CvPoint2D32f[corner_exp]; + CvSize board_sz = cvSize(board_w, board_h); + IplImage *gray_image = cvCreateImage(size,8,1); + + if (image->nChannels == 3) { + cvCvtColor(image, gray_image, CV_BGR2GRAY); + } else { + gray_image = image; + } + + int found = cvFindChessboardCorners(image, board_sz, corners, &corner_exp, + CV_CALIB_CB_ADAPTIVE_THRESH + | CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS); + + cout << "found corners:" << corner_exp << endl; + if (found != 0) {//if all corners found successfully + //Get Subpixel accuracy on those corners + if(size.width > 400) { + cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(11, 11), cvSize(-1, -1), + cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + } + else { + cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(3, 3), cvSize(-1, -1), + cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + } + } + + for (int i = 0; i < corner_exp; i++) { + point_array2[i][0] = corners[i].x; + point_array2[i][1] = corners[i].y; + } + return gray_image; + +} + +/** + * Writes the intrinsic calibration parameters to files. + */ +void writeCalibParam(int images, int corner_exp, int board_w, CvMat* +image_points, CvSize size, string dir, string substring) { + CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1); + CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1); + //ALLOCATE MATRICES ACCORDING TO HOW MANY CHESSBOARDS FOUND + CvMat* object_points2 = cvCreateMat(images * corner_exp, 3, CV_32FC1); + CvMat* image_points2 = cvCreateMat(images * corner_exp, 2, CV_32FC1); + CvMat* point_counts2 = cvCreateMat(images, 1, CV_32SC1); + CvMat* Rotation = cvCreateMat(images, 3, CV_32FC1); + CvMat* Translation = cvCreateMat(images, 3, CV_32FC1); + //TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES + int j; + for (int i = 0; i < images * corner_exp; ++i) { + j = i % corner_exp; + CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0); + CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1); + CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; + CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; + CV_MAT_ELEM( *object_points2, float, i, 2) = 0.0f; + } + for (int i = 0; i < images; ++i) { //These are all the same number + CV_MAT_ELEM( *point_counts2, int, i, 0) = corner_exp; + } + + // Initialize the intrinsic matrix with focal length = 1.0 + CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f; + CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f; + //CALIBRATE THE CAMERA! + cvCalibrateCamera2(object_points2, image_points2, point_counts2, size, + intrinsic_matrix, distortion_coeffs, Rotation, Translation, 0 + ); + + // SAVE AND PRINT THE INTRINSICS AND DISTORTIONS + string file = dir + "Intrinsics" + substring + ".xml"; + cvSave(file.c_str(), intrinsic_matrix); + file = dir + "Distortion" + substring + ".xml"; + cvSave(file.c_str(), distortion_coeffs); + cout << "Camera Intrinsic Matrix is:" << endl; + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + cout << CV_MAT_ELEM( *intrinsic_matrix, float, row, col ) << "\t"; + } + cout << endl; + } + cout << "Distortion Coefficients are:" << endl; + for (int row = 0; row < 5; row++) { + for (int col = 0; col < 1; col++) { + cout << CV_MAT_ELEM( *distortion_coeffs, float, row, col ) << "\t"; + } + } + cout << endl; + CvMat *intrinsic = intrinsic_matrix; + CvMat *distortion = distortion_coeffs; + + // CLEANUP + cvReleaseMat(&object_points2); + cvReleaseMat(&image_points2); + cvReleaseMat(&point_counts2); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); +} + +/** + * Main function for intrinsic calibration + */ +void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool +chess, bool quiet, string dir, int scale) { + cvNamedWindow("Original Image", 0); + cvNamedWindow("Final Result", 0); + cvResizeWindow( "Original Image", 480, 640 ); + cvResizeWindow( "Final Result", 480, 640 ); + int nr_img = end - start + 1; + if (nr_img == 0) { + cout << "ImageCount is zero!" << endl; + return; + } + + int corner_exp = board_w * board_h; + CvSize board_sz = cvSize(board_w, board_h); + CvSize size; + //ALLOCATE STORAGE(depending upon the number of images in(in case if command line arguments are given ) + //not on the basis of number of images in which all corner extracted/while in the other case the number is the same ) + CvMat* image_points = cvCreateMat(nr_img * corner_exp, 2, CV_32FC1); + //TODO CvPoint2D32f* corners = new CvPoint2D32f[ board_n ]; + + int successes = 0; + int step = 0; + + for (int count = start; count <= end; count++) { + cout << "count : " << count << endl; + string t; + string t1; + + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + t = dir + "/photo" + to_string(count, 3) + ".jpg"; + //t = dir + to_string(count, 3) + "/photo" + to_string(count, 3) + ".ppm"; + } else { + //t = dir + to_string(count, 3) + "/image" + to_string(count, 3) + ".ppm"; + t = dir + "/image" + to_string(count, 3) + ".ppm"; + } + cout << t << endl; + //loading images and finding corners + IplImage* image1 = cvLoadImage(t.c_str(), -1); + if (!image1) { + cout << "image cannot be loaded" << endl; + return; + } + cvShowImage("Original Image", image1); + + ///////////////////////////////////////////////////////////// + double point_array2[corner_exp][2]; + IplImage *image; + + int tmp_corners = corner_exp; + if(chess) { + cout << "detect corners" << endl; + image = detectCorners(image1, tmp_corners, board_h, board_w, quiet, point_array2, scale); + } else { + cout << "detect blob" << endl; + image = detectBlobs(image1, tmp_corners, board_h, board_w, quiet, point_array2); + } + + /* + for(int i = 0; i < corner_exp; i++) { + cout << (float) point_array2[i][0] << " " << (float) point_array2[i][1] << + endl; + } + + //drawing the lines on the image now + */ + drawLines(point_array2, corner_exp, image); + //cvDrawChessboardCorners(image, size, point_array2, tmp_corners, true); + //cvShowImage("Final Result", image); + char in; + if(tmp_corners == corner_exp) { + cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; + in = 'y'; + /* + int c = cvWaitKey(100); + if (c == 27) { + break; + } + cin >> in; + */ + if (in == 'y') { + //cvSaveImage(t1.c_str(), image); + size = cvGetSize(image); + step = successes * corner_exp; + //appending corner data to a generic data structure for all images + for (int i = step, j = 0; j < corner_exp; ++i, ++j) { + CV_MAT_ELEM(*image_points, float,i,0) = (float) point_array2[j][0]; + CV_MAT_ELEM(*image_points, float,i,1) = (float) point_array2[j][1]; + } + successes++; + } else if(in == 'x') { + break; + } + } + cvReleaseImage(&image); + cvReleaseImage(&image1); + + } + cout << "Images for which all corners were found successfully=" + << successes << endl; + if (successes == 0) { + cout << "No successful corners found from any image" << endl; + return; + } + + string substring = optical? "Optical" : ""; + writeCalibParam(successes, corner_exp, board_w, image_points, size, dir, substring); + + cvReleaseMat(&image_points); +} + +/** + * Reads the 3D information of the features from a file. + */ +bool readPoints(string filename, CvPoint3D32f *corners, int size) { + ifstream infile(filename.c_str(), ios::in); + if (!infile) { + cout << "3Ddata file cannot be loaded" << endl; + return false; + } + + string verify; + infile >> verify; + if(strcmp(verify.c_str(), "failed") == 0) return false; + for(int l = 0; l < size; l++) { + infile >> corners[l].y; + infile >> corners[l].z; + infile >> corners[l].x; + corners[l].y = -corners[l].y; + } + return true; +} + +/** + * Calculates the median of a set of translation vectors, i.e., the translation + * that has the smallest distance to all other translation. + */ +int realMedian(CvMat * vectors, int nr_vectors) { + double distances[nr_vectors]; + + for(int i = 0; i < nr_vectors; i++) { + double sum = 0; + double x1 = (CV_MAT_ELEM(*vectors,float,i,0)); + double y1 = (CV_MAT_ELEM(*vectors,float,i,1)); + double z1 = (CV_MAT_ELEM(*vectors,float,i,2)); + for(int j = 0; j < nr_vectors; j++) { + double x2 = (CV_MAT_ELEM(*vectors,float,j,0)); + double y2 = (CV_MAT_ELEM(*vectors,float,j,1)); + double z2 = (CV_MAT_ELEM(*vectors,float,j,2)); + double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); + sum += sqrt(tmp); + } + distances[i] = sum; + } + int min_pos = -1; + double min_dist = DBL_MAX; + for(int i = 0; i < nr_vectors; i++) { + if(distances[i] < min_dist) { + min_pos = i; + min_dist = distances[i]; + } + } + + return min_pos; +} + +/* + * Calculates the median of a set of vectors by iteratively calculating the + * median and cropping outliers. + */ +void filterMedian(CvMat * vectors, int nr_vectors, int thres, CvMat * mean) { + int threshold = thres; + // calculate Median + int min_pos = realMedian(vectors, nr_vectors); + if(thres == 4) { + (CV_MAT_ELEM(*mean,float,0,0)) = (CV_MAT_ELEM(*vectors,float,min_pos,0)); + (CV_MAT_ELEM(*mean,float,0,1)) = (CV_MAT_ELEM(*vectors,float,min_pos,1)); + (CV_MAT_ELEM(*mean,float,0,2)) = (CV_MAT_ELEM(*vectors,float,min_pos,2)); + (CV_MAT_ELEM(*mean,float,0,3)) = (CV_MAT_ELEM(*vectors,float,min_pos,3)); + (CV_MAT_ELEM(*mean,float,0,4)) = (CV_MAT_ELEM(*vectors,float,min_pos,4)); + (CV_MAT_ELEM(*mean,float,0,5)) = (CV_MAT_ELEM(*vectors,float,min_pos,5)); + return; + } + + // crop outliers + double x1 = (CV_MAT_ELEM(*vectors,float,min_pos,0)); + double y1 = (CV_MAT_ELEM(*vectors,float,min_pos,1)); + double z1 = (CV_MAT_ELEM(*vectors,float,min_pos,2)); + + int count = 0; + for(int i = 0; i < nr_vectors; i++) { + double x2 = (CV_MAT_ELEM(*vectors,float,i,0)); + double y2 = (CV_MAT_ELEM(*vectors,float,i,1)); + double z2 = (CV_MAT_ELEM(*vectors,float,i,2)); + double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); + if(sqrt(tmp) < 1.0/threshold) count++; + } + + CvMat* some_vectors = cvCreateMat(count, 6, CV_32FC1); + count = 0; + for(int i = 0; i < nr_vectors; i++) { + double x2 = (CV_MAT_ELEM(*vectors,float,i,0)); + double y2 = (CV_MAT_ELEM(*vectors,float,i,1)); + double z2 = (CV_MAT_ELEM(*vectors,float,i,2)); + double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); + if(sqrt(tmp) < 1.0/threshold) { + for(int j = 0; j < 6; j++) { + (CV_MAT_ELEM(*some_vectors,float,count,j)) = (CV_MAT_ELEM(*vectors,float,i,j)); + cout << (CV_MAT_ELEM(*some_vectors,float,count,j)) << " "; + } + cout << endl; + count++; + } + } + cout << "min_pos " << min_pos << endl; + // recurse + if(thres < 3) { + filterMedian(some_vectors, count, ++threshold, mean); + cvReleaseMat(&some_vectors); + // determine result + } else { + /* + x1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,0)); + y1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,1)); + z1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,2)); + */ + double x2 = 0; + double y2 = 0; + double z2 = 0; + double r1 = 0; + double r2 = 0; + double r3 = 0; + for(int i = 0; i < count; i++) { + x2 += (CV_MAT_ELEM(*some_vectors,float,i,0)); + y2 += (CV_MAT_ELEM(*some_vectors,float,i,1)); + z2 += (CV_MAT_ELEM(*some_vectors,float,i,2)); + r1 += (CV_MAT_ELEM(*some_vectors,float,i,3)); + r2 += (CV_MAT_ELEM(*some_vectors,float,i,4)); + r3 += (CV_MAT_ELEM(*some_vectors,float,i,5)); + } + (CV_MAT_ELEM(*mean,float,0,0)) = x2/count; + (CV_MAT_ELEM(*mean,float,0,1)) = y2/count; + (CV_MAT_ELEM(*mean,float,0,2)) = z2/count; + (CV_MAT_ELEM(*mean,float,0,3)) = r1/count; + (CV_MAT_ELEM(*mean,float,0,4)) = r2/count; + (CV_MAT_ELEM(*mean,float,0,5)) = r3/count; + } +} + +/** + * Sorts vectors element by element, enables one to calculate the median of + * each element separately. + */ +void sortElementByElement(CvMat * vectors, int nr_elems, int nr_vectors) { + bool swapped; + for (int i = 0; i < nr_elems; i++) { + do { + swapped = false; + for (int j = 1; j <= nr_vectors - 1; j++) { + if (CV_MAT_ELEM(*vectors,float,j,i) < CV_MAT_ELEM(*vectors,float,j-1,i)) { + float temp = CV_MAT_ELEM(*vectors,float,j,i); + CV_MAT_ELEM(*vectors,float,j,i) = CV_MAT_ELEM(*vectors,float,j-1,i); + CV_MAT_ELEM(*vectors,float,j-1,i) = temp; + swapped = true; + } + } + } while (swapped); + } +} + +/** + * Calculates the extrinsic parameters of a set of matches. Find the best match + * by calculating the reprojection error of each set of calibration parameters. + */ +void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat * + points3D, CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, CvMat + * distortion, CvMat * intrinsics, int corners, int successes, string dir, bool quiet, string substring) { + double reprojectionError[successes]; + for(int i = 0; i < successes; i++) { + reprojectionError[i] = 0.0; + } + + cout << "reprojectionError" << endl; + for(int i = 0; i < successes + 4; i++) { + reprojectionError[i] = 0.0; + CvMat * rotation = cvCreateMat(1, 3, CV_32FC1); + CvMat * translation = cvCreateMat(1, 3, CV_32FC1); + if(i==successes) cout << "now other stuff" << endl; + for(int k = 0; k < 3; k++) { + CV_MAT_ELEM(*rotation, float, 0, k) = CV_MAT_ELEM(*rotation_vectors_temp, float, i, k); + CV_MAT_ELEM(*translation, float, 0, k) = CV_MAT_ELEM(*translation_vectors_temp, float, i, k); + cerr << CV_MAT_ELEM(*translation, float, 0, k)<< " "; + } + for(int k = 0; k < 3; k++) { + cerr << CV_MAT_ELEM(*rotation, float, 0, k)<< " "; + } + cerr << endl; + for(int j = 0; j < successes; j++) { + double tmp = 0; + //calculate reprojection error + CvMat * point_3Dcloud = cvCreateMat(corners, 3, CV_32FC1); + CvMat * point_2Dcloud = cvCreateMat(corners, 2, CV_32FC1); + for(int l = 0; l < corners; l++) { + CV_MAT_ELEM(*point_2Dcloud,float,l,0) = 0.0; + CV_MAT_ELEM(*point_2Dcloud,float,l,1) = 1.0; + CV_MAT_ELEM(*point_3Dcloud,float,l,0) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).x; + CV_MAT_ELEM(*point_3Dcloud,float,l,1) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).y; + CV_MAT_ELEM(*point_3Dcloud,float,l,2) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).z; + } + cvProjectPoints2(point_3Dcloud, rotation, translation, intrinsics, + distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + for(int l = 0; l < corners; l++) { + double x = CV_MAT_ELEM(*point_2Dcloud,float,l,0) - CV_MAT_ELEM(*points2D,CvPoint2D32f,j,l).x; + double y = CV_MAT_ELEM(*point_2Dcloud,float,l,1) - CV_MAT_ELEM(*points2D,CvPoint2D32f,j,l).y; + tmp += sqrt(x*x + y*y); + } + cvReleaseMat(&point_2Dcloud); + reprojectionError[i] += tmp; + cvReleaseMat(&point_3Dcloud); + } + cout << reprojectionError[i]/successes/30 << endl; + cvReleaseMat(&rotation); + cvReleaseMat(&translation); + } + + int maxindex = -1; + double max = DBL_MAX; + for(int i = 0; i < successes + 4; i++) { + if(reprojectionError[i] < max) { + maxindex = i; + max = reprojectionError[i]; + } + } + cerr << "Reprojection error: " << max/successes << endl; + CvMat * rotation = cvCreateMat(1, 3, CV_32FC1); + CvMat * translation = cvCreateMat(1, 3, CV_32FC1); + + for(int i = 0; i < 3; i++) { + CV_MAT_ELEM(*rotation, float, 0, i) = CV_MAT_ELEM(*rotation_vectors_temp, float, maxindex, i); + CV_MAT_ELEM(*translation, float, 0, i) = CV_MAT_ELEM(*translation_vectors_temp, float, maxindex, i); + } + string file = dir + "Rotation" + substring + ".xml"; + cvSave(file.c_str(), rotation); + file = dir + "Translation" + substring + ".xml"; + cvSave(file.c_str(), translation); + +} + +/** + * Calculates the extrinsic parameters given a set of feature matches using the + * mean and median method. + */ +void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, int successes, string dir, bool quiet, string substring) { + + CvMat* rotation_vectors = cvCreateMat(successes, 3, CV_32FC1); + CvMat* translation_vectors = cvCreateMat(successes, 3, CV_32FC1); + CvMat* vectors = cvCreateMat(successes, 6, CV_32FC1); + CvMat* rotation_vector_mean = cvCreateMat(1, 3, CV_32FC1); + CvMat* translation_vector_mean = cvCreateMat(1, 3, CV_32FC1); + CvMat* rotation_vector_median = cvCreateMat(1, 3, CV_32FC1); + CvMat* translation_vector_median = cvCreateMat(1, 3, CV_32FC1); + CvMat* median = cvCreateMat(1, 6, CV_32FC1); + for (int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) = 0; + CV_MAT_ELEM(*translation_vector_mean,float,0,t) = 0; + CV_MAT_ELEM(*rotation_vector_median,float,0,t) = 0; + CV_MAT_ELEM(*translation_vector_median,float,0,t) = 0; + CV_MAT_ELEM(*median,float,0,t) = 0; + CV_MAT_ELEM(*median,float,0,t + 3) = 0; + } + + for (int h = 0; h < successes; h++) { + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vectors,float,h,t) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) += CV_MAT_ELEM(*rotation_vectors,float,h,t); + + CV_MAT_ELEM(*translation_vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t); + CV_MAT_ELEM(*translation_vector_mean,float,0,t) += CV_MAT_ELEM(*translation_vectors,float,h,t); + + CV_MAT_ELEM(*vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t); + CV_MAT_ELEM(*vectors,float,h,t + 3) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + cout << CV_MAT_ELEM(*translation_vectors,float,h,t) << " "; + } + cout << endl; + } + + // mean + cout << "Getting mean vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) /= successes; + CV_MAT_ELEM(*rotation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*rotation_vector_mean,float,0,t); + CV_MAT_ELEM(*translation_vector_mean,float,0,t) /= successes; + CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*translation_vector_mean,float,0,t); + cout << CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) << " "; + } + cout << endl; + + // getting the median vectors + // median + cout << "Getting median vector" << endl; + filterMedian(vectors, successes, 1, median); + cout << "Got median vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*translation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t); + CV_MAT_ELEM(*rotation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t+3); + CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*rotation_vector_median,float,0,t); + CV_MAT_ELEM(*translation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*translation_vector_median,float,0,t); + } + + + // filtered median + cout << "Getting filtered median vector" << endl; + filterMedian(vectors, successes, 4, median); + cout << "Got filtered median vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t+3); + CV_MAT_ELEM(*translation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t); + } + + // elementwise median + // finding the median of rotation and translation + // sorting the rotation vectors element by element + + sortElementByElement(vectors, 6, successes); + /* + sortElementByElement(translation_vectors, 3, successes); + + if(!quiet) { + cout << "number of successes : " << successes << endl; + cout << "rotation vectors are" << endl; + for (int i = 0; i < successes; i++) { + cout << CV_MAT_ELEM(*rotation_vectors,float,i,0) << " " + < -30) + if(CV_MAT_ELEM(*Translation, float, 0, 0) < -20) + if(CV_MAT_ELEM(*Translation, float, 0, 1) > -4 ) + if(CV_MAT_ELEM(*Translation, float, 0, 1) < -00) + if(CV_MAT_ELEM(*Translation, float, 0, 2) > -12) + if(CV_MAT_ELEM(*Translation, float, 0, 2) < -8 ) { + */ + if(!quiet) cout << "Rotation is:" << endl; + for (int row = 0; row < 1; row++) { + for (int col = 0; col < 3; col++) { + if(!quiet) cout << CV_MAT_ELEM( *Rotation, float, row, col ) << " "; + CV_MAT_ELEM( *rotation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Rotation, float, row, col ); + } + if(!quiet) cout << endl; + } + if(!quiet) cout << "Translation is:" << endl; + for (int row = 0; row < 1; row++) { + for (int col = 0; col < 3; col++) { + if(!quiet) cout << CV_MAT_ELEM( *Translation, float, row, col ) << " "; + CV_MAT_ELEM( *translation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Translation, float, row, col ); + } + if(!quiet) cout << endl; + } + + cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; + int c = cvWaitKey(100); + cin >> in; + if (c == 27) { + break; + } + + if (in == 'y') { + for (int j = 0; j < corner_exp; ++j) { + CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).x = (float)point_array2[j][0]; + CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).y = (float)point_array2[j][1]; + CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).x = corners[j].x; + CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).y = corners[j].y; + CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).z = corners[j].z; + } + successes++; + // } + + } else if(in == 'x') { + break; + } + //cvReleaseImage(&image); + cvReleaseMat(&image_points); + cvReleaseMat(&object_points); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + } + cvReleaseImage(&image); + cvReleaseImage(&image1); + cvReleaseImage(&image2); + }//for loop for imagecount + cvDestroyWindow("Original Image"); + cvDestroyWindow("Final Result"); + + cout << "Number of successes: " << successes << endl; + // Now calculating mean and median rotation and trans + string substring = optical? "Optical" : ""; + calculateExtrinsics(rotation_vectors_temp, translation_vectors_temp, successes, dir, quiet, substring); + calculateExtrinsicsWithReprojectionCheck(points2D, points3D, rotation_vectors_temp, translation_vectors_temp, distortion, intrinsic, corner_exp, successes, dir, quiet, substring); + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&translation_vectors_temp); + cvReleaseMat(&rotation_vectors_temp); + cvReleaseMat(&points2D); + cvReleaseMat(&points3D); +} + +//bool readFrames(char * dir, int index, double * rPos, rPosTheta) { +//bool readFrames(char * dir, int index, double * inMatrix, double * rPos) { +bool readFrames(const char * dir, int index, double * tMatrix, CvMat * inMatrix, CvMat * rPos) { + char frameFileName[255]; + snprintf(frameFileName,255,"%sscan%.3d.frames",dir,index); + ifstream pose_in; + pose_in.open(frameFileName); + + if (!pose_in.good()) return false; // no more files in the directory + + cout << "Reading frame " << frameFileName << "..." << endl; + double tmpMatrix[17]; + while(pose_in.good()) { + for (unsigned int i = 0; i < 17; pose_in >> tMatrix[i++]); + } + + M4inv(tMatrix, tmpMatrix); + + CV_MAT_ELEM(*inMatrix, float,0,0) = tmpMatrix[10]; + CV_MAT_ELEM(*inMatrix, float,0,1) = -tmpMatrix[2]; + CV_MAT_ELEM(*inMatrix, float,0,2) = tmpMatrix[6]; + CV_MAT_ELEM(*inMatrix, float,1,0) = -tmpMatrix[8]; + CV_MAT_ELEM(*inMatrix, float,1,1) = tmpMatrix[0]; + CV_MAT_ELEM(*inMatrix, float,1,2) = -tmpMatrix[4]; + CV_MAT_ELEM(*inMatrix, float,2,0) = tmpMatrix[9]; + CV_MAT_ELEM(*inMatrix, float,2,1) = -tmpMatrix[1]; + CV_MAT_ELEM(*inMatrix, float,2,2) = tmpMatrix[5]; + + CV_MAT_ELEM(*rPos, float,0,0) = tmpMatrix[14]; + CV_MAT_ELEM(*rPos, float,1,0) = -tmpMatrix[12]; + CV_MAT_ELEM(*rPos, float,2,0) = tmpMatrix[13]; + + return true; +} + +void writeGlobalCameras(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { + + string file; + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + CvMat *Rotation; + CvMat *Translation; + loadExtrinsicCalibration(Rotation, Translation, dir, method, optical); + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + + for (int count = start; count <= end; count++) { + // filling the rotation matrix + + // reading the frame files 3D points and projecting them back to 2d + CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1); + CvMat* rPos = cvCreateMat(3,1,CV_32FC1); + double * tMatrix = new double[17]; + readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos); + + // TODO make sure correct points are printed + /* + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + */ + + // write colored data + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + double angle = rot_angle * (p%nrP360); + int count0 = count * nrP360 + p; + string outname = outdir + "/image" + to_string(count0, 3) + ".mat"; + + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + /* + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + */ + } + cout << endl; + + // transform into global coordinate system (inMatrix, rPos) + CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* rodglob = cvCreateMat(3,1,CV_32FC1); + cvRodrigues2(inMatrix, rodglob); + cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI); + //cvRodrigues2(r_globalI, rot_tmp); + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + CvMat* rotmatrix = cvCreateMat(3,3,CV_32FC1); + cvRodrigues2(r_globalI, rotmatrix); + + //cvSave(outname.c_str(), rotmatrix); + + for(int i = 0; i < 3; i++) { + for(int j = 0; j < 3; j++) { + outfile << CV_MAT_ELEM(*rotmatrix,float,i,j) << " "; + } + + outfile << CV_MAT_ELEM(*t_globalI,float,i,0) << endl; + } + outfile << "0 0 0 1" << endl; + // checking whether projection lies within the image boundaries + cvReleaseMat(&rot_tmp); + + outfile.close(); + outfile.flush(); + + } + + } + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + +} +/** + * Read scans + * Read frames + */ +void calculateGlobalCameras(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { + + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + CvMat *intrinsic; + CvMat *distortion; + loadIntrinsicCalibration(intrinsic, distortion, dir, optical); + CvMat *Rotation; + CvMat *Translation; + loadExtrinsicCalibration(Translation, Rotation, dir, method, optical); + + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; + } + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + int pointcnt = 0; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + for (int count = start; count <= end; count++) { + // filling the rotation matrix + CvMat* point_3Dcloud; + CvMat* point_2Dcloud; + CvMat* undistort_2Dcloud; + + // reading the 3D points and projecting them back to 2d + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); + + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + cout << "Points: " << red_size << endl; + point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + cout << "readScans done" << endl; + // read frames + //double * rPos = new double[3]; + //double * rPosTheta= new double[3]; + //readFrames(dir, count, rPos, rPosTheta); + + CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1); + CvMat* rPos = cvCreateMat(3,1,CV_32FC1); + double * tMatrix = new double[17]; + readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos); + Scan::allScans[0]->transform(tMatrix, Scan::INVALID, 0); + + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + // TODO make sure correct points are printed + + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + + } + int nr_points = red_size; + cout << "Number of points read: " << red_size << endl; + delete Scan::allScans[0]; + Scan::allScans.clear(); + + // write colored data + string outname = outdir + "/scan" + to_string(count, 3) + ".3d"; + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + //for(int p = 4; p < 5; p++) { + //double angle = rot_angle * (p%nrP360) + 2.0; + double angle = rot_angle * (p%nrP360); + cout << angle << endl; + // loading images + int count0 = count * nrP360 + p; + + string t, t0; + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + //t = dir + "/photo" + to_string(count0, 3) + "_2.jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_90.jpg"; + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_1.jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + IplImage* image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + return; + } + CvSize size = cvGetSize(image); + + image = resizeImage(image, scale); + + + /** TODO!!! + * Transform rPos and rPosTheta into OpenCV (Rodrigues) + * cvComposeRT with rod_comI and t_comI + */ + // rotate Rotation and Translation + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + cout << "tmp2" << endl; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + cout << "tmp3" << endl; + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + cout << endl; + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + } + cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + + // transform into global coordinate system (inMatrix, rPos) + CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* rodglob = cvCreateMat(3,1,CV_32FC1); + cvRodrigues2(inMatrix, rodglob); + cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI); + //TODO verify order + cvRodrigues2(r_globalI, rot_tmp); + + //cvComposeRT(rod_comI, t_comI, rodglob, rPos, r_globalI, t_globalI); + + + cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + + + // END TODO + // Project Points + /* + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + */ + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + //for counting how many points get mapped to first and second image file + int point_map1 = 0; // #points mapped to first image + int point_map2 = 0; // " " " second image + int point_map3 = 0; // " " " second image + + // checking whether projection lies within the image boundaries + cout << "Now project points" << endl; + for (int k = 0; k < nr_points; k++) { + float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); + float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) { + point_map1++; + px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); + py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { + point_map2++; + CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3); + CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2); + + cvTransform(tmp1, tmp2, rot_tmp); + cvReleaseMat(&tmp1); + + //double tmpz = CV_MAT_ELEM(*t_globalI, CV_32FC1, 2,0); + double tmpz = -CV_MAT_ELEM(*t_globalI, float, 2,0); + //double tmpz = CV_MAT_ELEM(*TranslationI, float, 2,0); + + if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < tmpz) { + cvReleaseMat(&tmp2); + continue; + } + + //cout << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).x << " " + // << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).y << " " + // << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).z << endl; + + cvReleaseMat(&tmp2); + /* + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl; + */ + point_map3++; + int ppx = 0; + int ppy = 0; + if (px - int(px) < .5) { + ppx = int(px); + } else { + ppx = int(px) + 1; + } + if (py - int(py) < .5) { + ppy = int(py); + } else { + ppy = int(py) + 1; + } + + CvScalar c; + c = cvGet2D(image, ppy, ppx); + // check for overlap + /* + if(correction) { + vector temp_vec; + float p_id = 1; // 1 for pixel, 0 for neighboring pixel + temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); + temp_vec.push_back(c.val[2]); + temp_vec.push_back(c.val[1]); + temp_vec.push_back(c.val[0]); + temp_vec.push_back(p_id); + if(neighborhood > 1) { + int limit = neighborhood / 2; + + int lower_y = ppy - limit > 0 ? ppy - limit : 0; + int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; + int lower_x = ppx - limit > 0 ? ppx - limit : 0; + int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; + + for (int y = lower_y; y < upper_y; y++) { + for (int x = lower_x; x < upper_x; x++) { + if(x == ppx && y == ppy) { + temp_vec[6] = 1; + } else { + temp_vec[6] = 0; + } + data1[y][x].push_back(temp_vec); + } + } + + } else { + data1[ppy][ppx].push_back(temp_vec); + } + temp_vec.clear(); + } else { + */ + // write all the data + + outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outdat << c.val[2] <<" "<< c.val[1]<<" "< 100) { + outfile.write(outdat.str().c_str(), outdat.str().size()); + pointcnt = 0; + outdat.clear(); + outdat.str(""); + } + + /* + outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { + if(size.width > 0 && size.height > 0) { + CorrectErrorAndWrite(data2, outfile, size, optical); + } + } + } + + cout << "Done correction" << endl; + // clean up + outfile.flush(); + + double endtime = GetCurrentTimeInMilliSec(); + double time = endtime - starttime; + time = time/1000.0; + cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl; + + cout << point_map1 << " " << point_map2 << " " << point_map3 << endl; + cvReleaseImage(&image); + if(correction) { + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + data1[i][j].clear(); + if(fabs(rot_angle) > 1) { + data2[i][j].clear(); + } + } + } + } + cvReleaseMat(&rot_tmp); + } + outfile.close(); + + cvReleaseMat(&point_2Dcloud); + cvReleaseMat(&point_3Dcloud); + cvReleaseMat(&undistort_2Dcloud); + + } + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&undistort); + +} + +void loadIntrinsicCalibration(CvMat * intrinsic, CvMat * distortion, string dir, bool optical) { + string substring = optical? "Optical" : ""; + string file = dir + "Intrinsics" + substring + ".xml"; + intrinsic = (CvMat*) cvLoad(file.c_str()); + file = dir + "Distortion" + substring + ".xml"; + distortion = (CvMat*) cvLoad(file.c_str()); +} + +void loadExtrinsicCalibration(CvMat * Translation, CvMat * Rotation, string dir, int method, bool optical) { + string substring = optical? "Optical" : ""; + string file; + switch(method) { + case 0: + file = dir + "Rotation" + substring + ".xml"; + break; + case 1: + file = dir + "RotationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "RotationMean" + substring + ".xml"; + break; + } + Rotation = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Translation" + substring + ".xml"; + break; + case 1: + file = dir + "TranslationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "TranslationMean" + substring + ".xml"; + break; + } + Translation = (CvMat*) cvLoad(file.c_str()); +} + +void openOutputDirectory(string outdir) { +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } +} + +int openDirectory(CvMat* point_3Dcloud, string dir, IOType type, int count) { + // reading the 3D points and projecting them back to 2d + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); + + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + } + return red_size; +} + +void loadImage(IplImage *image, string dir, int count0, bool optical, int scale){ + string t, t0; + if(optical) { + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + exit(0); + } + + image = resizeImage(image, scale); +} + +void calculateGlobalPoses(CvMat *Translation, CvMat *Rotation, CvMat *t_comI, +CvMat *rod_comI, double angle, CvMat *rot_tmp) { + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + //cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + //cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + rot_tmp = cvCreateMat(3,3,CV_32FC1); + //CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + rod_comI = cvCreateMat(3,1,CV_32FC1); + t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + //cout << endl; + //cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + /* + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + */ + } + //cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + +} + +/** + * Main function for projecting the 3D points onto the corresponding image and + * associating temperature values to the data points. + */ +void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { + + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + + CvMat *distortion; + CvMat *intrinsic; + CvMat *Translation; + CvMat *Rotation; + loadExtrinsicCalibration(Translation, Rotation,dir,method,optical); + loadIntrinsicCalibration(intrinsic,distortion,dir,optical); + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; + } + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + int pointcnt = 0; + string outdir = dir + "/labscan-map"; + openOutputDirectory(outdir); + + for (int count = start; count <= end; count++) { + // filling the rotation matrix + + CvMat *point_3Dcloud; + int nr_points = openDirectory(point_3Dcloud, dir, type, count); + + CvMat* point_2Dcloud = cvCreateMat(nr_points, 2, CV_32FC1); + CvMat* undistort_2Dcloud = cvCreateMat(nr_points, 2, CV_32FC1); + + cout << "Number of points read: " << nr_points << endl; + delete Scan::allScans[0]; + Scan::allScans.clear(); + + // write colored data + string outname = outdir + "/scan" + to_string(count, 3) + ".3d"; + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + //for(int p = 0; p < 9; p++) { + //double angle = rot_angle * (p%nrP360) + 2.0; + double angle = rot_angle * (p%nrP360); + cout << angle << endl; + // loading images + int count0 = count * nrP360 + p; + + IplImage *image; + loadImage(image, dir, count0, optical, scale); + CvSize size = cvGetSize(image); + + // rotate Rotation and Translation + CvMat* rod_comI; + CvMat* t_comI; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + calculateGlobalPoses(Translation, Rotation, t_comI, rod_comI, angle, rot_tmp); + + // Project Points + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + + cvReleaseMat(&t_comI); + cvReleaseMat(&rod_comI); + cout << "Done projecting points" << endl; + + //for counting how many points get mapped to first and second image file + int point_map1 = 0; // #points mapped to first image + int point_map2 = 0; // " " " second image + int point_map3 = 0; // " " " second image + + // checking whether projection lies within the image boundaries + cout << "Now project points" << endl; + for (int k = 0; k < nr_points; k++) { + float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); + float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) { + point_map1++; + px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); + py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { + point_map2++; + CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3); + CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2); + + cvTransform(tmp1, tmp2, rot_tmp); + cvReleaseMat(&tmp1); + + + if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < 0) { + cvReleaseMat(&tmp2); + continue; + } + cvReleaseMat(&tmp2); + /* + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl; + */ + point_map3++; + int ppx = 0; + int ppy = 0; + if (px - int(px) < .5) { + ppx = int(px); + } else { + ppx = int(px) + 1; + } + if (py - int(py) < .5) { + ppy = int(py); + } else { + ppy = int(py) + 1; + } + + CvScalar c; + c = cvGet2D(image, ppy, ppx); + // check for overlap + /* + if(correction) { + vector temp_vec; + float p_id = 1; // 1 for pixel, 0 for neighboring pixel + temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); + temp_vec.push_back(c.val[2]); + temp_vec.push_back(c.val[1]); + temp_vec.push_back(c.val[0]); + temp_vec.push_back(p_id); + if(neighborhood > 1) { + int limit = neighborhood / 2; + + int lower_y = ppy - limit > 0 ? ppy - limit : 0; + int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; + int lower_x = ppx - limit > 0 ? ppx - limit : 0; + int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; + + for (int y = lower_y; y < upper_y; y++) { + for (int x = lower_x; x < upper_x; x++) { + if(x == ppx && y == ppy) { + temp_vec[6] = 1; + } else { + temp_vec[6] = 0; + } + data1[y][x].push_back(temp_vec); + } + } + + } else { + data1[ppy][ppx].push_back(temp_vec); + } + temp_vec.clear(); + } else { + */ + // write all the data + + outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outdat << c.val[2] <<" "<< c.val[1]<<" "< 100) { + outfile.write(outdat.str().c_str(), outdat.str().size()); + pointcnt = 0; + outdat.clear(); + outdat.str(""); + } + + /* + outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { + if(size.width > 0 && size.height > 0) { + CorrectErrorAndWrite(data2, outfile, size, optical); + } + } + } + + cout << "Done correction" << endl; + // clean up + outfile.flush(); + + double endtime = GetCurrentTimeInMilliSec(); + double time = endtime - starttime; + time = time/1000.0; + cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl; + + cout << point_map1 << " " << point_map2 << " " << point_map3 << endl; + cvReleaseImage(&image); + if(correction) { + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + data1[i][j].clear(); + if(fabs(rot_angle) > 1) { + data2[i][j].clear(); + } + } + } + } + cvReleaseMat(&rot_tmp); + } + outfile.close(); + + cvReleaseMat(&point_2Dcloud); + cvReleaseMat(&point_3Dcloud); + cvReleaseMat(&undistort_2Dcloud); + + } + // Final cleanup + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&undistort); + +} + +/** + * Sorts a number of float array according to their distance to the origin. + */ +void sortDistances(float ** points, int size) { + int swapped1 = 0; + do { + swapped1 = 0; + for(int a = 1; a <= size - 1; a++) { + if(Len(points[a]) < Len(points[a - 1])) { + float * tmp = points[a-1]; + points[a-1] = points[a]; + points[a] = tmp; + swapped1 = 1; + } + } + } while (swapped1 == 1); +} + +/** + * Performs clustering on all points that are projected onto one pixel. + * Writes only the points from the largest closest cluster. + */ +void clusterSearch(float ** points, int size, double thresh1, double thres2, fstream &outfile, bool optical) { + int position = 0; + int cluster_count = 0; + + double max_cluster = 0; + int max_position = 0; + vector clusters; + while (position < size) { + double sum = 0.0; + int j = position + 1; + while(j < size && (Len(points[j]) < (Len(points[j-1]) + thresh1))) { + j++; + cluster_count++; + sum+=Len(points[j-1]); + } + double * tmp = new double[4]; + tmp[0] = position; + tmp[1] = j - 1; + // tmp[2] = sum / (j - position); + // weird heuristic ;-) (clustersize/(rank of the cluster)) + tmp[2] = (double)(j - position) / (clusters.size() + 1.0); + tmp[3] = (double)(j - position); + if(tmp[3] > max_cluster) { + max_position = clusters.size(); + max_cluster = tmp[3]; + } + clusters.push_back(tmp); + position = j; + } + + /* + max_position = 0; + for(int p = clusters.size() - 1; p > -1; p--) { + max_position = p; + break; + } + */ + + for(int p = clusters[max_position][0]; p <= clusters[max_position][1]; p++) { + if(points[p][6] == 1) { + outfile << points[p][0] << " " << points[p][1] << " " << points[p][2] << " "; + if(optical) { + outfile << points[p][3] << " " << points[p][4] << " " << points[p][5] << endl; + } else { + outfile << (points[p][5] - 1000.0)/10.0 << endl; + } + } + } + + for(unsigned int i = 0; i < clusters.size(); i++) { + delete[] clusters[i]; + } +} + +void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical) { + double thresh1 = 4; + double thresh2 = 5; + + cout << size.height << " " << size.width << endl; + // getting points mapping to one pixel + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + int tmp_size = data[i][j].size(); + if (tmp_size > 0) { + float ** points = new float*[tmp_size]; + for (int k = 0; k < tmp_size; k++) { + points[k] = new float[7]; + for(int l = 0; l < 7; l++) { + points[k][l] = data[i][j][k][l]; + } + } + + //sorting the points now in ascending order wrt distance + sortDistances(points, tmp_size); + //look for clusters + clusterSearch(points, tmp_size, thresh1, thresh2, outfile, optical); + for (int k = 0; k < tmp_size; k++) { + delete[] points[k]; + } + delete[] points; + } + + } + } + +} + +/** + * Prints out usage message + */ +void usage(char* prog) { +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end at scan NR" << endl + << endl + << bold << " -x" << normal << " NR, " << bold << "--width=" << normal << "NR" << endl + << " NR of lamps/corners in x direction" << endl + << endl + << bold << " -y" << normal << " NR, " << bold << "--height=" << normal << "NR" << endl + << " NR of lamps/corners in y direction" << endl + << endl + << bold << " -o --=optical" << normal << endl + << " use optical camera instead of thermal camera" << endl + << endl + << bold << " -c --=chess" << normal << endl + << " use chessboard pattern for calibration instead of lightbulb pattern" << endl + << endl + << bold << " -I --=intrinsic" << normal << endl + << " perform intrinsic calibration" << endl + << endl + << bold << " -E --=extrinsic" << normal << endl + << " perform extrinsic calibration" << endl + << endl + << bold << " -P --=mapping" << normal << endl + << " perform mapping of image data to point cloud" << endl + << endl + << bold << " -q --=quiet" << normal << endl + << " " << endl + << endl << endl; + + cout << bold << "EXAMPLES " << normal << endl + << " " << prog << " -s 2 -e 10 dat" << endl << endl; + exit(1); + +} + +/** + * Parses command line arguments needed for plane detection. For details about + * the argument see usage(). + */ + +int parseArgs(int argc, char **argv, string &dir, int &start, int &end, double +&maxDist, double &minDist, IOType &type, bool &optical, bool &chess, int +&width, int &height, bool &intrinsic, bool &extrinsic, bool &mapping, bool +&correction, int &scale, int &neighborhood, double &angle, bool &quiet ) { + // from unistd.h: + int c; + extern char *optarg; + extern int optind; + + /* options descriptor */ + // 0: no arguments, 1: required argument, 2: optional argument + static struct option longopts[] = { + { "correction", no_argument, 0, 'C' }, + { "scale", required_argument, 0, 'S' }, + { "neighborhood", required_argument, 0, 'n' }, + { "angle", required_argument, 0, 'a' }, + { "format", required_argument, 0, 'f' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "width", required_argument, 0, 'x' }, + { "height", required_argument, 0, 'y' }, + { "quiet", no_argument, 0, 'q' }, + { "optical", no_argument, 0, 'o' }, + { "intrinsic", no_argument, 0, 'I' }, + { "extrinsic", no_argument, 0, 'E' }, + { "mapping", no_argument, 0, 'P' }, + { "chess", no_argument, 0, 'c' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + cout << endl; + while ((c = getopt_long(argc, argv, "f:s:e:x:y:m:M:qoIEPcCS:n:a:", longopts, NULL)) != -1) { + switch (c) + { + case 's': + start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'f': + try { + type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'q': + quiet = true; + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'o': + optical = true; + break; + case 'I': + intrinsic = true; + break; + case 'E': + extrinsic = true; + break; + case 'P': + mapping = true; + break; + case 'c': + chess = true; + break; + case 'x': + width = atoi(optarg); + break; + case 'y': + height = atoi(optarg); + break; + case 'S': + scale = atoi(optarg); + break; + case 'a': + angle = atof(optarg); + break; + case 'n': + neighborhood = atoi(optarg); + break; + case 'C': + correction = true; + break; + case '?': + usage(argv[0]); + return 1; + default: + cout << "Abort" << endl; + abort (); + } + } + if (optind != argc-1) { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + if(!(intrinsic || extrinsic || mapping)) { + cerr << "\n*** Please choose at least one method (intrinsic calibration, " + << "extrinsic calibration, mapping of image data to point data! ***\n" << + endl; + usage(argv[0]); + } + return 0; + +} + +/** + * Main function. Calls either function for color mapping or function for + * intrinsic and/or extrinsic calibration. + */ +int main(int argc, char** argv) { + string dir; + int start = 0; + int end = -1; + int width = 5; + int height = 6; + double maxDist = -1; + double minDist = -1; + IOType type = UOS; + bool optical = false; + bool chess = false; + bool intrinsic = false; + bool extrinsic = false; + bool mapping = false; + bool quiet = false; + int scale = 1; + //double rot_angle = -40; + double rot_angle = 0; + bool correction = false; + int neighborhood = 1; + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, type, optical, chess, + width, height, intrinsic, extrinsic, mapping, correction, scale, neighborhood, + rot_angle, quiet); + + // either mapping + if(mapping) { + if(!quiet) cout << "Starting projecting and mapping image data to point cloud..." << endl; + //TODO ProjectAndMap(start, end, optical, quiet, dir, type, scale, rot_angle, minDist, maxDist, correction, neighborhood); + + //calculateGlobalCameras(start, end, optical, quiet, dir, type, scale, + writeGlobalCameras(start, end, optical, quiet, dir, type, scale, + rot_angle, minDist, maxDist, correction, neighborhood, 0); + if(!quiet) cout << "\nDONE" << endl; + return 0; + } + + // or calibration + if(intrinsic) { + if(!quiet) { + cout << "Starting intrinsic calibration using "; + if(chess) cout << "chessboard pattern..." << endl; + else cout << "lightbulb pattern..." << endl; + } + CalibFunc(width, height, start, end, optical, chess, quiet, dir, scale); + if(!quiet) cout << "\nDONE" << endl; + } + + if(extrinsic) { + if(!quiet) { + cout << "Starting extrinsic calibration using "; + if(chess) cout << "chessboard pattern..." << endl; + else cout << "lightbulb pattern..." << endl; + } + ExtrCalibFunc(width, height, start, end, optical, chess, quiet, dir, scale); + if(!quiet) cout << "\nDONE" << endl; + } + + return 0; +} + diff --git a/.svn/pristine/dc/dccec6383fce1b84a2322c024100648467f1aee6.svn-base b/.svn/pristine/dc/dccec6383fce1b84a2322c024100648467f1aee6.svn-base new file mode 100644 index 0000000..64880ff --- /dev/null +++ b/.svn/pristine/dc/dccec6383fce1b84a2322c024100648467f1aee6.svn-base @@ -0,0 +1,587 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +/// \file cvblob.h +/// \brief OpenCV Blob header file. + +#ifdef SWIG +%module cvblob +%{ +#include "cvblob.h" +%} +#endif + +#ifndef CVBLOB_H +#define CVBLOB_H + +#include +#include +#include +#include +#include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#ifndef __CV_BEGIN__ +#define __CV_BEGIN__ __BEGIN__ +#endif +#ifndef __CV_END__ +#define __CV_END__ __END__ +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + namespace cvb + { + + //////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Contours + + // Chain code: + // 7 0 1 + // 6 2 + // 5 4 3 +#define CV_CHAINCODE_UP 0 ///< Up. +#define CV_CHAINCODE_UP_RIGHT 1 ///< Up and right. +#define CV_CHAINCODE_RIGHT 2 ///< Right. +#define CV_CHAINCODE_DOWN_RIGHT 3 ///< Down and right. +#define CV_CHAINCODE_DOWN 4 ///< Down. +#define CV_CHAINCODE_DOWN_LEFT 5 ///< Down and left. +#define CV_CHAINCODE_LEFT 6 ///< Left. +#define CV_CHAINCODE_UP_LEFT 7 ///< Up and left. + + /// \brief Move vectors of chain codes. + /// \see CV_CHAINCODE_UP + /// \see CV_CHAINCODE_UP_LEFT + /// \see CV_CHAINCODE_LEFT + /// \see CV_CHAINCODE_DOWN_LEFT + /// \see CV_CHAINCODE_DOWN + /// \see CV_CHAINCODE_DOWN_RIGHT + /// \see CV_CHAINCODE_RIGHT + /// \see CV_CHAINCODE_UP_RIGHT + const char cvChainCodeMoves[8][2] = { { 0, -1}, + { 1, -1}, + { 1, 0}, + { 1, 1}, + { 0, 1}, + {-1, 1}, + {-1, 0}, + {-1, -1} + }; + + /// \brief Direction. + /// \see CV_CHAINCODE_UP + /// \see CV_CHAINCODE_UP_LEFT + /// \see CV_CHAINCODE_LEFT + /// \see CV_CHAINCODE_DOWN_LEFT + /// \see CV_CHAINCODE_DOWN + /// \see CV_CHAINCODE_DOWN_RIGHT + /// \see CV_CHAINCODE_RIGHT + /// \see CV_CHAINCODE_UP_RIGHT + typedef unsigned char CvChainCode; + + /// \brief Chain code. + /// \see CvChainCode + typedef std::list CvChainCodes; + + /// \brief Chain code contour. + /// \see CvChainCodes + struct CvContourChainCode + { + CvPoint startingPoint; ///< Point where contour begin. + CvChainCodes chainCode; ///< Polygon description based on chain codes. + }; + + typedef std::list CvContoursChainCode; ///< List of contours (chain codes type). + + /// \brief Polygon based contour. + typedef std::vector CvContourPolygon; + + /// \fn void cvRenderContourChainCode(CvContourChainCode const *contour, IplImage const *img, CvScalar const &color=CV_RGB(255, 255, 255)) + /// \brief Draw a contour. + /// \param contour Chain code contour. + /// \param img Image to draw on. + /// \param color Color to draw (default, white). + /// \see CvContourChainCode + void cvRenderContourChainCode(CvContourChainCode const *contour, IplImage const *img, CvScalar const &color=CV_RGB(255, 255, 255)); + + /// \fn CvContourPolygon *cvConvertChainCodesToPolygon(CvContourChainCode const *cc) + /// \brief Convert a chain code contour to a polygon. + /// \param cc Chain code contour. + /// \return A polygon. + /// \see CvContourChainCode + /// \see CvContourPolygon + CvContourPolygon *cvConvertChainCodesToPolygon(CvContourChainCode const *cc); + + /// \fn void cvRenderContourPolygon(CvContourPolygon const *contour, IplImage *img, CvScalar const &color=CV_RGB(255, 255, 255)) + /// \brief Draw a polygon. + /// \param contour Polygon contour. + /// \param img Image to draw on. + /// \param color Color to draw (default, white). + /// \see CvContourPolygon + void cvRenderContourPolygon(CvContourPolygon const *contour, IplImage *img, CvScalar const &color=CV_RGB(255, 255, 255)); + + /// \fn double cvContourPolygonArea(CvContourPolygon const *p) + /// \brief Calculates area of a polygonal contour. + /// \param p Contour (polygon type). + /// \return Area of the contour. + double cvContourPolygonArea(CvContourPolygon const *p); + + /// \fn double cvContourChainCodePerimeter(CvContourChainCode const *c) + /// \brief Calculates perimeter of a chain code contour. + /// \param c Contour (chain code type). + /// \return Perimeter of the contour. + double cvContourChainCodePerimeter(CvContourChainCode const *c); + + /// \fn double cvContourPolygonPerimeter(CvContourPolygon const *p) + /// \brief Calculates perimeter of a polygonal contour. + /// \param p Contour (polygon type). + /// \return Perimeter of the contour. + double cvContourPolygonPerimeter(CvContourPolygon const *p); + + /// \fn double cvContourPolygonCircularity(const CvContourPolygon *p) + /// \brief Calculates the circularity of a polygon (compactness measure). + /// \param p Contour (polygon type). + /// \return Circularity: a non-negative value, where 0 correspond with a circumference. + double cvContourPolygonCircularity(const CvContourPolygon *p); + + /// \fn CvContourPolygon *cvSimplifyPolygon(CvContourPolygon const *p, double const delta=1.) + /// \brief Simplify a polygon reducing the number of vertex according the distance "delta". + /// Uses a version of the Ramer-Douglas-Peucker algorithm (http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm). + /// \param p Contour (polygon type). + /// \param delta Minimun distance. + /// \return A simplify version of the original polygon. + CvContourPolygon *cvSimplifyPolygon(CvContourPolygon const *p, double const delta=1.); + + /// \fn CvContourPolygon *cvPolygonContourConvexHull(CvContourPolygon const *p) + /// \brief Calculates convex hull of a contour. + /// Uses the Melkman Algorithm. Code based on the version in http://w3.impa.br/~rdcastan/Cgeometry/. + /// \param p Contour (polygon type). + /// \return Convex hull. + CvContourPolygon *cvPolygonContourConvexHull(CvContourPolygon const *p); + + /// \fn void cvWriteContourPolygonCSV(const CvContourPolygon& p, const std::string& filename) + /// \brief Write a contour to a CSV (Comma-separated values) file. + /// \param p Polygon contour. + /// \param filename File name. + void cvWriteContourPolygonCSV(const CvContourPolygon& p, const std::string& filename); + + /// \fn void cvWriteContourPolygonSVG(const CvContourPolygon& p, const std::string& filename, const CvScalar& stroke=cvScalar(0,0,0), const CvScalar& fill=cvScalar(255,255,255)) + /// \brief Write a contour to a SVG file (http://en.wikipedia.org/wiki/Scalable_Vector_Graphics). + /// \param p Polygon contour. + /// \param filename File name. + /// \param stroke Stroke color (black by default). + /// \param fill Fill color (white by default). + void cvWriteContourPolygonSVG(const CvContourPolygon& p, const std::string& filename, const CvScalar& stroke=cvScalar(0,0,0), const CvScalar& fill=cvScalar(255,255,255)); + + //////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Blobs + + /// \brief Type of label. + /// \see IPL_DEPTH_LABEL + typedef unsigned int CvLabel; + //typedef unsigned char CvLabel; + + /// \def IPL_DEPTH_LABEL + /// \brief Size of a label in bits. + /// \see CvLabel +#define IPL_DEPTH_LABEL (sizeof(cvb::CvLabel)*8) + + /// \def CV_BLOB_MAX_LABEL + /// \brief Max label number. + /// \see CvLabel. +#define CV_BLOB_MAX_LABEL std::numeric_limits::max() + + /// \brief Type of identification numbers. + typedef unsigned int CvID; + + /// \brief Struct that contain information about one blob. + struct CvBlob + { + CvLabel label; ///< Label assigned to the blob. + + union + { + unsigned int area; ///< Area (moment 00). + unsigned int m00; ///< Moment 00 (area). + }; + + unsigned int minx; ///< X min. + unsigned int maxx; ///< X max. + unsigned int miny; ///< Y min. + unsigned int maxy; ///< y max. + + CvPoint2D64f centroid; ///< Centroid. + + double m10; ///< Moment 10. + double m01; ///< Moment 01. + double m11; ///< Moment 11. + double m20; ///< Moment 20. + double m02; ///< Moment 02. + + double u11; ///< Central moment 11. + double u20; ///< Central moment 20. + double u02; ///< Central moment 02. + + double n11; ///< Normalized central moment 11. + double n20; ///< Normalized central moment 20. + double n02; ///< Normalized central moment 02. + + double p1; ///< Hu moment 1. + double p2; ///< Hu moment 2. + + CvContourChainCode contour; ///< Contour. + CvContoursChainCode internalContours; ///< Internal contours. + }; + + /// \var typedef std::map CvBlobs + /// \brief List of blobs. + /// A map is used to access each blob from its label number. + /// \see CvLabel + /// \see CvBlob + typedef std::map CvBlobs; + + /// \var typedef std::pair CvLabelBlob + /// \brief Pair (label, blob). + /// \see CvLabel + /// \see CvBlob + typedef std::pair CvLabelBlob; + + /// \fn unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs); + /// \brief Label the connected parts of a binary image. + /// Algorithm based on paper "A linear-time component-labeling algorithm using contour tracing technique" of Fu Chang, Chun-Jen Chen and Chi-Jen Lu. + /// \param img Input binary image (depth=IPL_DEPTH_8U and num. channels=1). + /// \param imgOut Output image (depth=IPL_DEPTH_LABEL and num. channels=1). + /// \param blobs List of blobs. + /// \return Number of pixels that has been labeled. + unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs); + + //IplImage *cvFilterLabel(IplImage *imgIn, CvLabel label); + + /// \fn void cvFilterLabels(IplImage *imgIn, IplImage *imgOut, const CvBlobs &blobs) + /// \brief Draw a binary image with the blobs that have been given. + /// \param imgIn Input image (depth=IPL_DEPTH_LABEL and num. channels=1). + /// \param imgOut Output binary image (depth=IPL_DEPTH_8U and num. channels=1). + /// \param blobs List of blobs to be drawn. + /// \see cvLabel + void cvFilterLabels(IplImage *imgIn, IplImage *imgOut, const CvBlobs &blobs); + + /// \fn CvLabel cvGetLabel(IplImage const *img, unsigned int x, unsigned int y) + /// \brief Get the label value from a labeled image. + /// \param img Label image. + /// \param x X coordenate. + /// \param y Y coordenate. + /// \return Label value. + /// \see CvLabel + CvLabel cvGetLabel(IplImage const *img, unsigned int x, unsigned int y); + + /// \fn inline void cvReleaseBlob(CvBlob *blob) + /// \brief Clear a blob structure. + /// \param blob Blob. + /// \see CvBlob + inline void cvReleaseBlob(CvBlob *blob) + { + if (blob) + { + for (CvContoursChainCode::iterator jt=blob->internalContours.begin(); jt!=blob->internalContours.end(); ++jt) + { + CvContourChainCode *contour = *jt; + if (contour) + delete contour; + } + blob->internalContours.clear(); + + delete blob; + } + } + + /// \fn inline void cvReleaseBlobs(CvBlobs &blobs) + /// \brief Clear blobs structure. + /// \param blobs List of blobs. + /// \see CvBlobs + inline void cvReleaseBlobs(CvBlobs &blobs) + { + for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it) + { + cvReleaseBlob((*it).second); + } + blobs.clear(); + } + + /// \fn CvLabel cvGreaterBlob(const CvBlobs &blobs) + /// \brief Find greater blob. + /// \param blobs List of blobs. + /// \return Label of the greater blob or 0 if there are no blobs. + /// \see cvLabel + CvLabel cvGreaterBlob(const CvBlobs &blobs); + + /// \fn void cvFilterByArea(CvBlobs &blobs, unsigned int minArea, unsigned int maxArea) + /// \brief Filter blobs by area. + /// Those blobs whose areas are not in range will be erased from the input list of blobs. + /// \param blobs List of blobs. + /// \param minArea Minimun area. + /// \param maxArea Maximun area. + void cvFilterByArea(CvBlobs &blobs, unsigned int minArea, unsigned int maxArea); + + /// \fn void cvFilterByLabel(CvBlobs &blobs, CvLabel label) + /// \brief Filter blobs by label. + /// Delete all blobs except those with label l. + /// \param blobs List of blobs. + /// \param label Label to leave. + void cvFilterByLabel(CvBlobs &blobs, CvLabel label); + + /// \fn inline CvPoint2D64f cvCentroid(CvBlob *blob) + /// \brief Calculates centroid. + /// Centroid will be returned and stored in the blob structure. + /// \param blob Blob whose centroid will be calculated. + /// \return Centroid. + /// \see CvBlob + inline CvPoint2D64f cvCentroid(CvBlob *blob) + { + return blob->centroid=cvPoint2D64f(blob->m10/blob->area, blob->m01/blob->area); + } + + /// \fn double cvAngle(CvBlob *blob) + /// \brief Calculates angle orientation of a blob. + /// This function uses central moments so cvCentralMoments should have been called before for this blob. + /// \param blob Blob. + /// \return Angle orientation in radians. + /// \see cvCentralMoments + /// \see CvBlob + double cvAngle(CvBlob *blob); + + /// \fn cvSaveImageBlob(const char *filename, IplImage *img, CvBlob const *blob) + /// \brief Save the image of a blob to a file. + /// The function uses an image (that can be the original pre-processed image or a processed one, or even the result of cvRenderBlobs, for example) and a blob structure. + /// Then the function saves a copy of the part of the image where the blob is. + /// \param filename Name of the file. + /// \param img Image. + /// \param blob Blob. + /// \see CvBlob + /// \see cvRenderBlob + void cvSaveImageBlob(const char *filename, IplImage *img, CvBlob const *blob); + +#define CV_BLOB_RENDER_COLOR 0x0001 ///< Render each blog with a different color. \see cvRenderBlobs +#define CV_BLOB_RENDER_CENTROID 0x0002 ///< Render centroid. \see cvRenderBlobs +#define CV_BLOB_RENDER_BOUNDING_BOX 0x0004 ///< Render bounding box. \see cvRenderBlobs +#define CV_BLOB_RENDER_ANGLE 0x0008 ///< Render angle. \see cvRenderBlobs +#define CV_BLOB_RENDER_TO_LOG 0x0010 ///< Print blob data to log out. \see cvRenderBlobs +#define CV_BLOB_RENDER_TO_STD 0x0020 ///< Print blob data to std out. \see cvRenderBlobs + + /// \fn void cvRenderBlob(const IplImage *imgLabel, CvBlob *blob, IplImage *imgSource, IplImage *imgDest, unsigned short mode=0x000f, CvScalar const &color=CV_RGB(255, 255, 255), double alpha=1.) + /// \brief Draws or prints information about a blob. + /// \param imgLabel Label image (depth=IPL_DEPTH_LABEL and num. channels=1). + /// \param blob Blob. + /// \param imgSource Input image (depth=IPL_DEPTH_8U and num. channels=3). + /// \param imgDest Output image (depth=IPL_DEPTH_8U and num. channels=3). + /// \param mode Render mode. By default is CV_BLOB_RENDER_COLOR|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_ANGLE. + /// \param color Color to render (if CV_BLOB_RENDER_COLOR is used). + /// \param alpha If mode CV_BLOB_RENDER_COLOR is used. 1.0 indicates opaque and 0.0 translucent (1.0 by default). + /// \see CV_BLOB_RENDER_COLOR + /// \see CV_BLOB_RENDER_CENTROID + /// \see CV_BLOB_RENDER_BOUNDING_BOX + /// \see CV_BLOB_RENDER_ANGLE + /// \see CV_BLOB_RENDER_TO_LOG + /// \see CV_BLOB_RENDER_TO_STD + void cvRenderBlob(const IplImage *imgLabel, CvBlob *blob, IplImage *imgSource, IplImage *imgDest, unsigned short mode=0x000f, CvScalar const &color=CV_RGB(255, 255, 255), double alpha=1.); + + /// \fn void cvRenderBlobs(const IplImage *imgLabel, CvBlobs &blobs, IplImage *imgSource, IplImage *imgDest, unsigned short mode=0x000f, double alpha=1.) + /// \brief Draws or prints information about blobs. + /// \param imgLabel Label image (depth=IPL_DEPTH_LABEL and num. channels=1). + /// \param blobs List of blobs. + /// \param imgSource Input image (depth=IPL_DEPTH_8U and num. channels=3). + /// \param imgDest Output image (depth=IPL_DEPTH_8U and num. channels=3). + /// \param mode Render mode. By default is CV_BLOB_RENDER_COLOR|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_ANGLE. + /// \param alpha If mode CV_BLOB_RENDER_COLOR is used. 1.0 indicates opaque and 0.0 translucent (1.0 by default). + /// \see CV_BLOB_RENDER_COLOR + /// \see CV_BLOB_RENDER_CENTROID + /// \see CV_BLOB_RENDER_BOUNDING_BOX + /// \see CV_BLOB_RENDER_ANGLE + /// \see CV_BLOB_RENDER_TO_LOG + /// \see CV_BLOB_RENDER_TO_STD + void cvRenderBlobs(const IplImage *imgLabel, CvBlobs &blobs, IplImage *imgSource, IplImage *imgDest, unsigned short mode=0x000f, double alpha=1.); + + /// \fn void cvSetImageROItoBlob(IplImage *img, CvBlob const *blob) + /// \brief Set the ROI of an image to the bounding box of a blob. + /// \param img Image. + /// \param blob Blob. + /// \see CvBlob + inline void cvSetImageROItoBlob(IplImage *img, CvBlob const *blob) + { + cvSetImageROI(img, cvRect(blob->minx, blob->miny, blob->maxx-blob->minx, blob->maxy-blob->miny)); + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Color + + /// \fn CvScalar cvBlobMeanColor(CvBlob const *blob, IplImage const *imgLabel, IplImage const *img) + /// \brief Calculates mean color of a blob in an image. + /// \param blob Blob. + /// \param imgLabel Image of labels. + /// \param img Original image. + /// \return Average color. + CvScalar cvBlobMeanColor(CvBlob const *blob, IplImage const *imgLabel, IplImage const *img); + + //////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Aux + + /// \fn double cvDotProductPoints(CvPoint const &a, CvPoint const &b, CvPoint const &c) + /// \brief Dot product of the vectors ab and bc. + /// \param a First point. + /// \param b Middle point. + /// \param c Last point. + /// \return Dot product of ab and bc. + double cvDotProductPoints(CvPoint const &a, CvPoint const &b, CvPoint const &c); + + /// \fn double cvCrossProductPoints(CvPoint const &a, CvPoint const &b, CvPoint const &c) + /// \brief Cross product of the vectors ab and bc. + /// \param a Point. + /// \param b Point. + /// \param c Point. + /// \return Cross product of ab and bc. + double cvCrossProductPoints(CvPoint const &a, CvPoint const &b, CvPoint const &c); + + /// \fn double cvDistancePointPoint(CvPoint const &a, CvPoint const &b) + /// \brief Distance between two points. + /// \param a Point. + /// \param b Point. + /// \return Distance. + double cvDistancePointPoint(CvPoint const &a, CvPoint const &b); + + /// \fn double cvDistanceLinePoint(CvPoint const &a, CvPoint const &b, CvPoint const &c, bool isSegment=true) + /// \brief Distance between line ab and point c. + /// \param a First point of the segment. + /// \param b Second point of the segment. + /// \param c Point. + /// \param isSegment If false then the distance will be calculated from the line defined by the points a and b, to the point c. + /// \return Distance between ab and c. + double cvDistanceLinePoint(CvPoint const &a, CvPoint const &b, CvPoint const &c, bool isSegment=true); + + //////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Tracking + + /// \brief Struct that contain information about one track. + /// \see CvID + /// \see CvLabel + struct CvTrack + { + CvID id; ///< Track identification number. + + CvLabel label; ///< Label assigned to the blob related to this track. + + unsigned int minx; ///< X min. + unsigned int maxx; ///< X max. + unsigned int miny; ///< Y min. + unsigned int maxy; ///< y max. + + CvPoint2D64f centroid; ///< Centroid. + + unsigned int lifetime; ///< Indicates how much frames the object has been in scene. + unsigned int active; ///< Indicates number of frames that has been active from last inactive period. + unsigned int inactive; ///< Indicates number of frames that has been missing. + }; + + /// \var typedef std::map CvTracks + /// \brief List of tracks. + /// \see CvID + /// \see CvTrack + typedef std::map CvTracks; + + /// \var typedef std::pair CvIDTrack + /// \brief Pair (identification number, track). + /// \see CvID + /// \see CvTrack + typedef std::pair CvIDTrack; + + /// \fn inline void cvReleaseTracks(CvTracks &tracks) + /// \brief Clear tracks structure. + /// \param tracks List of tracks. + /// \see CvTracks + inline void cvReleaseTracks(CvTracks &tracks) + { + for (CvTracks::iterator it=tracks.begin(); it!=tracks.end(); it++) + { + CvTrack *track = (*it).second; + if (track) delete track; + } + + tracks.clear(); + } + + /// \fn cvUpdateTracks(CvBlobs const &b, CvTracks &t, const double thDistance, const unsigned int thInactive, const unsigned int thActive=0) + /// \brief Updates list of tracks based on current blobs. + /// Tracking based on: + /// A. Senior, A. Hampapur, Y-L Tian, L. Brown, S. Pankanti, R. Bolle. Appearance Models for + /// Occlusion Handling. Second International workshop on Performance Evaluation of Tracking and + /// Surveillance Systems & CVPR'01. December, 2001. + /// (http://www.research.ibm.com/peoplevision/PETS2001.pdf) + /// \param b List of blobs. + /// \param t List of tracks. + /// \param thDistance Max distance to determine when a track and a blob match. + /// \param thInactive Max number of frames a track can be inactive. + /// \param thActive If a track becomes inactive but it has been active less than thActive frames, the track will be deleted. + /// \see CvBlobs + /// \see Tracks + void cvUpdateTracks(CvBlobs const &b, CvTracks &t, const double thDistance, const unsigned int thInactive, const unsigned int thActive=0); + +#define CV_TRACK_RENDER_ID 0x0001 ///< Print the ID of each track in the image. \see cvRenderTracks +#define CV_TRACK_RENDER_BOUNDING_BOX 0x0002 ///< Draw bounding box of each track in the image. \see cvRenderTracks +#define CV_TRACK_RENDER_TO_LOG 0x0010 ///< Print track info to log out. \see cvRenderTracks +#define CV_TRACK_RENDER_TO_STD 0x0020 ///< Print track info to log out. \see cvRenderTracks + + /// \fn void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode=0x00ff, CvFont *font=NULL) + /// \brief Prints tracks information. + /// \param tracks List of tracks. + /// \param imgSource Input image (depth=IPL_DEPTH_8U and num. channels=3). + /// \param imgDest Output image (depth=IPL_DEPTH_8U and num. channels=3). + /// \param mode Render mode. By default is CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX. + /// \param font OpenCV font for print on the image. + /// \see CV_TRACK_RENDER_ID + /// \see CV_TRACK_RENDER_BOUNDING_BOX + /// \see CV_TRACK_RENDER_TO_LOG + /// \see CV_TRACK_RENDER_TO_STD + void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode=0x000f, CvFont *font=NULL); + } +#ifdef __cplusplus +} +#endif + +/// \fn std::ostream& operator<< (std::ostream& output, const cvb::CvBlob& b) +/// \brief Overload operator "<<" for printing blob structure. +/// \return Stream. +std::ostream& operator<< (std::ostream& output, const cvb::CvBlob& b); + +/// \fn std::ostream& operator<< (std::ostream& output, const cvb::CvContourPolygon& p) +/// \brief Overload operator "<<" for printing polygons in CSV format. +/// \return Stream. +std::ostream& operator<< (std::ostream& output, const cvb::CvContourPolygon& p); + +/// \fn std::ostream& operator<< (std::ostream& output, const cvb::CvTrack& t) +/// \brief Overload operator "<<" for printing track structure. +/// \return Stream. +std::ostream& operator<< (std::ostream& output, const cvb::CvTrack& t); +#endif diff --git a/.svn/pristine/f3/f3a0fc96a7a862ec693ee2515e2d69422a0950bc.svn-base b/.svn/pristine/f3/f3a0fc96a7a862ec693ee2515e2d69422a0950bc.svn-base new file mode 100644 index 0000000..6d68899 --- /dev/null +++ b/.svn/pristine/f3/f3a0fc96a7a862ec693ee2515e2d69422a0950bc.svn-base @@ -0,0 +1,103 @@ +/** + * @file cvpmd.h + * @brief pmdaccess wrappers, PMD -> OpenCV marshalling and a bit of math. + * @author Stanislav `Cfr` Serebryakov + */ +#pragma once + +#include "pmdsdk2.h" +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +/** + * @brief Structure for PMD IO, see initPMD. + */ +typedef struct PMD { + PMDHandle hnd; /**< handle for pmd IO */ + float *data; /**< image quering buffer */ + PMDDataDescription dd; /**< contains image size, integration time, etc */ +} PMD; + +/** + * PMD Constructor + * @param *plugin IO plugin name i.e. o3d.L32.pcp + * @param *ip PMD camera IP address + * @return initialized PMD struct + */ +PMD *initPMD(const char* plugin, const char *ip); + +/** + * Destructor + */ +void releasePMD(PMD **pmd); + + +/** + * Returns pointer to the data (used to save float data for offline mode) + */ +float *pmdDataPtr(PMD *p); + + +/** + * Converts PMD data (stored in PMD struct) to provided IplImage (new will be created if NULL passed) + * @param *p initialized PMD struct with grabbed data (i.e. pmdGetIntensities(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float))) + * @param *img destination IplImage (default: 0, new one will be created) + * @return generated IplImage + */ +IplImage *toIplImage(const PMD *p, IplImage *img); + +/** + * Converts PMD distance point to cartesian coords with intrinsic matrix + * @param uv PMD depth point coords (row, col) //FIXME: order! + * @param dist PMD distance for given point + * @param intrinsicMatrix PMD camera' intrinsic matrix (from calibration) + * @return 3D Point in OpenCV format (CvPoint3D32f) + */ + +CvPoint3D32f *cvProjectArrayToCartesian( const CvMat *intrinsicMatrix + , const CvPoint2D32f *pts, const int ptsCnt + , CvPoint3D32f *unitVs); + + +CvPoint3D32f **pmdProjectArrayToCartesian(const PMD *p, const CvMat *intrinsicMatrix, CvPoint3D32f **pts); + +/** + * Get pmd camera' image size + * @param *p initialized PMD struct + * @return image size in OpenCV format (CvSize) + */ +CvSize pmdGetSize(const PMD *p); + +/** + * Query PMD inrensities image. Dont forget to call pmdUpdate before quering! + * @param *p initialized PMD struct + * @param *img destination IplImage (defaul: 0, new one will be created if null passed) + * @return image in OpenCV format (IplImage) + */ +IplImage *pmdQueryImage(PMD *p, IplImage *img); +IplImage *pmdQueryImageAsync(PMD *p, IplImage *img); + +/** + * Store PMD distances (inside p->data) + */ +void pmdRetriveDistances(PMD *p); +void pmdRetriveDistancesAsync(PMD *p); + + +/** + * Query PMD distance image in OpenCV format + */ +IplImage *pmdQueryDistances(PMD *p, IplImage *img); +IplImage *pmdQueryDistancesAsync(PMD *p, IplImage *img); + +/** + * Query PMD aplitudes image in OpenCV format (might be used as belief measure) + */ +IplImage *pmdQueryAmplitudes(PMD *p, IplImage *img); +IplImage *pmdQueryAmplitudesAsync(PMD *p, IplImage *img); + diff --git a/.svn/pristine/f4/f48c644c7b43095857378c88f8a3ce6b029e13f7.svn-base b/.svn/pristine/f4/f48c644c7b43095857378c88f8a3ce6b029e13f7.svn-base new file mode 100644 index 0000000..94bc096 --- /dev/null +++ b/.svn/pristine/f4/f48c644c7b43095857378c88f8a3ce6b029e13f7.svn-base @@ -0,0 +1,862 @@ +/* + * scan implementation + * + * Copyright (C) Andreas Nuechter, Kai Lingemann, Dorit Borrmann, Jan Elseberg, Thomas Escher + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/scan.h" + +#include "slam6d/basicScan.h" +#include "slam6d/managedScan.h" +#include "slam6d/metaScan.h" +#include "slam6d/searchTree.h" +#include "slam6d/kd.h" +#include "slam6d/Boctree.h" +#include "slam6d/globals.icc" + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif + +#ifdef _MSC_VER +#define _NO_PARALLEL_READ +#endif + +#ifdef __APPLE__ +#define _NO_PARALLEL_READ +#endif + +using std::vector; + + +vector Scan::allScans; +bool Scan::scanserver = false; + + +void Scan::openDirectory(bool scanserver, const std::string& path, IOType type, + int start, int end) +{ + Scan::scanserver = scanserver; + if(scanserver) + ManagedScan::openDirectory(path, type, start, end); + else + BasicScan::openDirectory(path, type, start, end); +} + +void Scan::closeDirectory() +{ + if(scanserver) + ManagedScan::closeDirectory(); + else + BasicScan::closeDirectory(); +} + +Scan::Scan() +{ + unsigned int i; + + // pose and transformations + for(i = 0; i < 3; ++i) rPos[i] = 0; + for(i = 0; i < 3; ++i) rPosTheta[i] = 0; + for(i = 0; i < 4; ++i) rQuat[i] = 0; + M4identity(transMat); + M4identity(transMatOrg); + M4identity(dalignxf); + + // trees and reduction methods + cuda_enabled = false; + nns_method = -1; + kd = 0; + ann_kd_tree = 0; + + // reduction on-demand + reduction_voxelSize = 0.0; + reduction_nrpts = 0; + reduction_pointtype = PointType(); + + // flags + m_has_reduced = false; + + // octtree + octtree_reduction_voxelSize = 0.0; + octtree_voxelSize = 0.0; + octtree_pointtype = PointType(); + octtree_loadOct = false; + octtree_saveOct = false; +} + +Scan::~Scan() +{ + if(kd) delete kd; +} + +void Scan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype) +{ + reduction_voxelSize = voxelSize; + reduction_nrpts = nrpts; + reduction_pointtype = pointtype; +} + +void Scan::setSearchTreeParameter(int nns_method, bool cuda_enabled) +{ + searchtree_nnstype = nns_method; + searchtree_cuda_enabled = cuda_enabled; +} + +void Scan::setOcttreeParameter(double reduction_voxelSize, double voxelSize, PointType pointtype, bool loadOct, bool saveOct) +{ + octtree_reduction_voxelSize = reduction_voxelSize; + octtree_voxelSize = voxelSize; + octtree_pointtype = pointtype; + octtree_loadOct = loadOct; + octtree_saveOct = saveOct; +} + +void Scan::clear(unsigned int types) +{ + if(types & DATA_XYZ) clear("xyz"); + if(types & DATA_RGB) clear("rgb"); + if(types & DATA_REFLECTANCE) clear("reflectance"); + if(types & DATA_TEMPERATURE) clear("temperature"); + if(types & DATA_AMPLITUDE) clear("amplitude"); + if(types & DATA_TYPE) clear("type"); + if(types & DATA_DEVIATION) clear("deviation"); +} + +SearchTree* Scan::getSearchTree() +{ + // if the search tree hasn't been created yet, calculate everything + if(kd == 0) { + createSearchTree(); + } + return kd; +} + +void Scan::toGlobal() { + calcReducedPoints(); + transform(transMatOrg, INVALID); +} + +/** + * Computes a search tree depending on the type. + */ +void Scan::createSearchTree() +{ + // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation + boost::lock_guard lock(m_mutex_create_tree); + if(kd != 0) return; + + // make sure the original points are created before starting the measurement + DataXYZ xyz_orig(get("xyz reduced original")); + +#ifdef WITH_METRICS + Timer tc = ClientMetric::create_tree_time.start(); +#endif //WITH_METRICS + + createSearchTreePrivate(); + +#ifdef WITH_METRICS + ClientMetric::create_tree_time.end(tc); +#endif //WITH_METRICS +} + +void Scan::calcReducedOnDemand() +{ + // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees count as zero) do the reduction + boost::lock_guard lock(m_mutex_reduction); + if(m_has_reduced) return; + +#ifdef WITH_METRICS + Timer t = ClientMetric::on_demand_reduction_time.start(); +#endif //WITH_METRICS + + calcReducedOnDemandPrivate(); + + m_has_reduced = true; + +#ifdef WITH_METRICS + ClientMetric::on_demand_reduction_time.end(t); +#endif //WITH_METRICS +} + +void Scan::copyReducedToOriginal() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::copy_original_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_reduced(get("xyz reduced")); + unsigned int size = xyz_reduced.size(); + DataXYZ xyz_reduced_orig(create("xyz reduced original", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced_orig[i][j] = xyz_reduced[i][j]; + } + } + +#ifdef WITH_METRICS + ClientMetric::copy_original_time.end(t); +#endif //WITH_METRICS +} + +void Scan::copyOriginalToReduced() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::copy_original_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_reduced_orig(get("xyz reduced original")); + unsigned int size = xyz_reduced_orig.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz_reduced_orig[i][j]; + } + } + +#ifdef WITH_METRICS + ClientMetric::copy_original_time.end(t); +#endif //WITH_METRICS +} + + + +/** + * Computes an octtree of the current scan, then getting the + * reduced points as the centers of the octree voxels. + */ +void Scan::calcReducedPoints() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::scan_load_time.start(); +#endif //WITH_METRICS + + + // get xyz to start the scan load, separated here for time measurement + + DataXYZ xyz(get("xyz")); + DataReflectance reflectance(get("reflectance")); + if(xyz.size() == 0) + throw runtime_error("Could not calculate reduced points, XYZ data is empty"); + + if (reflectance.size()==0) { + + // no reduction needed + // copy vector of points to array of points to avoid + // further copying + if(reduction_voxelSize <= 0.0) { + // copy the points + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz[i][j]; + } + } + } else { + // start reduction + // build octree-tree from CurrentScan + // put full data into the octtree + BOctTree *oct = new BOctTree(PointerArray(xyz).get(), + xyz.size(), reduction_voxelSize, reduction_pointtype); + + vector center; + center.clear(); + if (reduction_nrpts > 0) { + if (reduction_nrpts == 1) { + oct->GetOctTreeRandom(center); + } else { + oct->GetOctTreeRandom(center, reduction_nrpts); + } + } else { + oct->GetOctTreeCenter(center); + } + + // storing it as reduced scan + unsigned int size = center.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = center[i][j]; + } + } + delete oct; + } + } else { + if(xyz.size() != reflectance.size()) + throw runtime_error("Could not calculate reduced reflectance, reflectance size is different from points size"); + double **xyz_reflectance = new double*[xyz.size()]; + for (unsigned int i = 0; i < xyz.size(); ++i) { + xyz_reflectance[i] = new double[4]; + for (unsigned int j = 0; j < 3; ++j) + xyz_reflectance[i][j] = xyz[i][j]; + xyz_reflectance[i][3] = reflectance[i]; + } + +#ifdef WITH_METRICS + ClientMetric::scan_load_time.end(t); + Timer tl = ClientMetric::calc_reduced_points_time.start(); +#endif //WITH_METRICS + + // no reduction needed + // copy vector of points to array of points to avoid + // further copying + if(reduction_voxelSize <= 0.0) { + // copy the points + if (reduction_pointtype.hasReflectance()) { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(double)*reflectance.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) + xyz_reduced[i][j] = xyz[i][j]; + reflectance_reduced[i] = reflectance[i]; + } + } else { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz[i][j]; + } + } + } + } else { + // start reduction + // build octree-tree from CurrentScan + // put full data into the octtree + BOctTree *oct = new BOctTree(xyz_reflectance, xyz.size(), reduction_voxelSize, reduction_pointtype); + vector reduced; + reduced.clear(); + + if (reduction_nrpts > 0) { + if (reduction_nrpts == 1) { + oct->GetOctTreeRandom(reduced); + } else { + oct->GetOctTreeRandom(reduced, reduction_nrpts); + } + } else { + oct->GetOctTreeCenter(reduced); + } + + // storing it as reduced scan + unsigned int size = reduced.size(); + if (reduction_pointtype.hasReflectance()) { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = reduced[i][j]; + } + } + DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*size)); + for(unsigned int i = 0; i < size; ++i) + reflectance_reduced[i] = reduced[i][3]; + } else { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) + for(unsigned int j = 0; j < 3; ++j) + xyz_reduced[i][j] = reduced[i][j]; + } + delete oct; + } + for (unsigned int i = 0; i < xyz.size(); ++i) { + delete[] xyz_reflectance[i]; + } + delete[] xyz_reflectance; +#ifdef WITH_METRICS + ClientMetric::calc_reduced_points_time.end(tl); +#endif //WITH_METRICS + } +} + + +/** + * Merges the scan's intrinsic coordinates with the robot position. + * @param prevScan The scan that's transformation is extrapolated, + * i.e., odometry extrapolation + * + * For additional information see the following paper (jfr2007.pdf): + * + * Andreas Nüchter, Kai Lingemann, Joachim Hertzberg, and Hartmut Surmann, + * 6D SLAM - 3D Mapping Outdoor Environments Journal of Field Robotics (JFR), + * Special Issue on Quantitative Performance Evaluation of Robotic and Intelligent + * Systems, Wiley & Son, ISSN 1556-4959, Volume 24, Issue 8-9, pages 699 - 722, + * August/September, 2007 + * + */ +void Scan::mergeCoordinatesWithRoboterPosition(Scan* prevScan) +{ + double tempMat[16], deltaMat[16]; + M4inv(prevScan->get_transMatOrg(), tempMat); + MMult(prevScan->get_transMat(), tempMat, deltaMat); + transform(deltaMat, INVALID); //apply delta transformation of the previous scan +} + +/** + * The method transforms all points with the given transformation matrix. + */ +void Scan::transformAll(const double alignxf[16]) +{ + DataXYZ xyz(get("xyz")); + unsigned int i=0 ; +// #pragma omp parallel for + for(; i < xyz.size(); ++i) { + transform3(alignxf, xyz[i]); + } + // TODO: test for ManagedScan compability, may need a touch("xyz") to mark saving the new values +} + +//! Internal function of transform which alters the reduced points +void Scan::transformReduced(const double alignxf[16]) +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::transform_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_reduced(get("xyz reduced")); + unsigned int i=0; + // #pragma omp parallel for + for( ; i < xyz_reduced.size(); ++i) { + transform3(alignxf, xyz_reduced[i]); + } + +#ifdef WITH_METRICS + ClientMetric::transform_time.end(t); +#endif //WITH_METRICS +} + +//! Internal function of transform which handles the matrices +void Scan::transformMatrix(const double alignxf[16]) +{ + double tempxf[16]; + + // apply alignxf to transMat and update pose vectors + MMult(alignxf, transMat, tempxf); + memcpy(transMat, tempxf, sizeof(transMat)); + Matrix4ToEuler(transMat, rPosTheta, rPos); + Matrix4ToQuat(transMat, rQuat); + +#ifdef DEBUG + cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl; + + cerr << transMat << endl; +#endif + + // apply alignxf to dalignxf + MMult(alignxf, dalignxf, tempxf); + memcpy(dalignxf, tempxf, sizeof(transMat)); +} + +/** + * Transforms the scan by a given transformation and writes a new frame. The idea + * is to write for every transformation in all files, such that the show program + * is able to determine, whcih scans have to be drawn in which color. Hidden scans + * (or later processed scans) are written with INVALID. + * + * @param alignxf Transformation matrix + * @param colour Specifies which colour should the written to the frames file + * @param islum Is the transformtion part of LUM, i.e., all scans are transformed? + * In this case only LUM transformation is stored, otherwise all scans are processed + * -1 no transformation is stored + * 0 ICP transformation + * 1 LUM transformation, all scans except last scan + * 2 LUM transformation, last scan only + */ +void Scan::transform(const double alignxf[16], const AlgoType type, int islum) +{ + MetaScan* meta = dynamic_cast(this); + + if(meta) { + for(unsigned int i = 0; i < meta->size(); ++i) { + meta->getScan(i)->transform(alignxf, type, -1); + } + } + +#ifdef TRANSFORM_ALL_POINTS + transformAll(alignxf); +#endif //TRANSFORM_ALL_POINTS + +#ifdef DEBUG + cerr << alignxf << endl; + cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> "; +#endif + + // transform points + transformReduced(alignxf); + + // update matrices + transformMatrix(alignxf); + + // store transformation in frames + if(type != INVALID) { +#ifdef WITH_METRICS + Timer t = ClientMetric::add_frames_time.start(); +#endif //WITH_METRICS + bool in_meta; + MetaScan* meta = dynamic_cast(this); + int found = 0; + unsigned int scans_size = allScans.size(); + + switch (islum) { + case -1: + // write no tranformation + break; + case 0: + for(unsigned int i = 0; i < scans_size; ++i) { + Scan* scan = allScans[i]; + in_meta = false; + if(meta) { + for(unsigned int j = 0; j < meta->size(); ++j) { + if(meta->getScan(j) == scan) { + found = i; + in_meta = true; + } + } + } + + if(scan == this || in_meta) { + found = i; + scan->addFrame(type); + } else { + if(found == 0) { + scan->addFrame(ICPINACTIVE); + } else { + scan->addFrame(INVALID); + } + } + } + break; + case 1: + addFrame(type); + break; + case 2: + for(unsigned int i = 0; i < scans_size; ++i) { + Scan* scan = allScans[i]; + if(scan == this) { + found = i; + addFrame(type); + allScans[0]->addFrame(type); + continue; + } + if (found != 0) { + scan->addFrame(INVALID); + } + } + break; + default: + cerr << "invalid point transformation mode" << endl; + } + +#ifdef WITH_METRICS + ClientMetric::add_frames_time.end(t); +#endif //WITH_METRICS + } +} + +/** + * Transforms the scan by a given transformation and writes a new frame. The idea + * is to write for every transformation in all files, such that the show program + * is able to determine, whcih scans have to be drawn in which color. Hidden scans + * (or later processed scans) are written with INVALID. + * + * @param alignQuat Quaternion for the rotation + * @param alignt Translation vector + * @param colour Specifies which colour should the written to the frames file + * @param islum Is the transformtion part of LUM, i.e., all scans are transformed? + * In this case only LUM transformation is stored, otherwise all scans are processed + * -1 no transformation is stored + * 0 ICP transformation + * 1 LUM transformation, all scans except last scan + * 2 LUM transformation, last scan only + */ +void Scan::transform(const double alignQuat[4], const double alignt[3], + const AlgoType type, int islum) +{ + double alignxf[16]; + QuatToMatrix4(alignQuat, alignt, alignxf); + transform(alignxf, type, islum); +} + +/** + * Transforms the scan, so that the given Matrix + * prepresent the next pose. + * + * @param alignxf Transformation matrix to which this scan will be set to + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToMatrix(double alignxf[16], const AlgoType type, int islum) +{ + double tinv[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + transform(alignxf, type, islum); +} + +/** + * Transforms the scan, so that the given Euler angles + * prepresent the next pose. + * + * @param rP Translation to which this scan will be set to + * @param rPT Orientation as Euler angle to which this scan will be set + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToEuler(double rP[3], double rPT[3], const AlgoType type, int islum) +{ +#ifdef WITH_METRICS + // called in openmp context in lum6Deuler.cc:422 + ClientMetric::transform_time.set_threadsafety(true); + ClientMetric::add_frames_time.set_threadsafety(true); +#endif //WITH_METRICS + + double tinv[16]; + double alignxf[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + EulerToMatrix4(rP, rPT, alignxf); + transform(alignxf, type, islum); + +#ifdef WITH_METRICS + ClientMetric::transform_time.set_threadsafety(false); + ClientMetric::add_frames_time.set_threadsafety(false); +#endif //WITH_METRICS +} + +/** + * Transforms the scan, so that the given Euler angles + * prepresent the next pose. + * + * @param rP Translation to which this scan will be set to + * @param rPQ Orientation as Quaternion to which this scan will be set + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int islum) +{ + double tinv[16]; + double alignxf[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + QuatToMatrix4(rPQ, rP, alignxf); + transform(alignxf, type, islum); +} + +/** + * Calculates Source\Target + * Calculates a set of corresponding point pairs and returns them. It + * computes the k-d trees and deletes them after the pairs have been + * found. This slow function should be used only for testing + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + */ + +void Scan::getNoPairsSimple(vector &diff, + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2) +{ + DataXYZ xyz_reduced(Source->get("xyz reduced")); + KDtree* kd = new KDtree(PointerArray(Target->get("xyz reduced")).get(), Target->size("xyz reduced")); + + cout << "Max: " << max_dist_match2 << endl; + for (unsigned int i = 0; i < xyz_reduced.size(); i++) { + + double p[3]; + p[0] = xyz_reduced[i][0]; + p[1] = xyz_reduced[i][1]; + p[2] = xyz_reduced[i][2]; + + + double *closest = kd->FindClosest(p, max_dist_match2, thread_num); + if (!closest) { + diff.push_back(xyz_reduced[i]); + //diff.push_back(closest); + } + } + + delete kd; +} + +/** + * Calculates a set of corresponding point pairs and returns them. It + * computes the k-d trees and deletes them after the pairs have been + * found. This slow function should be used only for testing + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + */ +void Scan::getPtPairsSimple(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, + double *centroid_m, double *centroid_d) +{ + KDtree* kd = new KDtree(PointerArray(Source->get("xyz reduced")).get(), Source->size("xyz reduced")); + DataXYZ xyz_reduced(Target->get("xyz reduced")); + + for (unsigned int i = 0; i < xyz_reduced.size(); i++) { + if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only + + double p[3]; + p[0] = xyz_reduced[i][0]; + p[1] = xyz_reduced[i][1]; + p[2] = xyz_reduced[i][2]; + + double *closest = kd->FindClosest(p, max_dist_match2, thread_num); + if (closest) { + centroid_m[0] += closest[0]; + centroid_m[1] += closest[1]; + centroid_m[2] += closest[2]; + centroid_d[0] += p[0]; + centroid_d[1] += p[1]; + centroid_d[2] += p[2]; + PtPair myPair(closest, p); + pairs->push_back(myPair); + } + } + centroid_m[0] /= pairs[thread_num].size(); + centroid_m[1] /= pairs[thread_num].size(); + centroid_m[2] /= pairs[thread_num].size(); + centroid_d[0] /= pairs[thread_num].size(); + centroid_d[1] /= pairs[thread_num].size(); + centroid_d[2] /= pairs[thread_num].size(); + + delete kd; +} + + +/** + * Calculates a set of corresponding point pairs and returns them. + * The function uses the k-d trees stored the the scan class, thus + * the function createTrees and deletTrees have to be called before + * resp. afterwards. + * Here we implement the so called "fast corresponding points"; k-d + * trees are not recomputed, instead the apply the inverse transformation + * to to the given point set. + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + * @return a set of corresponding point pairs + */ +void Scan::getPtPairs(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d) +{ + // initialize centroids + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[i] = 0; + centroid_d[i] = 0; + } + + // get point pairs + DataXYZ xyz_reduced(Target->get("xyz reduced")); + Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf, + xyz_reduced, 0, xyz_reduced.size(), + thread_num, + rnd, max_dist_match2, sum, centroid_m, centroid_d); + + // normalize centroids + unsigned int size = pairs->size(); + if(size != 0) { + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[i] /= size; + centroid_d[i] /= size; + } + } +} + + +/** + * Calculates a set of corresponding point pairs and returns them. + * The function uses the k-d trees stored the the scan class, thus + * the function createTrees and delteTrees have to be called before + * resp. afterwards. + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num The number of the thread that is computing ptPairs in parallel + * @param step The number of steps for parallelization + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + * @param sum The sum of distances of the points + * + * These intermediate values are for the parallel ICP algorithm + * introduced in the paper + * "The Parallel Iterative Closest Point Algorithm" + * by Langis / Greenspan / Godin, IEEE 3DIM 2001 + * + */ +void Scan::getPtPairsParallel(vector *pairs, Scan* Source, Scan* Target, + int thread_num, int step, + int rnd, double max_dist_match2, + double *sum, + double centroid_m[OPENMP_NUM_THREADS][3], double centroid_d[OPENMP_NUM_THREADS][3]) +{ + // initialize centroids + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[thread_num][i] = 0; + centroid_d[thread_num][i] = 0; + } + + // get point pairs + SearchTree* search = Source->getSearchTree(); + // differentiate between a meta scan (which has no reduced points) and a normal scan + // if Source is also a meta scan it already has a special meta-kd-tree + MetaScan* meta = dynamic_cast(Target); + if(meta) { + for(unsigned int i = 0; i < meta->size(); ++i) { + // determine step for each scan individually + DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced")); + unsigned int max = xyz_reduced.size(); + unsigned int step = max / OPENMP_NUM_THREADS; + // call ptpairs for each scan and accumulate ptpairs, centroids and sum + search->getPtPairs(&pairs[thread_num], Source->dalignxf, + xyz_reduced, step * thread_num, step * thread_num + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num]); + } + } else { + DataXYZ xyz_reduced(Target->get("xyz reduced")); + search->getPtPairs(&pairs[thread_num], Source->dalignxf, + xyz_reduced, thread_num * step, thread_num * step + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num]); + } + + // normalize centroids + unsigned int size = pairs[thread_num].size(); + if(size != 0) { + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[thread_num][i] /= size; + centroid_d[thread_num][i] /= size; + } + } +} + +unsigned int Scan::getMaxCountReduced(ScanVector& scans) +{ + unsigned int max = 0; + for(std::vector::iterator it = scans.begin(); it != scans.end(); ++it) { + unsigned int count = (*it)->size("xyz reduced"); + if(count > max) + max = count; + } + return max; +} diff --git a/.svn/pristine/fb/fba02707e5358483632ce56d6e7727cebe506071.svn-base b/.svn/pristine/fb/fba02707e5358483632ce56d6e7727cebe506071.svn-base new file mode 100644 index 0000000..1876e6a --- /dev/null +++ b/.svn/pristine/fb/fba02707e5358483632ce56d6e7727cebe506071.svn-base @@ -0,0 +1,443 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +#include +#include +using namespace std; + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "cvblob.h" + +namespace cvb +{ + const char movesE[4][3][4] = { { {-1, -1, 3, CV_CHAINCODE_UP_LEFT }, { 0, -1, 0, CV_CHAINCODE_UP }, { 1, -1, 0, CV_CHAINCODE_UP_RIGHT } }, + { { 1, -1, 0, CV_CHAINCODE_UP_RIGHT }, { 1, 0, 1, CV_CHAINCODE_RIGHT}, { 1, 1, 1, CV_CHAINCODE_DOWN_RIGHT } }, + { { 1, 1, 1, CV_CHAINCODE_DOWN_RIGHT}, { 0, 1, 2, CV_CHAINCODE_DOWN }, {-1, 1, 2, CV_CHAINCODE_DOWN_LEFT } }, + { {-1, 1, 2, CV_CHAINCODE_DOWN_LEFT }, {-1, 0, 3, CV_CHAINCODE_LEFT }, {-1, -1, 3, CV_CHAINCODE_UP_LEFT } } + }; + + const char movesI[4][3][4] = { { { 1, -1, 3, CV_CHAINCODE_UP_RIGHT }, { 0, -1, 0, CV_CHAINCODE_UP }, {-1, -1, 0, CV_CHAINCODE_UP_LEFT } }, + { {-1, -1, 0, CV_CHAINCODE_UP_LEFT }, {-1, 0, 1, CV_CHAINCODE_LEFT }, {-1, 1, 1, CV_CHAINCODE_DOWN_LEFT } }, + { {-1, 1, 1, CV_CHAINCODE_DOWN_LEFT }, { 0, 1, 2, CV_CHAINCODE_DOWN }, { 1, 1, 2, CV_CHAINCODE_DOWN_RIGHT } }, + { { 1, 1, 2, CV_CHAINCODE_DOWN_RIGHT }, { 1, 0, 3, CV_CHAINCODE_RIGHT}, { 1, -1, 3, CV_CHAINCODE_UP_RIGHT } } + }; + + + unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs) + { + CV_FUNCNAME("cvLabel"); + __CV_BEGIN__; + { + CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==1)); + CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_LABEL)&&(img->nChannels==1)); + + unsigned int numPixels=0; + + cvSetZero(imgOut); + + CvLabel label=0; + cvReleaseBlobs(blobs); + + unsigned int stepIn = img->widthStep / (img->depth / 8); + unsigned int stepOut = imgOut->widthStep / (imgOut->depth / 8); + unsigned int imgIn_width = img->width; + unsigned int imgIn_height = img->height; + unsigned int imgIn_offset = 0; + unsigned int imgOut_width = imgOut->width; + unsigned int imgOut_height = imgOut->height; + unsigned int imgOut_offset = 0; + if(img->roi) + { + imgIn_width = img->roi->width; + imgIn_height = img->roi->height; + imgIn_offset = img->roi->xOffset + (img->roi->yOffset * stepIn); + } + if(imgOut->roi) + { + imgOut_width = imgOut->roi->width; + imgOut_height = imgOut->roi->height; + imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut); + } + + unsigned char *imgDataIn = (unsigned char *)img->imageData + imgIn_offset; + CvLabel *imgDataOut = (CvLabel *)imgOut->imageData + imgOut_offset; + +#define imageIn(X, Y) imgDataIn[(X) + (Y)*stepIn] +#define imageOut(X, Y) imgDataOut[(X) + (Y)*stepOut] + + CvLabel lastLabel = 0; + CvBlob *lastBlob = NULL; + + for (unsigned int y=0; y0) + imageOut(x, y-1) = CV_BLOB_MAX_LABEL; + + CvBlob *blob = new CvBlob; + blob->label = label; + blob->area = 1; + blob->minx = x; blob->maxx = x; + blob->miny = y; blob->maxy = y; + blob->m10=x; blob->m01=y; + blob->m11=x*y; + blob->m20=x*x; blob->m02=y*y; + blob->internalContours.clear(); + blobs.insert(CvLabelBlob(label,blob)); + + lastLabel = label; + lastBlob = blob; + + blob->contour.startingPoint = cvPoint(x, y); + + unsigned char direction=1; + unsigned int xx = x; + unsigned int yy = y; + + + bool contourEnd = false; + + do + { + for (unsigned int numAttempts=0; numAttempts<3; numAttempts++) + { + bool found = false; + + for (unsigned char i=0; i<3; i++) + { + int nx = xx+movesE[direction][i][0]; + int ny = yy+movesE[direction][i][1]; + if ((nx=0)&&(ny=0)) + { + if (imageIn(nx, ny)) + { + found = true; + + blob->contour.chainCode.push_back(movesE[direction][i][3]); + + xx=nx; + yy=ny; + + direction=movesE[direction][i][2]; + break; + } + else + { + imageOut(nx, ny) = CV_BLOB_MAX_LABEL; + } + } + } + + if (!found) + direction=(direction+1)%4; + else + { + imageOut(xx, yy) = label; + numPixels++; + + if (xxminx) blob->minx = xx; + else if (xx>blob->maxx) blob->maxx = xx; + if (yyminy) blob->miny = yy; + else if (yy>blob->maxy) blob->maxy = yy; + + blob->area++; + blob->m10+=xx; blob->m01+=yy; + blob->m11+=xx*yy; + blob->m20+=xx*xx; blob->m02+=yy*yy; + + break; + } + + if (contourEnd = ((xx==x) && (yy==y) && (direction==1))) + break; + } + } + while (!contourEnd); + + } + + if ((y+1second; + lastLabel = l; + lastBlob = blob; + } + blob->area++; + blob->m10+=x; blob->m01+=y; + blob->m11+=x*y; + blob->m20+=x*x; blob->m02+=y*y; + } + else + { + l = imageOut(x, y); + + if (l==lastLabel) + blob = lastBlob; + else + { + blob = blobs.find(l)->second; + lastLabel = l; + lastBlob = blob; + } + } + + // XXX This is not necessary (I believe). I only do this for consistency. + imageOut(x, y+1) = CV_BLOB_MAX_LABEL; + + CvContourChainCode *contour = new CvContourChainCode; + contour->startingPoint = cvPoint(x, y); + + unsigned char direction = 3; + unsigned int xx = x; + unsigned int yy = y; + + do + { + for (unsigned int numAttempts=0; numAttempts<3; numAttempts++) + { + bool found = false; + + for (unsigned char i=0; i<3; i++) + { + int nx = xx+movesI[direction][i][0]; + int ny = yy+movesI[direction][i][1]; + if (imageIn(nx, ny)) + { + found = true; + + contour->chainCode.push_back(movesI[direction][i][3]); + + xx=nx; + yy=ny; + + direction=movesI[direction][i][2]; + break; + } + else + { + imageOut(nx, ny) = CV_BLOB_MAX_LABEL; + } + } + + if (!found) + direction=(direction+1)%4; + else + { + if (!imageOut(xx, yy)) + { + imageOut(xx, yy) = l; + numPixels++; + + blob->area++; + blob->m10+=xx; blob->m01+=yy; + blob->m11+=xx*yy; + blob->m20+=xx*xx; blob->m02+=yy*yy; + } + + break; + } + } + } + while (!(xx==x && yy==y)); + + blob->internalContours.push_back(contour); + } + + //else if (!imageOut(x, y)) + if (!labeled) + { + // Internal pixel + CvLabel l = imageOut(x-1, y); + + imageOut(x, y) = l; + numPixels++; + + CvBlob *blob = NULL; + if (l==lastLabel) + blob = lastBlob; + else + { + blob = blobs.find(l)->second; + lastLabel = l; + lastBlob = blob; + } + blob->area++; + blob->m10+=x; blob->m01+=y; + blob->m11+=x*y; + blob->m20+=x*x; blob->m02+=y*y; + } + } + } + } + + for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it) + { + cvCentroid((*it).second); + + (*it).second->u11 = (*it).second->m11 - ((*it).second->m10*(*it).second->m01)/(*it).second->m00; + (*it).second->u20 = (*it).second->m20 - ((*it).second->m10*(*it).second->m10)/(*it).second->m00; + (*it).second->u02 = (*it).second->m02 - ((*it).second->m01*(*it).second->m01)/(*it).second->m00; + + double m00_2 = (*it).second->m00 * (*it).second->m00; + + (*it).second->n11 = (*it).second->u11 / m00_2; + (*it).second->n20 = (*it).second->u20 / m00_2; + (*it).second->n02 = (*it).second->u02 / m00_2; + + (*it).second->p1 = (*it).second->n20 + (*it).second->n02; + + double nn = (*it).second->n20 - (*it).second->n02; + (*it).second->p2 = nn*nn + 4.*((*it).second->n11*(*it).second->n11); + } + + return numPixels; + + } + __CV_END__; + } + + void cvFilterLabels(IplImage *imgIn, IplImage *imgOut, const CvBlobs &blobs) + { + CV_FUNCNAME("cvFilterLabels"); + __CV_BEGIN__; + { + CV_ASSERT(imgIn&&(imgIn->depth==IPL_DEPTH_LABEL)&&(imgIn->nChannels==1)); + CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_8U)&&(imgOut->nChannels==1)); + + int stepIn = imgIn->widthStep / (imgIn->depth / 8); + int stepOut = imgOut->widthStep / (imgOut->depth / 8); + int imgIn_width = imgIn->width; + int imgIn_height = imgIn->height; + int imgIn_offset = 0; + int imgOut_width = imgOut->width; + int imgOut_height = imgOut->height; + int imgOut_offset = 0; + if(imgIn->roi) + { + imgIn_width = imgIn->roi->width; + imgIn_height = imgIn->roi->height; + imgIn_offset = imgIn->roi->xOffset + (imgIn->roi->yOffset * stepIn); + } + if(imgOut->roi) + { + imgOut_width = imgOut->roi->width; + imgOut_height = imgOut->roi->height; + imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut); + } + + char *imgDataOut=imgOut->imageData + imgOut_offset; + CvLabel *imgDataIn=(CvLabel *)imgIn->imageData + imgIn_offset; + + for (unsigned int r=0;r<(unsigned int)imgIn_height;r++, + imgDataIn+=stepIn,imgDataOut+=stepOut) + { + for (unsigned int c=0;c<(unsigned int)imgIn_width;c++) + { + if (imgDataIn[c]) + { + if (blobs.find(imgDataIn[c])==blobs.end()) imgDataOut[c]=0x00; + else imgDataOut[c]=(char)0xff; + } + else + imgDataOut[c]=0x00; + } + } + } + __CV_END__; + } + + + CvLabel cvGetLabel(IplImage const *img, unsigned int x, unsigned int y) + { + CV_FUNCNAME("cvGetLabel"); + __CV_BEGIN__; + { + CV_ASSERT(img&&(img->depth==IPL_DEPTH_LABEL)&&(img->nChannels==1)); + + int step = img->widthStep / (img->depth / 8); + int img_width = 0; + int img_height= 0; + int img_offset = 0; + if(img->roi) + { + img_width = img->roi->width; + img_height = img->roi->height; + img_offset = img->roi->xOffset + (img->roi->yOffset * step); + } + else + { + img_width = img->width; + img_height= img->height; + } + + CV_ASSERT((x>=0)&&(x=0)&&(yimageData + img_offset))[x + y*step]; + } + __CV_END__; + } + +} diff --git a/.svn/pristine/fc/fc9624dd4f2296d23aa07507a6d4cced05b0e03d.svn-base b/.svn/pristine/fc/fc9624dd4f2296d23aa07507a6d4cced05b0e03d.svn-base new file mode 100644 index 0000000..4bb2f8c --- /dev/null +++ b/.svn/pristine/fc/fc9624dd4f2296d23aa07507a6d4cced05b0e03d.svn-base @@ -0,0 +1,106 @@ +// Copyright (C) 2007 by Cristóbal Carnero Liñán +// grendel.ccl@gmail.com +// +// This file is part of cvBlob. +// +// cvBlob is free software: you can redistribute it and/or modify +// it under the terms of the Lesser GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// cvBlob is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// Lesser GNU General Public License for more details. +// +// You should have received a copy of the Lesser GNU General Public License +// along with cvBlob. If not, see . +// + +#include +#include +using namespace std; + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + +#include "cvblob.h" + +namespace cvb +{ + + CvScalar cvBlobMeanColor(CvBlob const *blob, IplImage const *imgLabel, IplImage const *img) + { + CV_FUNCNAME("cvBlobMeanColor"); + __CV_BEGIN__; + { + CV_ASSERT(imgLabel&&(imgLabel->depth==IPL_DEPTH_LABEL)&&(imgLabel->nChannels==1)); + CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==3)); + + int stepLbl = imgLabel->widthStep/(imgLabel->depth/8); + int stepImg = img->widthStep/(img->depth/8); + int imgLabel_width = imgLabel->width; + int imgLabel_height = imgLabel->height; + int imgLabel_offset = 0; + int img_width = img->width; + int img_height = img->height; + int img_offset = 0; + if(imgLabel->roi) + { + imgLabel_width = imgLabel->roi->width; + imgLabel_height = imgLabel->roi->height; + imgLabel_offset = (imgLabel->nChannels * imgLabel->roi->xOffset) + (imgLabel->roi->yOffset * stepLbl); + } + if(img->roi) + { + img_width = img->roi->width; + img_height = img->roi->height; + img_offset = (img->nChannels * img->roi->xOffset) + (img->roi->yOffset * stepImg); + } + + CvLabel *labels = (CvLabel *)imgLabel->imageData + imgLabel_offset; + unsigned char *imgData = (unsigned char *)img->imageData + img_offset; + + double mb = 0; + double mg = 0; + double mr = 0; + double pixels = (double)blob->area; + + for (unsigned int r=0; r<(unsigned int)imgLabel_height; r++, labels+=stepLbl, imgData+=stepImg) + for (unsigned int c=0; c<(unsigned int)imgLabel_width; c++) + { + if (labels[c]==blob->label) + { + mb += ((double)imgData[img->nChannels*c+0])/pixels; // B + mg += ((double)imgData[img->nChannels*c+1])/pixels; // G + mr += ((double)imgData[img->nChannels*c+2])/pixels; // R + } + } + + /*double mb = 0; + double mg = 0; + double mr = 0; + double pixels = (double)blob->area; + for (unsigned int y=0; yheight; y++) + for (unsigned int x=0; xwidth; x++) + { + if (cvGetLabel(imgLabel, x, y)==blob->label) + { + CvScalar color = cvGet2D(img, y, x); + mb += color.val[0]/pixels; + mg += color.val[1]/pixels; + mr += color.val[2]/pixels; + } + }*/ + + return cvScalar(mr, mg, mb); + } + __CV_END__; + } + +} diff --git a/.svn/wc.db b/.svn/wc.db index 6e7c8fd..66e7bfd 100644 Binary files a/.svn/wc.db and b/.svn/wc.db differ diff --git a/3rdparty/CMakeModules/OpenCV.cmake b/3rdparty/CMakeModules/OpenCV.cmake new file mode 100644 index 0000000..ecec8d8 --- /dev/null +++ b/3rdparty/CMakeModules/OpenCV.cmake @@ -0,0 +1,15 @@ + find_package(OpenCV REQUIRED) + if(EXISTS "${OpenCV_DIR}/OpenCVConfig.cmake") + include("${OpenCV_DIR}/OpenCVConfig.cmake") + set(ADDITIONAL_OPENCV_FLAGS + "-DCV_MINOR_VERSION=${OpenCV_VERSION_MINOR} -DCV_MAJOR_VERSION=${OpenCV_VERSION_MAJOR}" + CACHE STRING"OpenCV Version Defines)" + ) + ## Include the standard CMake script + ELSE(EXISTS "${OpenCV_DIR}/OpenCVConfig.cmake") + set(ADDITIONAL_OPENCV_FLAGS + "" + CACHE STRING"OpenCV Version Defines (BLUB)" + ) + endif(EXISTS "${OpenCV_DIR}/OpenCVConfig.cmake") + diff --git a/3rdparty/cvblob/CMakeLists.txt b/3rdparty/cvblob/CMakeLists.txt index 2837e95..1820544 100644 --- a/3rdparty/cvblob/CMakeLists.txt +++ b/3rdparty/cvblob/CMakeLists.txt @@ -33,7 +33,7 @@ set(CVBLOB_SRCS set_source_files_properties(${cvBlob_SRC} PROPERTIES - COMPILE_FLAGS "-O3" + COMPILE_FLAGS "-O3 ${ADDITIONAL_OPENCV_FLAGS}" ) add_library(cvblob STATIC ${CVBLOB_SRCS}) diff --git a/3rdparty/cvblob/cvaux.cpp b/3rdparty/cvblob/cvaux.cpp index 0404c88..cf30654 100644 --- a/3rdparty/cvblob/cvaux.cpp +++ b/3rdparty/cvblob/cvaux.cpp @@ -21,8 +21,10 @@ #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include +#else +#include #endif #include "cvblob.h" diff --git a/3rdparty/cvblob/cvblob.cpp b/3rdparty/cvblob/cvblob.cpp index 83126d8..9296166 100644 --- a/3rdparty/cvblob/cvblob.cpp +++ b/3rdparty/cvblob/cvblob.cpp @@ -24,9 +24,11 @@ using namespace std; #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include #include +#else +#include #endif #include "cvblob.h" diff --git a/3rdparty/cvblob/cvblob.h b/3rdparty/cvblob/cvblob.h index f5d7c3c..64880ff 100644 --- a/3rdparty/cvblob/cvblob.h +++ b/3rdparty/cvblob/cvblob.h @@ -38,8 +38,10 @@ #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include +#else +#include #endif #ifndef __CV_BEGIN__ diff --git a/3rdparty/cvblob/cvcolor.cpp b/3rdparty/cvblob/cvcolor.cpp index fda20d4..4bb2f8c 100644 --- a/3rdparty/cvblob/cvcolor.cpp +++ b/3rdparty/cvblob/cvcolor.cpp @@ -23,8 +23,10 @@ using namespace std; #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include +#else +#include #endif #include "cvblob.h" diff --git a/3rdparty/cvblob/cvcontour.cpp b/3rdparty/cvblob/cvcontour.cpp index 71b514d..10803a1 100644 --- a/3rdparty/cvblob/cvcontour.cpp +++ b/3rdparty/cvblob/cvcontour.cpp @@ -30,8 +30,10 @@ using namespace std; #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include +#else +#include #endif #include "cvblob.h" diff --git a/3rdparty/cvblob/cvlabel.cpp b/3rdparty/cvblob/cvlabel.cpp index 70be761..1876e6a 100644 --- a/3rdparty/cvblob/cvlabel.cpp +++ b/3rdparty/cvblob/cvlabel.cpp @@ -23,8 +23,10 @@ using namespace std; #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include +#else +#include #endif #include "cvblob.h" diff --git a/3rdparty/cvblob/cvtrack.cpp b/3rdparty/cvblob/cvtrack.cpp index 6bc8ca1..a14b237 100644 --- a/3rdparty/cvblob/cvtrack.cpp +++ b/3rdparty/cvblob/cvtrack.cpp @@ -24,8 +24,10 @@ using namespace std; #if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include -#else +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include +#else +#include #endif #include "cvblob.h" diff --git a/CMakeLists.txt b/CMakeLists.txt index be5ba8e..9c7bfac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required (VERSION 2.8.2) SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH}) -project (Slam6D) +project (3DTK) + + + #include_directories(OPENGL_INCLUDE_DIR) IF(WIN32) @@ -27,8 +30,75 @@ endif() # Declare Options and modify build accordingly ## ################################################# + +FUNCTION(ENFORCE_OPTION_DEP_3DTK option VALUE) + SET (${option} "${VALUE}" CACHE BOOL "${${option}_DESCRIPTION}" FORCE) # this option set to VALUE as advised + + #now make sure other dependencies are also true + FOREACH(d ${${option}_DEPENDENCIES}) # look through all my dependencies + STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") + + # check for a not in front + STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP) + STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT) + #STRING(REPLACE "NOT " "" CMAKE_DEPENDENT_OPTION_DEP "${d}") + IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT + STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}") + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF) + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON) + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT) + ENDFOREACH(d) + +ENDFUNCTION(ENFORCE_OPTION_DEP_3DTK) + +MACRO(OPT_DEP option doc default depends) + OPTION(${option} "${doc}" "${default}") + SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) + SET(${option}_DEPENDENCIES "${depends}" CACHE INTERNAL "" FORCE) + SET(${option}_DESCRIPTION "${doc}" CACHE INTERNAL "" FORCE) + + IF (${option}) + #MESSAGE(STATUS "Yes ${option} is true") + # MESSAGE("FOREACH d in ${depends}") + + FOREACH(d ${depends}) + STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") + + # check for a not in front + STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP) + STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3) + STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT + STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}") + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF) + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT) + IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met + ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is + ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON) + ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) + ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT) + ENDFOREACH(d) + + ENDIF(${option}) +ENDMACRO(OPT_DEP) + + + ## FreeGLUT -OPTION(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON) +OPT_DEP(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON "") IF(WITH_FREEGLUT) MESSAGE(STATUS "With freeglut") @@ -38,7 +108,7 @@ ELSE(WITH_FREEGLUT) ENDIF(WITH_FREEGLUT) ## Show -OPTION(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON) +OPT_DEP(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON "" ) IF(WITH_SHOW) FIND_PACKAGE(OpenGL REQUIRED) @@ -50,7 +120,7 @@ ELSE(WITH_SHOW) ENDIF(WITH_SHOW) ## WXShow -OPTION(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF) +OPT_DEP(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF "") IF(WITH_WXSHOW) FIND_PACKAGE(OpenGL REQUIRED) @@ -61,12 +131,12 @@ IF(WITH_WXSHOW) # wxWidgets include (this will do all the magic to configure everything) include( ${wxWidgets_USE_FILE}) MESSAGE(STATUS "With wxshow") -ELSE(WITH_XWSHOW) +ELSE(WITH_WXSHOW) MESSAGE(STATUS "Without wxshow") ENDIF(WITH_WXSHOW) ## Shapes -OPTION(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF) +OPT_DEP(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF "") IF(WITH_SHAPE_DETECTION) MESSAGE(STATUS "With shape detection") ELSE(WITH_SHAPE_DETECTION) @@ -74,7 +144,7 @@ ELSE(WITH_SHAPE_DETECTION) ENDIF(WITH_SHAPE_DETECTION) ## Interior reconstruction -option(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF) +OPT_DEP(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF "") if(WITH_MODEL) message(STATUS "With interior reconstruction") @@ -83,9 +153,12 @@ else(WITH_MODEL) endif(WITH_MODEL) ## Thermo -OPTION(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF) +OPT_DEP(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF "WITH_SHAPE_DETECTION") IF(WITH_THERMO) + #for OpenCV 2.1 FIND_PACKAGE(OpenCV REQUIRED) + include("3rdparty/CMakeModules/OpenCV.cmake") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") add_subdirectory(3rdparty/cvblob) include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) @@ -94,21 +167,9 @@ ELSE(WITH_THERMO) MESSAGE(STATUS "Without thermo") ENDIF(WITH_THERMO) -## Octree -OPTION(WITH_OCTREE_DISPLAY "Whether to use octree display for efficiently culling scans ON/OFF" ON) - -IF(WITH_OCTREE_DISPLAY) - MESSAGE(STATUS "Using octree display") - SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_GL_POINTS") -ELSE(WITH_OCTREE_DISPLAY) - MESSAGE(STATUS "Using displaylists: Warning potentially much slower") -ENDIF(WITH_OCTREE_DISPLAY) -#SET (WITH_OCTREE_DISPLAY ${WITH_OCTREE_DISPLAY} CACHE BOOL -#"Whether to use octree display for efficiently culling scans ON/OFF" FORCE) - ## Octree -OPTION(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF) +OPT_DEP(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF "") IF(WITH_COMPACT_OCTREE) MESSAGE(STATUS "Using compact octrees") @@ -118,7 +179,7 @@ ELSE(WITH_COMPACT_OCTREE) ENDIF(WITH_COMPACT_OCTREE) ## Glee? -OPTION(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF) +OPT_DEP(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF "") IF(WITH_GLEE) MESSAGE(STATUS "Using opengl extensions") @@ -128,7 +189,7 @@ ELSE(WITH_GLEE) ENDIF(WITH_GLEE) ## Gridder -OPTION(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF) +OPT_DEP(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF "") IF(WITH_GRIDDER) MESSAGE(STATUS "With 2DGridder") @@ -137,7 +198,7 @@ ELSE(WITH_GRIDDER) ENDIF(WITH_GRIDDER) ## Dynamic VELOSLAM -OPTION(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF) +OPT_DEP(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF "WITH_SHOW") IF(WITH_VELOSLAM) MESSAGE(STATUS "With VELOSLAM") @@ -146,7 +207,7 @@ ELSE(WITH_VELOSLAM) ENDIF(WITH_VELOSLAM) ## Home-made Laserscanner -OPTION(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF) +OPT_DEP(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF "") IF(WITH_DAVID_3D_SCANNER) MESSAGE(STATUS "With David scanner") @@ -156,17 +217,18 @@ ENDIF(WITH_DAVID_3D_SCANNER) ## Tools -OPTION(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF) +OPT_DEP(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF "WITH_FBR") IF(WITH_TOOLS) MESSAGE(STATUS "With Tools") + find_package (Boost COMPONENTS program_options REQUIRED) ELSE(WITH_TOOLS) MESSAGE(STATUS "Without Tools") ENDIF(WITH_TOOLS) ## Segmentation -OPTION(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF) +OPT_DEP(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF "WITH_FBR") IF(WITH_SEGMENTATION) MESSAGE(STATUS "With segmentation") @@ -177,7 +239,7 @@ ENDIF(WITH_SEGMENTATION) ## Normals -OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF) +OPT_DEP(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF "WITH_FBR") IF(WITH_NORMALS) MESSAGE(STATUS "With normals") @@ -185,19 +247,8 @@ ELSE(WITH_NORMALS) MESSAGE(STATUS "Without normals") ENDIF(WITH_NORMALS) -## CAD matching - -OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF) - -IF (WITH_CAD) - MESSAGE (STATUS "With CAD import") - find_package (Boost COMPONENTS program_options filesystem REQUIRED) -ELSE (WITH_CAD) - MESSAGE (STATUS "Without CAD import") -ENDIF (WITH_CAD) - ## RivLib -OPTION(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF) +OPT_DEP(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF "") IF(WITH_RIVLIB) MESSAGE(STATUS "Compiling a scan IO for RXP files") @@ -216,7 +267,7 @@ ELSE(WITH_RIVLIB) ENDIF(WITH_RIVLIB) ## CUDA support, TODO depend on CUDA_FIND -OPTION(WITH_CUDA "Compile with CUDA support" OFF) +OPT_DEP(WITH_CUDA "Compile with CUDA support" OFF "") IF(WITH_CUDA) MESSAGE(STATUS "Compiling WITH CUDA support") FIND_PACKAGE(CUDA) @@ -226,7 +277,7 @@ ELSE(WITH_CUDA) ENDIF(WITH_CUDA) ## PMD -OPTION(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF) +OPT_DEP(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF "") IF(WITH_PMD) FIND_PACKAGE(OpenGL REQUIRED) @@ -236,31 +287,21 @@ ELSE(WITH_PMD) ENDIF(WITH_PMD) ## FBR -OPTION(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF) +OPT_DEP(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF "") IF(WITH_FBR) + FIND_PACKAGE(OpenCV REQUIRED) + include("3rdparty/CMakeModules/OpenCV.cmake") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}") MESSAGE(STATUS "With FBR ") ELSE(WITH_FBR) MESSAGE(STATUS "Without FBR") ENDIF(WITH_FBR) -## Special treatment for system specifics -IF(APPLE) -add_definitions(-Dfopen64=fopen) -ENDIF(APPLE) - -## Multiple Cores -IF(APPLE) - SET(PROCESSOR_COUNT 2) -ELSE(APPLE) - INCLUDE(CountProcessors) - SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" ) -ENDIF(APPLE) - # OPEN FIND_PACKAGE(OpenMP) IF(OPENMP_FOUND) - OPTION(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON) + OPT_DEP(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON "") ENDIF(OPENMP_FOUND) IF(OPENMP_FOUND AND WITH_OPENMP) @@ -272,7 +313,7 @@ ELSE(OPENMP_FOUND AND WITH_OPENMP) ENDIF(OPENMP_FOUND AND WITH_OPENMP) ## TORO -OPTION(WITH_TORO "Whether to use TORO. ON/OFF" OFF) +OPT_DEP(WITH_TORO "Whether to use TORO. ON/OFF" OFF "") IF(WITH_TORO) IF(WIN32) @@ -295,7 +336,7 @@ ENDIF(WITH_TORO) ## HOGMAN -OPTION(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF) +OPT_DEP(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF "") IF(WITH_HOGMAN) # dependant on libqt4-devi @@ -324,15 +365,19 @@ ELSE(WITH_HOGMAN) MESSAGE(STATUS "Without HOGMAN") ENDIF(WITH_HOGMAN) -OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF) +OPT_DEP(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF "WITH_SHOW;WITH_FBR") IF(EXPORT_SHARED_LIBS) + ## Compile a single shared library containing all of 3DTK + add_library(slam SHARED src/slam6d/icp6D.cc) + target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s) MESSAGE(STATUS "exporting additional libraries") ELSE(EXPORT_SHARED_LIBS) MESSAGE(STATUS "not exporting libraries") ENDIF(EXPORT_SHARED_LIBS) -OPTION(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF) + +OPT_DEP(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF "") IF(WITH_METRICS) MESSAGE(STATUS "With metrics in slam6d.") @@ -341,8 +386,25 @@ ELSE(WITH_METRICS) MESSAGE(STATUS "Without metrics in slam6d.") ENDIF(WITH_METRICS) +################################################# +# OPERATING SYSTEM SPECIFIC BEHAVIOUR ## +################################################# + +## Special treatment for system specifics +IF(APPLE) +add_definitions(-Dfopen64=fopen) +ENDIF(APPLE) + +## Multiple Cores +IF(APPLE) + SET(PROCESSOR_COUNT 2) +ELSE(APPLE) + INCLUDE(CountProcessors) + SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" ) +ENDIF(APPLE) + IF(WIN32) - SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" ) + SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING "Additional flags given to the compiler ( -O2)" ) include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/) link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows) link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64) @@ -364,6 +426,30 @@ IF(APPLE) include_directories(/System/Library/Frameworks/OpenGL.framework/Headers) ENDIF(APPLE) +# hack to "circumvent" Debug and Release folders that are created under visual studio +# this is why the INSTALL target has to be used in visual studio +IF(MSVC) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) + + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) +ENDIF(MSVC) + + + +################################################# +# GENERAL PROJECT SETTINGS ## +################################################# + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}") # Hide CMake variables @@ -398,35 +484,8 @@ add_subdirectory(src/grid) add_subdirectory(src/pmd) add_subdirectory(src/shapes) add_subdirectory(src/thermo) -IF(WITH_FBR) - add_subdirectory(src/slam6d/fbr) -ENDIF(WITH_FBR) +add_subdirectory(src/slam6d/fbr) add_subdirectory(src/scanner) add_subdirectory(src/model) -IF(EXPORT_SHARED_LIBS) -## Compiling a shared library containing all of the project -add_library(slam SHARED src/slam6d/icp6D.cc) -target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s) -ENDIF(EXPORT_SHARED_LIBS) - MESSAGE (STATUS "Build environment is set up!") - - -# hack to "circumvent" Debug and Release folders that are created under visual studio -# this is why the INSTALL target has to be used in visual studio -IF(MSVC) - INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") - IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) - INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") - ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) - INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") - ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) - - INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") - IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) - INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") - ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) - INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") - ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) -ENDIF(MSVC) diff --git a/CONTRIBUTORS b/CONTRIBUTORS index e1921b1..0494f6d 100644 --- a/CONTRIBUTORS +++ b/CONTRIBUTORS @@ -25,13 +25,14 @@ Mohammad Faisal Abdullah m.faisal@jacobs-university.de Li Ming liming751218@whu.edu.cn Li Wei xpaulee@gmail.com Shams Feyzabadi sh.feyzabadi@gmail.co -Vladislav Perelmann v.perelman@jacobs-university.de +Vladislav Perelmann v.perelman@jacobs-university.de Chen Long lchen.whu@gmail.com Remus Dumitru r.dumitru@jaocbs-university.de Billy Okal okal.billy@googlemail.com Razvan-George Mihalyi r.mihalyi@jacobs-university.de Johannes Schauer j.schauer@jacobs-university.de Corneliu-Claudiu Prodescu c.prodescu@jacobs-university.de +Vaibhav Kumar Mehta v.metha@jacobs-university.de Further contributors diff --git a/Makefile b/Makefile index 53c8ae7..40086f9 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,7 @@ config: .build touch .configured .configured: .build - cd .build && cmake .. + cd .build && cmake .. && cmake .. touch .configured .build: diff --git a/README b/README index 69f732c..530cd90 100644 --- a/README +++ b/README @@ -82,7 +82,7 @@ bin/slam6D -s 1 -e 76 -r 10 -m 3000 -d 50 -i 1000 --epsICP=0.000001 bin/show -s 1 -e 76 -m 3000 -f old /home/nuechter/dat/dat_mine1/ (using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) -bin/scan_red -s 0 -e 12 -r 10 /home/nuechter/dat/bremen_city +bin/scan_red -s 0 -e 12 -r OCTREE -v 10 --octree 0 -f RIEGL_TXT /home/nuechter/dat/bremen_city bin/slam6D -a 2 -q /home/nuechter/dat/bremen_city/reduced -f uos -d 150 -s 0 -e 12 --anim=1 -n /home/nuechter/dat/bremen_city/bremen.net -G 1 -D 100 -i 0 -I 50 -p --epsSLAM=0.0 diff --git a/bin/Debug/freeglut.dll b/bin/Debug/freeglut.dll deleted file mode 100644 index e70e8af..0000000 Binary files a/bin/Debug/freeglut.dll and /dev/null differ diff --git a/bin/Debug/testresult.bat b/bin/Debug/testresult.bat deleted file mode 100644 index 367896c..0000000 --- a/bin/Debug/testresult.bat +++ /dev/null @@ -1 +0,0 @@ -scanserver -b 0 \ No newline at end of file diff --git a/bin/Release/freeglut.dll b/bin/Release/freeglut.dll deleted file mode 100644 index e70e8af..0000000 Binary files a/bin/Release/freeglut.dll and /dev/null differ diff --git a/bin/Release/testresult.bat b/bin/Release/testresult.bat deleted file mode 100644 index 367896c..0000000 --- a/bin/Release/testresult.bat +++ /dev/null @@ -1 +0,0 @@ -scanserver -b 0 \ No newline at end of file diff --git a/include/pmd/cvpmd.h b/include/pmd/cvpmd.h index ef51e06..6d68899 100644 --- a/include/pmd/cvpmd.h +++ b/include/pmd/cvpmd.h @@ -6,7 +6,13 @@ #pragma once #include "pmdsdk2.h" +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif /** * @brief Structure for PMD IO, see initPMD. diff --git a/include/pmd/pmdWrap.h b/include/pmd/pmdWrap.h index f4cd589..629585c 100644 --- a/include/pmd/pmdWrap.h +++ b/include/pmd/pmdWrap.h @@ -1,7 +1,13 @@ #pragma once +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif #include diff --git a/include/pmd/pose/history.h b/include/pmd/pose/history.h index c54e33d..6f20f70 100644 --- a/include/pmd/pose/history.h +++ b/include/pmd/pose/history.h @@ -1,6 +1,13 @@ #pragma once +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + struct Frame { CvMat *trn; diff --git a/include/slam6d/Boctree.h b/include/slam6d/Boctree.h index 0d1f3c0..ae0c948 100644 --- a/include/slam6d/Boctree.h +++ b/include/slam6d/Boctree.h @@ -226,9 +226,6 @@ public: this->POINTDIM = pointtype.getPointDim(); - //@@@ - cout << "POINTDIM" << this->POINTDIM << endl; - mins = alloc->allocate(POINTDIM); maxs = alloc->allocate(POINTDIM); @@ -822,11 +819,24 @@ protected: if ( ( 1 << i ) & node.valid ) { // if ith node exists childcenter(center, ccenter, size, i); // childrens center if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center - T * cp = new T[3]; - for (unsigned int iterator = 0; iterator < 3; iterator++) { + T * cp = new T[POINTDIM]; + // compute average of reflectance values and store in last position of cp + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + T *point = &(points[1].v); + float reflectance_center = 0.; + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + reflectance_center += point[POINTDIM-1]; // add current reflectance + point+=BOctTree::POINTDIM; + } + reflectance_center /= length * 1.0; + + for (unsigned int iterator = 0; iterator < POINTDIM-1; iterator++) { cp[iterator] = ccenter[iterator]; } + cp[POINTDIM-1] = reflectance_center; // reflectance is the last dimension in POINTDIM c.push_back(cp); + } else { // recurse GetOctTreeCenter(c, children->node, ccenter, size/2.0); } diff --git a/include/slam6d/fbr/fbr_global.h b/include/slam6d/fbr/fbr_global.h index 749e9d2..8f9ad24 100644 --- a/include/slam6d/fbr/fbr_global.h +++ b/include/slam6d/fbr/fbr_global.h @@ -11,7 +11,11 @@ #include #include #include +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else #include +#endif //for opencv 2.4 #if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4) #include diff --git a/include/slam6d/fbr/panorama.h b/include/slam6d/fbr/panorama.h index f58f936..90b950d 100644 --- a/include/slam6d/fbr/panorama.h +++ b/include/slam6d/fbr/panorama.h @@ -64,7 +64,7 @@ namespace fbr{ * @param image - input range image to be converted to point cloud * @param file - destination of .3d file containing the point cloud */ - void recoverPointCloud(const cv::Mat& range_image, const std::string& file); + void recoverPointCloud(const cv::Mat& range_image, cv::Mat& reflectance_image, vector &reduced_points); unsigned int getImageWidth(); unsigned int getImageHeight(); diff --git a/include/slam6d/globals.icc b/include/slam6d/globals.icc index 878323e..803cda3 100644 --- a/include/slam6d/globals.icc +++ b/include/slam6d/globals.icc @@ -1189,7 +1189,7 @@ inline int LU_solve(const double A[4][4], const int indx[4], double b[4]) } /** - * Calculates the cross product of two 4-vectors + * Calculates the cross product of two 4-vectors * * @param x input 1 * @param y input 2 @@ -1205,6 +1205,19 @@ static inline void Cross(const T *x, const T *y, T *result) return; } +/** + * Computes the dot product of two 3-vector + * + * @param x input 3-vector + * @param y input 3-vector + * @return dot product of x and y + */ +template +inline T Dot(const T *x, const T *y) +{ + return x[0] * y[0] + x[1] * y[1] + x[2] * y[2]; +} + /** * converts a quaternion to Euler angels in the roll pitch yaw system */ diff --git a/include/slam6d/scan.h b/include/slam6d/scan.h index 761bae7..bf7c9b4 100644 --- a/include/slam6d/scan.h +++ b/include/slam6d/scan.h @@ -390,10 +390,13 @@ protected: //! Internal function of transform which handles the matrices void transformMatrix(const double alignxf[16]); - + + //@FIXME + public: //! Creating reduced points void calcReducedPoints(); - + + protected: //! Copies reduced points to original points without any transformation. void copyReducedToOriginal(); diff --git a/include/thermo/thermo.h b/include/thermo/thermo.h index 1d7743e..47e2b81 100644 --- a/include/thermo/thermo.h +++ b/include/thermo/thermo.h @@ -1,8 +1,13 @@ #ifndef __THERMO_H__ #define __THERMO_H__ +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) #include #include +#else +#include +#endif +//#include #include #include using namespace std; @@ -18,6 +23,8 @@ IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int bo void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1); void writeCalibParam(int images, int corner_exp, int board_w, CvMat* image_points, CvSize size, string dir); +void loadIntrinsicCalibration(CvMat * intrinsic, CvMat * distortion, string dir, bool optical=false) ; +void loadExtrinsicCalibration(CvMat * Translation, CvMat * Rotation, string dir, int method, bool optical=false) ; void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, IOType type, int scale, double rot_angle, double minDist, double maxDist, bool correction, int neighborhood, int method=0); diff --git a/src/normals/normals.cc b/src/normals/normals.cc index da9f414..846583c 100644 --- a/src/normals/normals.cc +++ b/src/normals/normals.cc @@ -109,7 +109,7 @@ void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver if (vmap.count("help")) { cout << cmd_options << endl << endl; cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; - cout << " bin/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &normals,vector &points, int k, c ANNpointArray pa = annAllocPts(points.size(), 3); for (size_t i=0; i &normals,vector &points, int k, c n = n / n.NormFrobenius(); normals.push_back(Point(n(1), n(2), n(3))); } + delete[] nidx; delete[] d; + annDeallocPts(pa); } //////////////////////////////////////////////////////////////// /////////////NORMALS USING ADAPTIVE AKNN METHOD //////////////// @@ -213,7 +214,6 @@ void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, ANNpointArray pa = annAllocPts(points.size(), 3); for (size_t i=0; i &normals,vector &points, n = n / n.NormFrobenius(); normals.push_back(Point(n(1), n(2), n(3))); } + annDeallocPts(pa); } /////////////////////////////////////////////////////// diff --git a/src/pmd/calibrate/calibrate.cc b/src/pmd/calibrate/calibrate.cc index 2c52758..a6ec492 100644 --- a/src/pmd/calibrate/calibrate.cc +++ b/src/pmd/calibrate/calibrate.cc @@ -9,8 +9,14 @@ #include +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif void usage(char *progName) { diff --git a/src/pmd/calibrate/extrinsic.cc b/src/pmd/calibrate/extrinsic.cc index 0362b0c..413878e 100644 --- a/src/pmd/calibrate/extrinsic.cc +++ b/src/pmd/calibrate/extrinsic.cc @@ -7,10 +7,16 @@ * */ +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif int main(int argc, char **argv) { diff --git a/src/pmd/calibrate/grabFramesCam.cc b/src/pmd/calibrate/grabFramesCam.cc index 80870e9..4fff493 100644 --- a/src/pmd/calibrate/grabFramesCam.cc +++ b/src/pmd/calibrate/grabFramesCam.cc @@ -10,8 +10,14 @@ #include #include +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif //TODO: flip image flag void usage(char *progName) { diff --git a/src/pmd/calibrate/grabFramesPMD.cc b/src/pmd/calibrate/grabFramesPMD.cc index a074f2c..0254367 100644 --- a/src/pmd/calibrate/grabFramesPMD.cc +++ b/src/pmd/calibrate/grabFramesPMD.cc @@ -7,8 +7,14 @@ * */ +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif #include #include diff --git a/src/pmd/cvpmd.cc b/src/pmd/cvpmd.cc index 67b4cf8..c4eea67 100644 --- a/src/pmd/cvpmd.cc +++ b/src/pmd/cvpmd.cc @@ -7,7 +7,13 @@ * */ -#include /* IplImage, cvCreateImage */ +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) +#include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif #include "pmdsdk2.h" diff --git a/src/pmd/offline/grabVideoAnd3D.cc b/src/pmd/offline/grabVideoAnd3D.cc index e61b742..b76fd0f 100644 --- a/src/pmd/offline/grabVideoAnd3D.cc +++ b/src/pmd/offline/grabVideoAnd3D.cc @@ -10,8 +10,14 @@ #include #include +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif #include "pmdsdk2.h" #include "cvpmd.h" diff --git a/src/pmd/pmdWrap.cc b/src/pmd/pmdWrap.cc index c559f08..e51c1b9 100644 --- a/src/pmd/pmdWrap.cc +++ b/src/pmd/pmdWrap.cc @@ -10,8 +10,14 @@ #include #include +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif #include diff --git a/src/pmd/pose/history.cc b/src/pmd/pose/history.cc index 7db2319..34af0a2 100644 --- a/src/pmd/pose/history.cc +++ b/src/pmd/pose/history.cc @@ -8,7 +8,13 @@ */ #include "history.h" +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif Frame *allocFrame3DData(CvSize pmdSz) { Frame *f = (Frame*)cvAlloc(sizeof(Frame)); diff --git a/src/pmd/pose/pose.cc b/src/pmd/pose/pose.cc index 84add0c..b82ea53 100644 --- a/src/pmd/pose/pose.cc +++ b/src/pmd/pose/pose.cc @@ -14,8 +14,14 @@ #include // OpenCV +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif // GL: GLFW (window etc, ala glut) and FTGL (text rendering) #include diff --git a/src/scanio/CMakeLists.txt b/src/scanio/CMakeLists.txt index 6e15f8c..d70c639 100644 --- a/src/scanio/CMakeLists.txt +++ b/src/scanio/CMakeLists.txt @@ -17,12 +17,6 @@ if(WITH_RIVLIB) endif(LIBXML2_FOUND) endif(WITH_RIVLIB) -#IF (WITH_CAD) -# IF(NOT WIN32) -# add_library(scan_io_cad SHARED scan_io_cad.cc) -# target_link_libraries(scan_io_cad ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) -# ENDIF(NOT WIN32) -#ENDIF (WITH_CAD) foreach(libname ${SCANIO_LIBNAMES}) if(WIN32) diff --git a/src/scanner/david_scanner.cc b/src/scanner/david_scanner.cc index e53c11f..17c9b08 100644 --- a/src/scanner/david_scanner.cc +++ b/src/scanner/david_scanner.cc @@ -36,10 +36,18 @@ #include #include #include + +#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__))) #include #include #include #include +#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#else +#include +#endif + #include #include #define PI 3.14159265 @@ -196,7 +204,7 @@ int main(int argc, char** argv){ //open file for writing ofstream scanfile; - char scanname[10]; + char scanname[20]; sprintf(scanname,"scan000.3d"); scanfile.open(scanname); diff --git a/src/show/CMakeLists.txt b/src/show/CMakeLists.txt index c3c2ded..4e43fdf 100644 --- a/src/show/CMakeLists.txt +++ b/src/show/CMakeLists.txt @@ -29,5 +29,5 @@ ENDIF(WITH_WXSHOW) ### EXPORT SHARED LIBS IF(EXPORT_SHARED_LIBS) add_library(show_s SHARED ${SHOW_SRCS}) -target_link_libraries(show_s ${SHOW_LIBS}) +target_link_libraries(show_s newmat_s) ENDIF(EXPORT_SHARED_LIBS) diff --git a/src/show/show_common.cc b/src/show/show_common.cc index f160e05..26f2e55 100644 --- a/src/show/show_common.cc +++ b/src/show/show_common.cc @@ -29,9 +29,6 @@ using std::ifstream; using std::exception; #include -#ifdef WITH_METRICS -#include "slam6d/metrics.h" -#endif #ifdef _MSC_VER #include "XGetopt.h" @@ -779,100 +776,6 @@ void generateFrames(int start, int end, bool identity) { } } - - -/* - * create display lists - * @to do general framework for color & type definitions - */ -void createDisplayLists(bool reduced) -{ - for(unsigned int s = 0; s < Scan::allScans.size() ; s++) { - Scan* scan = Scan::allScans[s]; - - vertexArray* myvertexArray1; - vertexArray* myvertexArray2; - - // count points - unsigned int color1 = 0, color2 = 0; - if(!reduced) { - scan->get(DATA_XYZ | DATA_TYPE); - DataType type(scan->get("type")); - - if(type.valid()) { - for(unsigned int i = 0; i < type.size(); ++i) { - if(type[i] & TYPE_GROUND) { - color1 += 3; - } else { - color2 += 3; - } - } - } else { - color2 = 3 * scan->size("xyz"); - } - - myvertexArray1 = new vertexArray(color1); - myvertexArray2 = new vertexArray(color2); - - color1 = 0; color2 = 0; - - DataXYZ xyz(scan->get("xyz")); - for(unsigned int i = 0; i < xyz.size(); ++i) { - if(type[i] & TYPE_GROUND) { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray1->array[color1++] = xyz[i][j]; - } - } else { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray2->array[color2++] = xyz[i][j]; - } - } - } - } else { - color2 = 3 * scan->size("xyz reduced"); - - myvertexArray1 = new vertexArray(0); - myvertexArray2 = new vertexArray(color2); - - color2 = 0; - - DataXYZ xyz_r(scan->get("xyz reduced")); - for(unsigned int i = 0; i < xyz_r.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray2->array[color2++] = xyz_r[i][j]; - } - } - } - - - - glNewList(myvertexArray1->name, GL_COMPILE); - //@ - //glColor4d(0.44, 0.44, 0.44, 1.0); - //glColor4d(0.66, 0.66, 0.66, 1.0); - glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array); - glEnableClientState(GL_VERTEX_ARRAY); - glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender); - glDisableClientState(GL_VERTEX_ARRAY); - glEndList(); - - glNewList(myvertexArray2->name, GL_COMPILE); - //glColor4d(1.0, 1.0, 1.0, 1.0); - //glColor4d(0.0, 0.0, 0.0, 1.0); - glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array); - glEnableClientState(GL_VERTEX_ARRAY); - glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender); - glDisableClientState(GL_VERTEX_ARRAY); - glEndList(); - - // append to vector - vector vvertexArray; - vvertexArray.push_back(myvertexArray1); - vvertexArray.push_back(myvertexArray2); - vvertexArrayList.push_back(vvertexArray); - } -} - void cycleLOD() { LevelOfDetail = 0.00001; for (unsigned int i = 0; i < octpts.size(); i++) @@ -980,7 +883,6 @@ void initShow(int argc, char **argv){ } cm = new ScanColorManager(4096, pointtype); -#ifdef USE_GL_POINTS // use octtrees #ifdef USE_COMPACT_TREE cout << "Creating compact display octrees.." << endl; #else @@ -1117,9 +1019,6 @@ set heuristic, do locking, catch exception, reset heuristic to default or old #endif // !COMPACT_TREE -#else // not using octtrees - createDisplayLists(red > 0); -#endif // USE_GL_POINTS // load frames now that we know how many scans we actually loaded unsigned int real_end = min((unsigned int)(end), diff --git a/src/show/show_gl.cc b/src/show/show_gl.cc index 13e043f..bafb673 100644 --- a/src/show/show_gl.cc +++ b/src/show/show_gl.cc @@ -42,11 +42,7 @@ void DrawPoints(GLenum mode, bool interruptable) if(frameNr != 0) { cm->setMode(ScanColorManager::MODE_ANIMATION); -#ifdef USE_GL_POINTS for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { -#else - for(int iterator = (int)Scan::allScans.size()-1; iterator >= 0; iterator--) { -#endif // ignore scans that don't have any frames associated with them if((unsigned int)iterator >= MetaMatrix.size()) continue; // set usable frame @@ -67,7 +63,6 @@ void DrawPoints(GLenum mode, bool interruptable) glPointSize(pointsize); -#ifdef USE_GL_POINTS ExtractFrustum(pointsize); cm->selectColors(type); if (pointmode == 1 ) { @@ -75,18 +70,6 @@ void DrawPoints(GLenum mode, bool interruptable) } else { octpts[iterator]->displayLOD(LevelOfDetail); } -#else - for (unsigned int jterator = 0; jterator < vvertexArrayList[iterator].size(); jterator++) { - - if ((jterator == 0) && vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { - cm->selectColors(type); - } - - if (vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { - glCallList(vvertexArrayList[iterator][jterator]->name); - } - } -#endif glPopMatrix(); } @@ -128,13 +111,8 @@ void DrawPoints(GLenum mode, bool interruptable) vector sequence; calcPointSequence(sequence, current_frame); -#ifdef USE_GL_POINTS - //for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { for(unsigned int i = 0; i < sequence.size(); i++) { int iterator = sequence[i]; -#else - for(int iterator = (int)Scan::allScans.size()-1; iterator >= 0; iterator--) { -#endif // ignore scans that don't have any frames associated with them if((unsigned int)iterator >= MetaMatrix.size()) continue; // set usable frame @@ -162,14 +140,6 @@ void DrawPoints(GLenum mode, bool interruptable) } glMultMatrixd(frame); -#ifdef USE_GL_POINTS - //cout << endl << endl; calcRay(570, 266, 1.0, 40000.0); - /* // for height mapped color in the vertex shader - GLfloat v[16]; - for (unsigned int l = 0; l < 16; l++) - v[l] = MetaMatrix[iterator].back()[l]; - glUniformMatrix4fvARB(glGetUniformLocationARB(p, "MYMAT"), 1, 0, v); - */ ExtractFrustum(pointsize); if (pointmode == 1 ) { octpts[iterator]->display(); @@ -197,13 +167,6 @@ void DrawPoints(GLenum mode, bool interruptable) glPointSize(pointsize); } -#else - for (unsigned int jterator = 0; jterator < vvertexArrayList[iterator].size(); jterator++) { - if (vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { - glCallList(vvertexArrayList[iterator][jterator]->name); - } - } -#endif glPopMatrix(); } } diff --git a/src/slam6d/CMakeLists.txt b/src/slam6d/CMakeLists.txt index 98fee53..a9ede6d 100644 --- a/src/slam6d/CMakeLists.txt +++ b/src/slam6d/CMakeLists.txt @@ -6,7 +6,7 @@ IF(WITH_TOOLS) add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc) IF(UNIX) - target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS}) + target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS} ${Boost_LIBRARIES}) ENDIF(UNIX) IF (WIN32) @@ -90,7 +90,7 @@ ENDIF(UNIX) IF(EXPORT_SHARED_LIBS) add_library(scan_s SHARED ${SCANLIB_SRCS}) #target_link_libraries(scan_s ${Boost_LIBRARIES} newmat) -target_link_libraries(scan_s newmat sparse ANN scanclient pointfilter scanio) +target_link_libraries(scan_s newmat_s sparse_s ANN_s ) ENDIF(EXPORT_SHARED_LIBS) ### SLAM6D diff --git a/src/slam6d/basicScan.cc b/src/slam6d/basicScan.cc index 0956967..4d56468 100644 --- a/src/slam6d/basicScan.cc +++ b/src/slam6d/basicScan.cc @@ -126,7 +126,10 @@ BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOT BasicScan::~BasicScan() { - // TODO: clean m_data up + for (map>::iterator it = m_data.begin(); it != m_data.end(); it++) { + delete it->second.first; + } + } void BasicScan::init() diff --git a/src/slam6d/fbr/CMakeLists.txt b/src/slam6d/fbr/CMakeLists.txt index 18330dc..000bf21 100644 --- a/src/slam6d/fbr/CMakeLists.txt +++ b/src/slam6d/fbr/CMakeLists.txt @@ -1,3 +1,4 @@ +IF(WITH_FBR) FIND_PACKAGE(OpenCV REQUIRED) SET(FBR_IO_SRC scan_cv.cc) @@ -20,17 +21,17 @@ add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC}) SET(FBR_SRC scan_cv.cc panorama.cc feature.cc feature_matcher.cc registration.cc fbr_global.cc) add_library(fbr STATIC ${FBR_SRC}) -IF(WITH_FBR) SET(FBR_LIBS scan ANN ${OpenCV_LIBS}) add_executable(featurebasedregistration feature_based_registration.cc fbr_global.cc) #target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS}) target_link_libraries(featurebasedregistration fbr ${FBR_LIBS}) -ENDIF(WITH_FBR) ### EXPORT SHARED LIBS IF(EXPORT_SHARED_LIBS) add_library(fbr_s SHARED ${FBR_SRC}) -target_link_libraries(fbr_s scan ANN ${OpenCV_LIBS}) -ENDIF(EXPORT_SHARED_LIBS) \ No newline at end of file +target_link_libraries(fbr_s scan_s ANN_s ${OpenCV_LIBS}) +ENDIF(EXPORT_SHARED_LIBS) + +ENDIF(WITH_FBR) diff --git a/src/slam6d/fbr/panorama.cc b/src/slam6d/fbr/panorama.cc index e488f0b..8561a85 100644 --- a/src/slam6d/fbr/panorama.cc +++ b/src/slam6d/fbr/panorama.cc @@ -554,8 +554,15 @@ namespace fbr{ } } - void panorama::recoverPointCloud(const cv::Mat& range_image, const string& file ) { - std::ofstream scan_file (file.c_str()); + void panorama::recoverPointCloud(const cv::Mat& range_image, + cv::Mat& reflectance_image, vector &reduced_points) { + if (range_image.cols != reflectance_image.cols + || range_image.rows != reflectance_image.rows) { + cerr << "range image and reflectance image have different geometries - using empty range image" << endl; + reflectance_image.create(range_image.size(), CV_8U); + reflectance_image = cv::Scalar::all(0); + } + //recover from EQUIRECTANGULAR projection if(pMethod == EQUIRECTANGULAR) { double xFactor = (double) range_image.size().width / 2 / M_PI; @@ -568,6 +575,7 @@ namespace fbr{ for (int row = 0; row < range_image.size().height; ++row) { for (int col = 0; col < range_image.size().width; ++col) { float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; float theta = (heightMax - row) / yFactor - heightLow; float phi = col / xFactor; phi *= 180.0 / M_PI; @@ -583,7 +591,10 @@ namespace fbr{ if (first_seen) first_seen = false; else continue; } - scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); } } } @@ -600,6 +611,7 @@ namespace fbr{ for (int row = 0; row < range_image.size().height; ++row) { for (int col = 0; col < range_image.size().width; ++col) { float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; float theta = atan2(row + yFactor * tan(heightLow), yFactor); float phi = col / xFactor; phi *= 180.0 / M_PI; @@ -615,7 +627,10 @@ namespace fbr{ if (first_seen) first_seen = false; else continue; } - scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); } } } @@ -631,6 +646,7 @@ namespace fbr{ for (int row = 0; row < range_image.size().height; ++row) { for (int col = 0; col < range_image.size().width; ++col) { float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; float theta = 2 * atan2(exp((heightMax - row) / yFactor + heightLow), 1.) - M_PI_2; float phi = col / xFactor; phi *= 180.0 / M_PI; @@ -646,7 +662,10 @@ namespace fbr{ if (first_seen) first_seen = false; else continue; } - scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); } } } @@ -675,6 +694,7 @@ namespace fbr{ for (int row = 0; row < range_image.size().height; ++row) { for (int col = 0; col < range_image.size().width; ++col) { float range = range_image.at(row, col); + float reflectance = reflectance_image.at(row,col)/255.0; float x = col * 1. / xFactor - fabs(xmin); float y = (heightMax - row) * 1. / yFactor - fabs(ymin); float theta = asin((C - (x*x + (Rho0 - y) * (Rho0 - y)) * n * n) / (2 * n)); @@ -695,12 +715,13 @@ namespace fbr{ if (first_seen) first_seen = false; else continue; } - scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + reduced_points.push_back(cv::Vec4f(-100.0*cartesian[1], + 100.0*cartesian[2], + 100.0*cartesian[0], + reflectance)); } } } - - scan_file.close(); } unsigned int panorama::getImageWidth(){ diff --git a/src/slam6d/scan.cc b/src/slam6d/scan.cc index 4badc29..94bc096 100644 --- a/src/slam6d/scan.cc +++ b/src/slam6d/scan.cc @@ -188,12 +188,12 @@ void Scan::copyReducedToOriginal() Timer t = ClientMetric::copy_original_time.start(); #endif //WITH_METRICS - DataXYZ xyz_r(get("xyz reduced")); - unsigned int size = xyz_r.size(); - DataXYZ xyz_r_orig(create("xyz reduced original", sizeof(double)*3*size)); + DataXYZ xyz_reduced(get("xyz reduced")); + unsigned int size = xyz_reduced.size(); + DataXYZ xyz_reduced_orig(create("xyz reduced original", sizeof(double)*3*size)); for(unsigned int i = 0; i < size; ++i) { for(unsigned int j = 0; j < 3; ++j) { - xyz_r_orig[i][j] = xyz_r[i][j]; + xyz_reduced_orig[i][j] = xyz_reduced[i][j]; } } @@ -208,12 +208,12 @@ void Scan::copyOriginalToReduced() Timer t = ClientMetric::copy_original_time.start(); #endif //WITH_METRICS - DataXYZ xyz_r_orig(get("xyz reduced original")); - unsigned int size = xyz_r_orig.size(); - DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size)); + DataXYZ xyz_reduced_orig(get("xyz reduced original")); + unsigned int size = xyz_reduced_orig.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); for(unsigned int i = 0; i < size; ++i) { for(unsigned int j = 0; j < 3; ++j) { - xyz_r[i][j] = xyz_r_orig[i][j]; + xyz_reduced[i][j] = xyz_reduced_orig[i][j]; } } @@ -234,13 +234,67 @@ void Scan::calcReducedPoints() Timer t = ClientMetric::scan_load_time.start(); #endif //WITH_METRICS - // get xyz to start the scan load, separated here for time measurement - DataXYZ xyz(get("xyz")); - // if the scan hasn't been loaded we can't calculate anything + // get xyz to start the scan load, separated here for time measurement + + DataXYZ xyz(get("xyz")); + DataReflectance reflectance(get("reflectance")); if(xyz.size() == 0) throw runtime_error("Could not calculate reduced points, XYZ data is empty"); + if (reflectance.size()==0) { + + // no reduction needed + // copy vector of points to array of points to avoid + // further copying + if(reduction_voxelSize <= 0.0) { + // copy the points + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz[i][j]; + } + } + } else { + // start reduction + // build octree-tree from CurrentScan + // put full data into the octtree + BOctTree *oct = new BOctTree(PointerArray(xyz).get(), + xyz.size(), reduction_voxelSize, reduction_pointtype); + + vector center; + center.clear(); + if (reduction_nrpts > 0) { + if (reduction_nrpts == 1) { + oct->GetOctTreeRandom(center); + } else { + oct->GetOctTreeRandom(center, reduction_nrpts); + } + } else { + oct->GetOctTreeCenter(center); + } + + // storing it as reduced scan + unsigned int size = center.size(); + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = center[i][j]; + } + } + delete oct; + } + } else { + if(xyz.size() != reflectance.size()) + throw runtime_error("Could not calculate reduced reflectance, reflectance size is different from points size"); + double **xyz_reflectance = new double*[xyz.size()]; + for (unsigned int i = 0; i < xyz.size(); ++i) { + xyz_reflectance[i] = new double[4]; + for (unsigned int j = 0; j < 3; ++j) + xyz_reflectance[i][j] = xyz[i][j]; + xyz_reflectance[i][3] = reflectance[i]; + } + #ifdef WITH_METRICS ClientMetric::scan_load_time.end(t); Timer tl = ClientMetric::calc_reduced_points_time.start(); @@ -251,50 +305,71 @@ void Scan::calcReducedPoints() // further copying if(reduction_voxelSize <= 0.0) { // copy the points - DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*xyz.size())); - for(unsigned int i = 0; i < xyz.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) { - xyz_r[i][j] = xyz[i][j]; + if (reduction_pointtype.hasReflectance()) { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(double)*reflectance.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) + xyz_reduced[i][j] = xyz[i][j]; + reflectance_reduced[i] = reflectance[i]; + } + } else { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = xyz[i][j]; + } } } } else { // start reduction - // build octree-tree from CurrentScan // put full data into the octtree - BOctTree *oct = new BOctTree(PointerArray(xyz).get(), - xyz.size(), reduction_voxelSize, reduction_pointtype); - - vector center; - center.clear(); + BOctTree *oct = new BOctTree(xyz_reflectance, xyz.size(), reduction_voxelSize, reduction_pointtype); + vector reduced; + reduced.clear(); if (reduction_nrpts > 0) { if (reduction_nrpts == 1) { - oct->GetOctTreeRandom(center); + oct->GetOctTreeRandom(reduced); } else { - oct->GetOctTreeRandom(center, reduction_nrpts); + oct->GetOctTreeRandom(reduced, reduction_nrpts); } } else { - oct->GetOctTreeCenter(center); + oct->GetOctTreeCenter(reduced); } // storing it as reduced scan - unsigned int size = center.size(); - DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size)); - for(unsigned int i = 0; i < size; ++i) { - for(unsigned int j = 0; j < 3; ++j) { - xyz_r[i][j] = center[i][j]; + unsigned int size = reduced.size(); + if (reduction_pointtype.hasReflectance()) { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_reduced[i][j] = reduced[i][j]; + } } + DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*size)); + for(unsigned int i = 0; i < size; ++i) + reflectance_reduced[i] = reduced[i][3]; + } else { + DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) + for(unsigned int j = 0; j < 3; ++j) + xyz_reduced[i][j] = reduced[i][j]; } - delete oct; } - + for (unsigned int i = 0; i < xyz.size(); ++i) { + delete[] xyz_reflectance[i]; + } + delete[] xyz_reflectance; #ifdef WITH_METRICS ClientMetric::calc_reduced_points_time.end(tl); #endif //WITH_METRICS + } } + /** * Merges the scan's intrinsic coordinates with the robot position. * @param prevScan The scan that's transformation is extrapolated, @@ -338,11 +413,11 @@ void Scan::transformReduced(const double alignxf[16]) Timer t = ClientMetric::transform_time.start(); #endif //WITH_METRICS - DataXYZ xyz_r(get("xyz reduced")); + DataXYZ xyz_reduced(get("xyz reduced")); unsigned int i=0; // #pragma omp parallel for - for( ; i < xyz_r.size(); ++i) { - transform3(alignxf, xyz_r[i]); + for( ; i < xyz_reduced.size(); ++i) { + transform3(alignxf, xyz_reduced[i]); } #ifdef WITH_METRICS @@ -584,21 +659,21 @@ void Scan::getNoPairsSimple(vector &diff, int thread_num, double max_dist_match2) { - DataXYZ xyz_r(Source->get("xyz reduced")); + DataXYZ xyz_reduced(Source->get("xyz reduced")); KDtree* kd = new KDtree(PointerArray(Target->get("xyz reduced")).get(), Target->size("xyz reduced")); cout << "Max: " << max_dist_match2 << endl; - for (unsigned int i = 0; i < xyz_r.size(); i++) { + for (unsigned int i = 0; i < xyz_reduced.size(); i++) { double p[3]; - p[0] = xyz_r[i][0]; - p[1] = xyz_r[i][1]; - p[2] = xyz_r[i][2]; + p[0] = xyz_reduced[i][0]; + p[1] = xyz_reduced[i][1]; + p[2] = xyz_reduced[i][2]; double *closest = kd->FindClosest(p, max_dist_match2, thread_num); if (!closest) { - diff.push_back(xyz_r[i]); + diff.push_back(xyz_reduced[i]); //diff.push_back(closest); } } @@ -625,15 +700,15 @@ void Scan::getPtPairsSimple(vector *pairs, double *centroid_m, double *centroid_d) { KDtree* kd = new KDtree(PointerArray(Source->get("xyz reduced")).get(), Source->size("xyz reduced")); - DataXYZ xyz_r(Target->get("xyz reduced")); + DataXYZ xyz_reduced(Target->get("xyz reduced")); - for (unsigned int i = 0; i < xyz_r.size(); i++) { + for (unsigned int i = 0; i < xyz_reduced.size(); i++) { if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only double p[3]; - p[0] = xyz_r[i][0]; - p[1] = xyz_r[i][1]; - p[2] = xyz_r[i][2]; + p[0] = xyz_reduced[i][0]; + p[1] = xyz_reduced[i][1]; + p[2] = xyz_reduced[i][2]; double *closest = kd->FindClosest(p, max_dist_match2, thread_num); if (closest) { @@ -688,9 +763,9 @@ void Scan::getPtPairs(vector *pairs, } // get point pairs - DataXYZ xyz_r(Target->get("xyz reduced")); + DataXYZ xyz_reduced(Target->get("xyz reduced")); Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf, - xyz_r, 0, xyz_r.size(), + xyz_reduced, 0, xyz_reduced.size(), thread_num, rnd, max_dist_match2, sum, centroid_m, centroid_d); @@ -746,20 +821,20 @@ void Scan::getPtPairsParallel(vector *pairs, Scan* Source, Scan* Target if(meta) { for(unsigned int i = 0; i < meta->size(); ++i) { // determine step for each scan individually - DataXYZ xyz_r(meta->getScan(i)->get("xyz reduced")); - unsigned int max = xyz_r.size(); + DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced")); + unsigned int max = xyz_reduced.size(); unsigned int step = max / OPENMP_NUM_THREADS; // call ptpairs for each scan and accumulate ptpairs, centroids and sum search->getPtPairs(&pairs[thread_num], Source->dalignxf, - xyz_r, step * thread_num, step * thread_num + step, + xyz_reduced, step * thread_num, step * thread_num + step, thread_num, rnd, max_dist_match2, sum[thread_num], centroid_m[thread_num], centroid_d[thread_num]); } } else { - DataXYZ xyz_r(Target->get("xyz reduced")); + DataXYZ xyz_reduced(Target->get("xyz reduced")); search->getPtPairs(&pairs[thread_num], Source->dalignxf, - xyz_r, thread_num * step, thread_num * step + step, + xyz_reduced, thread_num * step, thread_num * step + step, thread_num, rnd, max_dist_match2, sum[thread_num], centroid_m[thread_num], centroid_d[thread_num]); diff --git a/src/slam6d/scan_red.cc b/src/slam6d/scan_red.cc index 5c05f84..ca13036 100644 --- a/src/slam6d/scan_red.cc +++ b/src/slam6d/scan_red.cc @@ -40,6 +40,7 @@ using std::ofstream; #include "slam6d/metaScan.h" #include "slam6d/io_utils.h" #include "slam6d/scan.h" +#include "slam6d/Boctree.h" #include "slam6d/fbr/fbr_global.h" #include "slam6d/fbr/panorama.h" #include "slam6d/fbr/scan_cv.h" @@ -71,205 +72,389 @@ using std::ofstream; #include #endif -//Vertical angle of view of scanner -#define MAX_ANGLE 60.0 -#define MIN_ANGLE -40.0 - -#define IMAGE_HEIGHT 1000 -#define IMAGE_WIDTH 3600 - using namespace fbr; -projection_method strToPMethod(string method){ - if(strcasecmp(method.c_str(), "EQUIRECTANGULAR") == 0) return EQUIRECTANGULAR; - else if(strcasecmp(method.c_str(), "CYLINDRICAL") == 0) return CYLINDRICAL; - else if(strcasecmp(method.c_str(), "MERCATOR") == 0) return MERCATOR; - else if(strcasecmp(method.c_str(), "CONIC") == 0) return CONIC; - else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown")); +#include +namespace po = boost::program_options; + +enum reduction_method {OCTREE, RANGE, INTERPOLATE}; + +/* Function used to check that 'opt1' and 'opt2' are not specified + at the same time. */ +void conflicting_options(const po::variables_map & vm, + const char *opt1, const char *opt2) +{ + if (vm.count(opt1) && !vm[opt1].defaulted() + && vm.count(opt2) && !vm[opt2].defaulted()) + throw std::logic_error(string("Conflicting options '") + + opt1 + "' and '" + opt2 + "'."); } -/** - * Explains the usage of this program's command line parameters - */ -void usage(char* prog) +/* Function used to check that if 'for_what' is specified, then + 'required_option' is specified too. */ +void option_dependency(const po::variables_map & vm, + const char *for_what, const char *required_option) { + if (vm.count(for_what) && !vm[for_what].defaulted()) + if (vm.count(required_option) == 0 + || vm[required_option].defaulted()) + throw std::logic_error(string("Option '") + for_what + + "' requires option '" + + required_option + "'."); +} + +/* + * validates panorama method specification + */ +namespace fbr { + void validate(boost::any& v, const std::vector& values, + projection_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "EQUIRECTANGULAR") == 0) v = EQUIRECTANGULAR; + else if(strcasecmp(arg.c_str(), "CYLINDRICAL") == 0) v = CYLINDRICAL; + else if(strcasecmp(arg.c_str(), "MERCATOR") == 0) v = MERCATOR; + else if(strcasecmp(arg.c_str(), "CONIC") == 0) v = CONIC; + else throw std::runtime_error(std::string("projection method ") + arg + std::string(" is unknown")); + } +} + +/* + * validates input type specification + */ +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +void reduction_option_dependency(const po::variables_map & vm, reduction_method stype, const char *option) +{ + if (vm.count("reduction") && vm["reduction"].as() == stype) { + if (!vm.count(option)) { + throw std::logic_error (string("this reduction option needs ")+option+" to be set"); + } + } +} + +void reduction_option_conflict(const po::variables_map & vm, reduction_method stype, const char *option) +{ + if (vm.count("reduction") && vm["reduction"].as() == stype) { + if (vm.count(option)) { + throw std::logic_error (string("this reduction option is incompatible with ")+option); + } + } +} + + +/* + * validates reduction method specification + */ +void validate(boost::any& v, const std::vector& values, + reduction_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "OCTREE") == 0) v = OCTREE; + else if(strcasecmp(arg.c_str(), "RANGE") == 0) v = RANGE; + else if(strcasecmp(arg.c_str(), "INTERPOLATE") == 0) v = INTERPOLATE; + else throw std::runtime_error(std::string("reduction method ") + arg + std::string(" is unknown")); +} + +void parse_options(int argc, char **argv, int &start, int &end, + bool &scanserver, int &width, int &height, + fbr::projection_method &ptype, string &dir, IOType &iotype, + int &maxDist, int &minDist, reduction_method &rtype, double &scale, + double &voxel, int &octree, bool &use_reflectance) +{ + po::options_description generic("Generic options"); + generic.add_options() + ("help,h", "output this help message"); + + po::options_description input("Input options"); + input.add_options() + ("start,s", po::value(&start)->default_value(0), + "start at scan (i.e., neglects the first scans) " + "[ATTENTION: counting naturally starts with 0]") + ("end,e", po::value(&end)->default_value(-1), + "end after scan ") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose F from {uos, uos_map, " + "uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, " + "riegl_txt, riegl_rgb, riegl_bin, zahn, ply})") + ("max,M", po::value(&maxDist)->default_value(-1), + "neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&minDist)->default_value(-1), + "neglegt all data points with a distance smaller than 'units") + ("scanserver,S", po::bool_switch(&scanserver), + "Use the scanserver as an input method and handling of scan data"); + + po::options_description reduction("Reduction options"); + reduction.add_options() + ("reduction,r", po::value(&rtype)->required(), + "choose reduction method (OCTREE, RANGE, INTERPOLATE)") + ("scale,S", po::value(&scale), + "scaling factor") + ("voxel,v", po::value(&voxel), + "voxel size") + ("projection,P", po::value(&ptype), + "projection method or panorama image") + ("octree,O", po::value(&octree), + "0 -> center\n1 -> random\nN>1 -> random N") + ("width,w", po::value(&width), + "width of panorama") + ("height,h", po::value(&height), + "height of panorama"); + + po::options_description output("Output options"); + output.add_options() + ("reflectance,R", po::bool_switch(&use_reflectance), + "Use reflectance when reducing points and save scan files in UOSR format"); + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + // all options + po::options_description all; + all.add(generic).add(input).add(reduction).add(output).add(hidden); + + // options visible with --help + po::options_description cmdline_options; + cmdline_options.add(generic).add(input).add(reduction).add(output); + + // positional argument + po::positional_options_description pd; + pd.add("input-dir", 1); + + // process options + po::variables_map vm; + po::store(po::command_line_parser(argc, argv). + options(all).positional(pd).run(), vm); + + // display help + if (vm.count("help")) { + cout << cmdline_options; + cout << endl + << "Example usage:" << endl + << "\t./bin/scan_red -s 0 -e 0 -f uos --reduction OCTREE --voxel 10 --octree 0 dat" << endl + << "\t./bin/scan_red -s 0 -e 0 -f uos --reduction RANGE --scale 0.5 --projection EQUIRECTANGULAR --width 3600 --height 1000 dat" << endl + << "\t./bin/scan_red -s 0 -e 0 -f uos --reduction INTERPOLATE --scale 0.2 --projection EQUIRECTANGULAR --width 3600 --height 1000 dat" << endl; + exit(0); + } + + po::notify(vm); + + reduction_option_dependency(vm, OCTREE, "voxel"); + reduction_option_dependency(vm, OCTREE, "octree"); + reduction_option_conflict(vm, OCTREE, "scale"); + reduction_option_conflict(vm, OCTREE, "projection"); + reduction_option_conflict(vm, OCTREE, "width"); + reduction_option_conflict(vm, OCTREE, "height"); + + reduction_option_conflict(vm, RANGE, "voxel"); + reduction_option_conflict(vm, RANGE, "octree"); + reduction_option_dependency(vm, RANGE, "scale"); + reduction_option_dependency(vm, RANGE, "projection"); + reduction_option_dependency(vm, RANGE, "width"); + reduction_option_dependency(vm, RANGE, "height"); + + reduction_option_conflict(vm, INTERPOLATE, "voxel"); + reduction_option_conflict(vm, INTERPOLATE, "octree"); + reduction_option_dependency(vm, INTERPOLATE, "scale"); + reduction_option_dependency(vm, INTERPOLATE, "projection"); + reduction_option_dependency(vm, INTERPOLATE, "width"); + reduction_option_dependency(vm, INTERPOLATE, "height"); + #ifndef _MSC_VER - const string bold("\033[1m"); - const string normal("\033[m"); + if (dir[dir.length()-1] != '/') dir = dir + "/"; #else - const string bold(""); - const string normal(""); + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; #endif - cout << endl - << bold << "USAGE " << normal << endl - << " " << prog << " [options] -r directory" << endl << endl; - cout << bold << "OPTIONS" << normal << endl - << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl - << " start at scan NR (i.e., neglects the first NR scans)" << endl - << " [ATTENTION: counting naturally starts with 0]" << endl - << endl - << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl - << " end after scan NR" << endl - << endl - << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl - << " using shared library F for input" << endl - << " (choose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl - << endl - << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl - << " neglegt all data points with a distance larger than NR 'units'" << endl - << endl - << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl - << " neglegt all data points with a distance smaller than NR 'units'" << endl - << endl - << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl - << " if NR >= 0, turns on octree based point reduction (voxel size=)" << endl - << " if NR < 0, turns on rescaling based reduction" << endl - << endl - << bold << " -I" << normal << " NR," << bold << "--rangeimage=" << normal << "NR" << endl - << " use rescaling of the range image as reduction method" << endl - << " if NR = 1 recovers ranges from range image" << endl - << " if NR = 2 interpolates 3D points in the image map" << endl - << " if NR is omitted, then NR=1 is selected" << endl - << endl - << bold << " -p" << normal << " MET," << bold << "--projection=" << normal << "MET" << endl - << " create range image using the MET projection method" << endl - << " (choose MET from [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|CONIC])" << endl - << bold << " -S, --scanserver" << normal << endl - << " Use the scanserver as an input method and handling of scan data" << endl - << endl << endl; - - cout << bold << "EXAMPLES " << normal << endl - << " " << prog << " -m 500 -r 5 dat" << endl - << " " << prog << " --max=5000 -r 10.2 dat" << endl - << " " << prog << " -s 2 -e 10 -r dat" << endl - << " " << prog << " -s 0 -e 1 -r 10 -I=1 dat " << endl << endl; - exit(1); } -/** A function that parses the command-line arguments and sets the respective flags. - * @param argc the number of arguments - * @param argv the arguments - * @param dir the directory - * @param red using point reduction? - * @param rand use randomized point reduction? - * @param start starting at scan number 'start' - * @param end stopping at scan number 'end' - * @param maxDist - maximal distance of points being loaded - * @param minDist - minimal distance of points being loaded - * @param projection - projection method for building range image - * @param quiet switches on/off the quiet mode - * @param veryQuiet switches on/off the 'very quiet' mode - * @return 0, if the parsing was successful. 1 otherwise - */ -int parseArgs(int argc, char **argv, string &dir, double &red, - int &start, int &end, int &maxDist, int &minDist, - string &projection, int &octree, IOType &type, - int &rangeimage, bool &scanserver) +void createdirectory(string dir) { - bool reduced = false; - int c; - // from unistd.h: - extern char *optarg; - extern int optind; + int success = mkdir(dir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); - WriteOnce w_type(type); - WriteOnce w_start(start), w_end(end); + if (success == 0 || errno == EEXIST) { + cout << "Writing to " << dir << endl; + } else { + cerr << "Creating directory " << dir << " failed" << endl; + exit(1); + } +} - /* options descriptor */ - // 0: no arguments, 1: required argument, 2: optional argument - static struct option longopts[] = { - { "format", required_argument, 0, 'f' }, - { "max", required_argument, 0, 'm' }, - { "min", required_argument, 0, 'M' }, - { "start", required_argument, 0, 's' }, - { "end", required_argument, 0, 'e' }, - { "reduce", required_argument, 0, 'r' }, - { "octree", optional_argument, 0, 'O' }, - { "rangeimage", optional_argument, 0, 'I' }, - { "projection", required_argument, 0, 'p' }, - { "scanserver", no_argument, 0, 'S' }, - { 0, 0, 0, 0} // needed, cf. getopt.h - }; +void scan2mat(Scan *source, cv::Mat &mat) +{ + DataXYZ xyz = source->get("xyz"); + DataReflectance xyz_reflectance = (((DataReflectance)source->get("reflectance")).size() == 0) ? + source->create("reflectance", sizeof(float)*xyz.size()) + : source->get("reflectance"); + if(((DataReflectance)source->get("reflectance")).size() == 0){ + for(unsigned int i = 0; i < xyz.size(); i++) + xyz_reflectance[i] = 255; + } + unsigned int nPoints = xyz.size(); + mat.create(nPoints,1,CV_32FC(4)); + mat = cv::Scalar::all(0); + cv::MatIterator_ it = mat.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float reflectance = xyz_reflectance[i]; + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; - cout << endl; - while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:O:p:", longopts, NULL)) != -1) - switch (c) - { - case 'r': - red = atof(optarg); - reduced = true; - break; - case 's': - w_start = atoi(optarg); - if (w_start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } - break; - case 'e': - w_end = atoi(optarg); - if (w_end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } - if (w_end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } - break; - case 'f': - try { - w_type = formatname_to_io_type(optarg); - } catch (...) { // runtime_error - cerr << "Format " << optarg << " unknown." << endl; - abort(); - } - break; - case 'm': - maxDist = atoi(optarg); - break; - case 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } - break; - case 'M': - minDist = atoi(optarg); - break; - case 'I': - if (optarg) { - rangeimage = atoi(optarg); - } else { - rangeimage = 1; - } - break; - case 'p': - projection = optarg; - break; - case 'S': - scanserver = true; - break; - case '?': - usage(argv[0]); - return 1; - default: - abort (); + (*it)[0] = xyz[i][0]; + (*it)[1] = xyz[i][1]; + (*it)[2] = xyz[i][2]; + (*it)[3] = reflectance; + ++it; + } +} + +void reduce_octree(Scan *scan, vector &reduced_points, int octree, + int red, bool use_reflectance) +{ + if (use_reflectance) { + unsigned int types = PointType::USE_REFLECTANCE; + PointType pointtype(types); + scan->setReductionParameter(red, octree, pointtype); + scan->calcReducedPoints(); + + DataXYZ xyz_reduced(scan->get("xyz reduced")); + DataReflectance reflectance_reduced(scan->get("reflectance reduced")); + + if (xyz_reduced.size() != reflectance_reduced.size()) { + cerr << "xyz_reduced size different than reflectance_reduced size" << endl; + return; } + + for(unsigned int j = 0; j < xyz_reduced.size(); j++) { + reduced_points.push_back(cv::Vec4f(xyz_reduced[j][0], xyz_reduced[j][1], xyz_reduced[j][2], reflectance_reduced[j])); + } + } + else { + scan->setReductionParameter(red, octree); + scan->calcReducedPoints(); - if(!reduced) { - cerr << "\n*** Reduction method missed ***" << endl; - usage(argv[0]); - } - if (optind != argc-1) { - cerr << "\n*** Directory missing ***" << endl; - usage(argv[0]); - } - dir = argv[optind]; - -#ifndef _MSC_VER - if (dir[dir.length()-1] != '/') dir = dir + "/"; -#else - if (dir[dir.length()-1] != '\\') dir = dir + "\\"; -#endif - - parseFormatFile(dir, w_type, w_start, w_end); - - return 0; + DataXYZ xyz_reduced(scan->get("xyz reduced")); + for(unsigned int j = 0; j < xyz_reduced.size(); j++) { + reduced_points.push_back(cv::Vec4f(xyz_reduced[j][0], xyz_reduced[j][1], xyz_reduced[j][2], 0.0)); + } + } } +void reduce_range(Scan *scan, vector &reduced_points, int width, + int height, fbr::projection_method ptype, double scale, + bool use_reflectance) +{ + panorama image(width, height, ptype); + cv::Mat mat; + scan2mat(scan, mat); + image.createPanorama(mat); + image.getDescription(); + + cv::Mat range_image_resized; + cv::Mat reflectance_image_resized; + resize(image.getRangeImage(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + if (use_reflectance) { + resize(image.getReflectanceImage(), reflectance_image_resized, + cv::Size(), scale, scale, cv::INTER_NEAREST); + } else { + reflectance_image_resized.create(range_image_resized.size(), CV_8U); + reflectance_image_resized = cv::Scalar::all(0); + } + image.recoverPointCloud(range_image_resized, reflectance_image_resized, reduced_points); +} + +void reduce_interpolation(Scan *scan, vector &reduced_points, + int width, int height, fbr::projection_method ptype, double scale, + bool use_reflectance) +{ + panorama image(width, height, ptype); + cv::Mat mat; + scan2mat(scan, mat); + image.createPanorama(mat); + image.getDescription(); + + cv::Mat range_image_resized; + cv::Mat reflectance_image_resized; + resize(image.getMap(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + if (use_reflectance) { + resize(image.getReflectanceImage(), reflectance_image_resized, + cv::Size(), scale, scale, cv::INTER_NEAREST); + } + for(int i = 0; i < range_image_resized.rows; i++) { + for(int j = 0; j < range_image_resized.cols; j++) { + cv::Vec3f vec = range_image_resized.at(i, j); + if (use_reflectance) { + reduced_points.push_back(cv::Vec4f( + vec[0], vec[1], vec[2], + reflectance_image_resized.at(i, j)/255.0)); + } else { + reduced_points.push_back(cv::Vec4f(vec[0], vec[1], vec[2], 0.0)); + } + } + } +} + +/* + * given a vector of 3d points, write them out as uos files + */ +void write_uos(vector &points, string &dir, string id) +{ + ofstream outfile(dir + "/scan" + id + ".3d"); + + outfile << "# header is ignored" << endl; + + for (vector::iterator it=points.begin(); it < points.end(); it++) { + outfile << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << endl; + } + + outfile.close(); +} + +/* + * given a vector of 3d points, write them out as uosr files + */ +void write_uosr(vector &points, string &dir, string id) +{ + ofstream outfile(dir + "/scan" + id + ".3d"); + + outfile << "# header is ignored" << endl; + + for (vector::iterator it=points.begin(); it < points.end(); it++) { + outfile << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << " " << (*it)[3] << endl; + } + + outfile.close(); +} + +// write .pose files +// .frames files can later be generated from them using ./bin/pose2frames +void writeposefile(string &dir, const double* rPos, const double* rPosTheta, string id) +{ + ofstream posefile(dir + "/scan" + id + ".pose"); + posefile << rPos[0] << " " << rPos[1] << " " << rPos[2] << endl; + posefile << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + posefile.close(); +} /** * Main program for reducing scans. @@ -281,186 +466,63 @@ int parseArgs(int argc, char **argv, string &dir, double &red, */ int main(int argc, char **argv) { + int start, end; + bool scanserver; + int width, height; + int maxDist, minDist; + fbr::projection_method ptype; + string dir; + IOType iotype; + reduction_method rtype; + double scale, voxel; + int octree; + bool use_reflectance; - cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl; - - if (argc <= 1) { - usage(argv[0]); - } + parse_options(argc, argv, start, end, scanserver, width, height, ptype, + dir, iotype, maxDist, minDist, rtype, scale, voxel, octree, + use_reflectance); - // parsing the command line parameters - // init, default values if not specified - string dir; - double red = -1.0; - int start = 0, end = -1; - int maxDist = -1; - int minDist = -1; - string projection = "EQUIRECTANGULAR"; - int octree = 0; - IOType type = RIEGL_TXT; - int rangeimage = 0; - bool scanserver = false; - - parseArgs(argc, argv, dir, red, start, end, maxDist, minDist, projection, - octree, type, rangeimage, scanserver); - - if (scanserver) { - try { - ClientInterface::create(); - } catch(std::runtime_error& e) { - cerr << "ClientInterface could not be created: " << e.what() << endl; - cerr << "Start the scanserver first." << endl; - exit(-1); - } - } - - // Get Scans - string reddir = dir + "reduced"; - -#ifdef _MSC_VER - int success = mkdir(reddir.c_str()); -#else - int success = mkdir(reddir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); -#endif - if(success == 0) { - cout << "Writing scans to " << reddir << endl; - } else if(errno == EEXIST) { - cout << "Directory " << reddir << " exists already. CONTINUE" << endl; - } else { - cerr << "Creating directory " << reddir << " failed" << endl; - exit(1); - } - - Scan::openDirectory(scanserver, dir, type, start, end); - if(Scan::allScans.size() == 0) { - cerr << "No scans found. Did you use the correct format?" << endl; - exit(-1); - } - - string scanFileName; - string poseFileName; - - /// Use the OCTREE based reduction - if (rangeimage == 0) { - cout << endl << "Reducing point cloud using octrees" << endl; - int scan_number = start; - for(std::vector::iterator it = Scan::allScans.begin(); - it != Scan::allScans.end(); - ++it, ++scan_number) { - Scan* scan = *it; - const double* rPos = scan->get_rPos(); - const double* rPosTheta = scan->get_rPosTheta(); + for (int iter = start; iter <= end; iter++) { - scan->setRangeFilter(maxDist, minDist); - scan->setReductionParameter(red, octree); - // get reduced points - DataXYZ xyz_r(scan->get("xyz reduced")); - unsigned int nPoints = xyz_r.size(); + Scan::openDirectory(scanserver, dir, iotype, iter, iter); - const char* id = scan->getIdentifier(); - cout << "Writing Scan No. " << id; - cout << " with " << xyz_r.size() << " points" << endl; - scanFileName = reddir + "/scan" + id + ".3d"; - poseFileName = reddir + "/scan" + id + ".pose"; - - ofstream redptsout(scanFileName.c_str()); - for(unsigned int j = 0; j < nPoints; j++) { - redptsout << xyz_r[j][0] << " " - << xyz_r[j][1] << " " - << xyz_r[j][2] << endl; - } - redptsout.close(); - redptsout.clear(); - - ofstream posout(poseFileName.c_str()); - posout << rPos[0] << " " - << rPos[1] << " " - << rPos[2] << endl - << deg(rPosTheta[0]) << " " - << deg(rPosTheta[1]) << " " - << deg(rPosTheta[2]) << endl; - - posout.close(); - posout.clear(); - if (scanserver) { - scan->clear("xyz reduced"); - } - } - } else { /// use the RESIZE based reduction - cout << endl << "Reducing point cloud by rescaling the range image" << endl; - - Scan::openDirectory(false, dir, type, start, end); - if (Scan::allScans.size() <= 0) { - cerr << "No scans found!" << endl; - exit(-1); - } - - for (int scan_number = start; scan_number <= end; scan_number++) { - - Scan* scan = Scan::allScans[scan_number]; - scan->setRangeFilter(maxDist, minDist); - const double* rPos = scan->get_rPos(); - const double* rPosTheta = scan->get_rPosTheta(); - - scanFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".3d"; - poseFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".pose"; - // Create a panorama. The iMap inside does all the tricks for us. - scan_cv sScan(dir, scan_number, type); - sScan.convertScanToMat(); - - /// Project point cloud using the selected projection method - panorama image(IMAGE_WIDTH, IMAGE_HEIGHT, strToPMethod(projection)); - image.createPanorama(sScan.getMatScan()); - image.getDescription(); - - /// Resize the range image, specify desired interpolation method - double scale = 1.0/red; - cv::Mat range_image_resized; // reflectance_image_resized; - string ofilename; - stringstream ss; - ss << setw(3) << setfill('0') << (scan_number); - ofilename = reddir + "/scan" + ss.str() + ".3d"; - if (rangeimage == 1) { - resize(image.getRangeImage(), range_image_resized, cv::Size(), - scale, scale, cv::INTER_NEAREST); - // Recover point cloud from image and write scan to file - stringstream ss; - ss << setw(3) << setfill('0') << (scan_number); - image.recoverPointCloud(range_image_resized, ofilename); - } else { - resize(image.getMap(), range_image_resized, cv::Size(), - scale, scale, cv::INTER_NEAREST); - ofstream redptsout(ofilename.c_str()); - // Convert back to 3D. - for(int i = 0; i < range_image_resized.rows; i++) { - for(int j = 0; j < range_image_resized.cols; j++) { - cv::Vec3f vec = range_image_resized.at(i, j); - double x = vec[0]; - double y = vec[1]; - double z = vec[2]; - redptsout << x << " " << y << " " << z << endl; - } - } + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); } - ofstream posout(poseFileName.c_str()); - posout << rPos[0] << " " - << rPos[1] << " " - << rPos[2] << endl - << deg(rPosTheta[0]) << " " - << deg(rPosTheta[1]) << " " - << deg(rPosTheta[2]) << endl; - posout.clear(); - posout.close(); + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + scan->setRangeFilter(maxDist, minDist); + + vector reduced_points; + + string reddir = dir + "reduced"; + createdirectory(reddir); + + switch (rtype) { + case OCTREE: + reduce_octree(scan, reduced_points, octree, voxel, use_reflectance); + break; + case RANGE: + reduce_range(scan, reduced_points, width, height, ptype, scale, use_reflectance); + break; + case INTERPOLATE: + reduce_interpolation(scan, reduced_points, width, height, ptype, scale, use_reflectance); + break; + default: + cerr << "unknown method" << endl; + return 1; + break; + } + + if (use_reflectance) + write_uosr(reduced_points, reddir, scan->getIdentifier()); + else + write_uos(reduced_points, reddir, scan->getIdentifier()); + writeposefile(reddir, scan->get_rPos(), scan->get_rPosTheta(), scan->getIdentifier()); + } + Scan::closeDirectory(); } - } - - cout << endl << endl; - cout << "Normal program end." << endl << endl; - - if (scanserver) { - Scan::closeDirectory(); - ClientInterface::destroy(); - } } - diff --git a/src/thermo/CMakeLists.txt b/src/thermo/CMakeLists.txt index 8d5e7f8..51155b7 100644 --- a/src/thermo/CMakeLists.txt +++ b/src/thermo/CMakeLists.txt @@ -14,13 +14,13 @@ IF (WITH_THERMO) IF(UNIX) target_link_libraries(caliboard scan shape newmat dl ANN) target_link_libraries(thermo scan shape newmat dl ANN) - target_link_libraries(thermo cv cvaux cxcore GL GLU highgui cvblob scan ANN) + target_link_libraries(thermo GL GLU cvblob ${OpenCV_LIBS} scan ANN) ENDIF(UNIX) IF (WIN32) target_link_libraries(caliboard scan shape newmat XGetopt ANN) target_link_libraries(thermo scan shape newmat XGetopt ANN) - target_link_libraries(thermo cv cvaux cxcore GL GLU highgui cvblob scan ANN) + target_link_libraries(thermo GL GLU cvblob ${OpenCV_LIBS} scan ANN) ENDIF(WIN32) diff --git a/src/thermo/thermo.cc b/src/thermo/thermo.cc index 8c4ad8f..822a845 100644 --- a/src/thermo/thermo.cc +++ b/src/thermo/thermo.cc @@ -190,7 +190,6 @@ void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, b IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]) { IplImage *gray_image = cvCloneImage(org_image); - //cvThreshold(gray_image, gray_image, 100, 255, CV_THRESH_BINARY); cvThreshold(gray_image, gray_image, GRAY_TH, 255, CV_THRESH_BINARY); IplImage *labelImg = cvCreateImage(cvGetSize(gray_image), IPL_DEPTH_LABEL, 1); @@ -270,7 +269,7 @@ IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int boa * Connects the detected calibration features in the image with lines. */ void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color) { - for (int i = 4; i <= corner_exp - 2; i++) { + for (int i = 0; i <= corner_exp - 2; i++) { CvPoint pt1; CvPoint pt2; CvScalar s; @@ -368,7 +367,6 @@ IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int bo cout << "found corners:" << corner_exp << endl; if (found != 0) {//if all corners found successfully - //if (corner_exp != 0) {//if all corners found successfully //Get Subpixel accuracy on those corners if(size.width > 400) { cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(11, 11), cvSize(-1, -1), @@ -379,7 +377,6 @@ IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int bo cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); } } - cout << "blub " << found << endl; for (int i = 0; i < corner_exp; i++) { point_array2[i][0] = corners[i].x; @@ -410,8 +407,6 @@ image_points, CvSize size, string dir, string substring) { CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1); CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; - //CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; - //CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; CV_MAT_ELEM( *object_points2, float, i, 2) = 0.0f; } for (int i = 0; i < images; ++i) { //These are all the same number @@ -423,10 +418,10 @@ image_points, CvSize size, string dir, string substring) { CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f; //CALIBRATE THE CAMERA! cvCalibrateCamera2(object_points2, image_points2, point_counts2, size, - intrinsic_matrix, distortion_coeffs, Rotation, Translation, 0 //CV_CALIB_FIX_ASPECT_RATIO + intrinsic_matrix, distortion_coeffs, Rotation, Translation, 0 ); - // SAVE AND PRINT THE INTRINSICS AND DISTORTIONS + // SAVE AND PRINT THE INTRINSICS AND DISTORTIONS string file = dir + "Intrinsics" + substring + ".xml"; cvSave(file.c_str(), intrinsic_matrix); file = dir + "Distortion" + substring + ".xml"; @@ -467,10 +462,6 @@ chess, bool quiet, string dir, int scale) { cvNamedWindow("Final Result", 0); cvResizeWindow( "Original Image", 480, 640 ); cvResizeWindow( "Final Result", 480, 640 ); - /* - cvNamedWindow("Final Result", 0); - cvResizeWindow( "Final Result", 320, 240 ); - */ int nr_img = end - start + 1; if (nr_img == 0) { cout << "ImageCount is zero!" << endl; @@ -496,14 +487,10 @@ chess, bool quiet, string dir, int scale) { if(optical) { //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; t = dir + "/photo" + to_string(count, 3) + ".jpg"; - //t1 = dir + "/cimage" + to_string(count, 3) + ".ppm"; //t = dir + to_string(count, 3) + "/photo" + to_string(count, 3) + ".ppm"; - //t1 = dir + to_string(count, 3) + "/cimage" + to_string(count, 3) + ".ppm"; } else { //t = dir + to_string(count, 3) + "/image" + to_string(count, 3) + ".ppm"; - //t1 = dir + to_string(count, 3) + "/timage" + to_string(count, 3) + ".ppm"; t = dir + "/image" + to_string(count, 3) + ".ppm"; - //t1 = dir + "/timage" + to_string(count, 3) + ".ppm"; } cout << t << endl; //loading images and finding corners @@ -515,7 +502,6 @@ chess, bool quiet, string dir, int scale) { cvShowImage("Original Image", image1); ///////////////////////////////////////////////////////////// - double point_array2[corner_exp][2]; IplImage *image; @@ -579,7 +565,6 @@ chess, bool quiet, string dir, int scale) { writeCalibParam(successes, corner_exp, board_w, image_points, size, dir, substring); cvReleaseMat(&image_points); - } /** @@ -987,12 +972,9 @@ void ExtrCalibFunc(int board_w, int board_h, int start, int end, bool optical, b //ALLOCATE STORAGE(depending upon the number of images in(in case if command line arguments are given ) //not on the basis of number of images in which all corner extracted/while in the other case the number is the same ) - string substring = optical? "Optical" : ""; - string file = dir + "Intrinsics" + substring + ".xml"; - cout << file << endl; - CvMat *intrinsic = (CvMat*) cvLoad(file.c_str()); - file = dir + "Distortion" + substring + ".xml"; - CvMat *distortion = (CvMat*) cvLoad(file.c_str()); + CvMat *intrinsic; + CvMat *distortion; + loadIntrinsicCalibration(intrinsic, distortion, dir, optical); //for storing the rotations and translation vectors @@ -1134,6 +1116,7 @@ void ExtrCalibFunc(int board_w, int board_h, int start, int end, bool optical, b cout << "Number of successes: " << successes << endl; // Now calculating mean and median rotation and trans + string substring = optical? "Optical" : ""; calculateExtrinsics(rotation_vectors_temp, translation_vectors_temp, successes, dir, quiet, substring); calculateExtrinsicsWithReprojectionCheck(points2D, points3D, rotation_vectors_temp, translation_vectors_temp, distortion, intrinsic, corner_exp, successes, dir, quiet, substring); cvReleaseMat(&intrinsic); @@ -1189,31 +1172,9 @@ void writeGlobalCameras(int start, int end, bool optical, bool quiet, string dir cout << "ImageCount is zero!" << endl; return; } - string substring = optical? "Optical" : ""; - switch(method) { - case 0: - file = dir + "Rotation" + substring + ".xml"; - break; - case 1: - file = dir + "RotationMedian" + substring + ".xml"; - break; - case 2: - file = dir + "RotationMean" + substring + ".xml"; - break; - } - CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); - switch(method) { - case 0: - file = dir + "Translation" + substring + ".xml"; - break; - case 1: - file = dir + "TranslationMedian" + substring + ".xml"; - break; - case 2: - file = dir + "TranslationMean" + substring + ".xml"; - break; - } - CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + CvMat *Rotation; + CvMat *Translation; + loadExtrinsicCalibration(Rotation, Translation, dir, method, optical); double starttime = GetCurrentTimeInMilliSec(); @@ -1355,35 +1316,13 @@ void calculateGlobalCameras(int start, int end, bool optical, bool quiet, string cout << "ImageCount is zero!" << endl; return; } - string substring = optical? "Optical" : ""; - string file = dir + "Intrinsics" + substring + ".xml"; - CvMat *intrinsic = (CvMat*) cvLoad(file.c_str()); - file = dir + "Distortion" + substring + ".xml"; - CvMat *distortion = (CvMat*) cvLoad(file.c_str()); - switch(method) { - case 0: - file = dir + "Rotation" + substring + ".xml"; - break; - case 1: - file = dir + "RotationMedian" + substring + ".xml"; - break; - case 2: - file = dir + "RotationMean" + substring + ".xml"; - break; - } - CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); - switch(method) { - case 0: - file = dir + "Translation" + substring + ".xml"; - break; - case 1: - file = dir + "TranslationMedian" + substring + ".xml"; - break; - case 2: - file = dir + "TranslationMean" + substring + ".xml"; - break; - } - CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + CvMat *intrinsic; + CvMat *distortion; + loadIntrinsicCalibration(intrinsic, distortion, dir, optical); + CvMat *Rotation; + CvMat *Translation; + loadExtrinsicCalibration(Translation, Rotation, dir, method, optical); + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); for (int hh = 0; hh < 5; hh++) { CV_MAT_ELEM(*undistort, float,hh,0) = 0; @@ -1759,24 +1698,17 @@ void calculateGlobalCameras(int start, int end, bool optical, bool quiet, string } -/** - * Main function for projecting the 3D points onto the corresponding image and - * associating temperature values to the data points. - */ -void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, - IOType type, int scale, double rot_angle, double minDist, double maxDist, - bool correction, int neighborhood, int method) { - - int nr_img = end - start + 1; - if (nr_img < 1) { - cout << "ImageCount is zero!" << endl; - return; - } +void loadIntrinsicCalibration(CvMat * intrinsic, CvMat * distortion, string dir, bool optical) { string substring = optical? "Optical" : ""; string file = dir + "Intrinsics" + substring + ".xml"; - CvMat *intrinsic = (CvMat*) cvLoad(file.c_str()); + intrinsic = (CvMat*) cvLoad(file.c_str()); file = dir + "Distortion" + substring + ".xml"; - CvMat *distortion = (CvMat*) cvLoad(file.c_str()); + distortion = (CvMat*) cvLoad(file.c_str()); +} + +void loadExtrinsicCalibration(CvMat * Translation, CvMat * Rotation, string dir, int method, bool optical) { + string substring = optical? "Optical" : ""; + string file; switch(method) { case 0: file = dir + "Rotation" + substring + ".xml"; @@ -1788,7 +1720,7 @@ void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, file = dir + "RotationMean" + substring + ".xml"; break; } - CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); + Rotation = (CvMat*) cvLoad(file.c_str()); switch(method) { case 0: file = dir + "Translation" + substring + ".xml"; @@ -1800,17 +1732,10 @@ void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, file = dir + "TranslationMean" + substring + ".xml"; break; } - CvMat *Translation = (CvMat*) cvLoad(file.c_str()); - CvMat* undistort = cvCreateMat(5,1,CV_32FC1); - for (int hh = 0; hh < 5; hh++) { - CV_MAT_ELEM(*undistort, float,hh,0) = 0; - } + Translation = (CvMat*) cvLoad(file.c_str()); +} - double starttime = GetCurrentTimeInMilliSec(); - - stringstream outdat; - int pointcnt = 0; - string outdir = dir + "/labscan-map"; +void openOutputDirectory(string outdir) { #ifdef _MSC_VER int success = mkdir(outdir.c_str()); #else @@ -1824,38 +1749,139 @@ void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, cerr << "Creating directory " << outdir << " failed" << endl; exit(1); } +} + +int openDirectory(CvMat* point_3Dcloud, string dir, IOType type, int count) { + // reading the 3D points and projecting them back to 2d + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); + + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + } + return red_size; +} + +void loadImage(IplImage *image, string dir, int count0, bool optical, int scale){ + string t, t0; + if(optical) { + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + exit(0); + } + + image = resizeImage(image, scale); +} + +void calculateGlobalPoses(CvMat *Translation, CvMat *Rotation, CvMat *t_comI, +CvMat *rod_comI, double angle, CvMat *rot_tmp) { + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + //cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + //cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + rot_tmp = cvCreateMat(3,3,CV_32FC1); + //CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + rod_comI = cvCreateMat(3,1,CV_32FC1); + t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + //cout << endl; + //cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + /* + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + */ + } + //cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + +} + +/** + * Main function for projecting the 3D points onto the corresponding image and + * associating temperature values to the data points. + */ +void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { + + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + + CvMat *distortion; + CvMat *intrinsic; + CvMat *Translation; + CvMat *Rotation; + loadExtrinsicCalibration(Translation, Rotation,dir,method,optical); + loadIntrinsicCalibration(intrinsic,distortion,dir,optical); + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; + } + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + int pointcnt = 0; + string outdir = dir + "/labscan-map"; + openOutputDirectory(outdir); + for (int count = start; count <= end; count++) { // filling the rotation matrix - CvMat* point_3Dcloud; - CvMat* point_2Dcloud; - CvMat* undistort_2Dcloud; - - // reading the 3D points and projecting them back to 2d - Scan::openDirectory(false, dir, type, count, count); - Scan::allScans[0]->setRangeFilter(-1, -1); - Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); - Scan::allScans[0]->setReductionParameter(-1, 0); - //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); - DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); - int red_size = reduced.size(); - - point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); - cout << "Points: " << red_size << endl; - point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); - undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); - cout << "readScans done" << endl; - for (int j = 0; j < red_size; j++) { - Point p(reduced[j]); - // TODO make sure correct points are printed - - CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; - CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; - CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; - - } - int nr_points = red_size; - cout << "Number of points read: " << red_size << endl; + CvMat *point_3Dcloud; + int nr_points = openDirectory(point_3Dcloud, dir, type, count); + + CvMat* point_2Dcloud = cvCreateMat(nr_points, 2, CV_32FC1); + CvMat* undistort_2Dcloud = cvCreateMat(nr_points, 2, CV_32FC1); + + cout << "Number of points read: " << nr_points << endl; delete Scan::allScans[0]; Scan::allScans.clear(); @@ -1874,80 +1900,22 @@ void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, // loading images int count0 = count * nrP360 + p; - string t, t0; - if(optical) { - //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; - //t = dir + "/photo" + to_string(count0, 3) + "_2.jpg"; - //t = dir + "/photo" + to_string(count0, 3) + "_90.jpg"; - t = dir + "/photo" + to_string(count0, 3) + ".jpg"; - //t = dir + "/photo" + to_string(count0, 3) + "_1.jpg"; - } else { - t = dir + "/image" + to_string(count0, 3) + ".ppm"; - } - - IplImage* image = cvLoadImage(t.c_str(), -1); - if (!image) { - cout << "first image " << t << " cannot be loaded" << endl; - return; - } + IplImage *image; + loadImage(image, dir, count0, optical, scale); CvSize size = cvGetSize(image); - image = resizeImage(image, scale); - // rotate Rotation and Translation - CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); - CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); - CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); - cout << "Angle: " << angle << " " << rad(angle) << endl; - CV_MAT_ELEM(*rod40,float,0,0) = 0.0; - CV_MAT_ELEM(*rod40,float,1,0) = 0.0; - CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); - cout << "tmp" << endl; - CvMat* t40 = cvCreateMat(3,1,CV_32FC1); - CV_MAT_ELEM(*t40,float,0,0) = 0.0; - CV_MAT_ELEM(*t40,float,1,0) = 0.0; - CV_MAT_ELEM(*t40,float,2,0) = 0.0; - cout << "tmp2" << endl; + CvMat* rod_comI; + CvMat* t_comI; CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); - CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); - CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); - CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); - CvMat* t_com = cvCreateMat(1,3,CV_32FC1); - cout << "tmp3" << endl; - for(int w = 0; w < 3; w++) { - CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); - CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); - } - cout << endl; - cout << "Final Rotation" << endl; - - cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); - for(int w = 0; w < 3; w++) { - CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); - CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); - cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; - cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; - cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; - cout << CV_MAT_ELEM(*t40,float,w,0) << " "; - cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; - cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; - } - cout << endl; - - cvRodrigues2(rod_comI, rot_tmp); + calculateGlobalPoses(Translation, Rotation, t_comI, rod_comI, angle, rot_tmp); // Project Points cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); - cvReleaseMat(&rod40); - cvReleaseMat(&RotationI); - cvReleaseMat(&TranslationI); - cvReleaseMat(&t40); - cvReleaseMat(&rod_comI); - cvReleaseMat(&rod_com); - cvReleaseMat(&t_com); cvReleaseMat(&t_comI); + cvReleaseMat(&rod_comI); cout << "Done projecting points" << endl; //for counting how many points get mapped to first and second image file @@ -2126,6 +2094,7 @@ void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, cvReleaseMat(&undistort_2Dcloud); } + // Final cleanup cvReleaseMat(&intrinsic); cvReleaseMat(&distortion); cvReleaseMat(&Rotation); diff --git a/src/veloslam/veloshow.cc b/src/veloslam/veloshow.cc index 34be143..ad9c313 100644 --- a/src/veloslam/veloshow.cc +++ b/src/veloslam/veloshow.cc @@ -925,9 +925,7 @@ void initShow(int argc, char **argv){ #endif } } else { -#ifndef USE_GL_POINTS - createDisplayLists(red > 0); -#elif USE_COMPACT_TREE +#if USE_COMPACT_TREE cout << "Creating compact display octrees.." << endl; for(int i = 0; i < (int)Scan::allScans.size() ; i++) { compactTree *tree;