update to svn rev 735
This commit is contained in:
josch
parent
5255e4f9f0
commit
29567b8de4
18 changed files with 5729 additions and 173 deletions
1166
.svn/pristine/33/3311774e885a91641251368710b1a05a30391331.svn-base
Normal file
1166
.svn/pristine/33/3311774e885a91641251368710b1a05a30391331.svn-base
Normal file
File diff suppressed because it is too large
Load diff
1570
.svn/pristine/40/40906d7a83a6c7878ccf6990ea0bd862525ab2f7.svn-base
Normal file
1570
.svn/pristine/40/40906d7a83a6c7878ccf6990ea0bd862525ab2f7.svn-base
Normal file
File diff suppressed because it is too large
Load diff
|
|
@ -0,0 +1,432 @@
|
||||||
|
cmake_minimum_required (VERSION 2.8.2)
|
||||||
|
SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH})
|
||||||
|
project (Slam6D)
|
||||||
|
|
||||||
|
#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 ##
|
||||||
|
#################################################
|
||||||
|
|
||||||
|
## FreeGLUT
|
||||||
|
OPTION(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
|
||||||
|
OPTION(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
|
||||||
|
OPTION(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_XWSHOW)
|
||||||
|
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)
|
||||||
|
IF(WITH_SHAPE_DETECTION)
|
||||||
|
MESSAGE(STATUS "With shape detection")
|
||||||
|
ELSE(WITH_SHAPE_DETECTION)
|
||||||
|
MESSAGE(STATUS "Without shape detection")
|
||||||
|
ENDIF(WITH_SHAPE_DETECTION)
|
||||||
|
|
||||||
|
## Interior reconstruction
|
||||||
|
option(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
|
||||||
|
OPTION(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF)
|
||||||
|
IF(WITH_THERMO)
|
||||||
|
FIND_PACKAGE(OpenCV REQUIRED)
|
||||||
|
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
|
||||||
|
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)
|
||||||
|
|
||||||
|
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?
|
||||||
|
OPTION(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
|
||||||
|
OPTION(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
|
||||||
|
OPTION(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF)
|
||||||
|
|
||||||
|
IF(WITH_VELOSLAM)
|
||||||
|
MESSAGE(STATUS "With VELOSLAM")
|
||||||
|
ELSE(WITH_VELOSLAM)
|
||||||
|
MESSAGE(STATUS "Without 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)
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
OPTION(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF)
|
||||||
|
|
||||||
|
IF(WITH_TOOLS)
|
||||||
|
MESSAGE(STATUS "With Tools")
|
||||||
|
ELSE(WITH_TOOLS)
|
||||||
|
MESSAGE(STATUS "Without Tools")
|
||||||
|
ENDIF(WITH_TOOLS)
|
||||||
|
|
||||||
|
## Segmentation
|
||||||
|
|
||||||
|
OPTION(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF)
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF)
|
||||||
|
|
||||||
|
IF(WITH_NORMALS)
|
||||||
|
MESSAGE(STATUS "With normals")
|
||||||
|
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)
|
||||||
|
|
||||||
|
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
|
||||||
|
OPTION(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
|
||||||
|
OPTION(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
|
||||||
|
OPTION(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF)
|
||||||
|
|
||||||
|
IF(WITH_FBR)
|
||||||
|
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)
|
||||||
|
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
|
||||||
|
OPTION(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
|
||||||
|
OPTION(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 <SOURCE_DIR>/configure --prefix=<INSTALL_DIR>
|
||||||
|
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)
|
||||||
|
|
||||||
|
OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF)
|
||||||
|
|
||||||
|
IF(EXPORT_SHARED_LIBS)
|
||||||
|
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)
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
|
IF(WIN32)
|
||||||
|
SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" )
|
||||||
|
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)
|
||||||
|
|
||||||
|
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)
|
||||||
|
IF(WITH_FBR)
|
||||||
|
add_subdirectory(src/slam6d/fbr)
|
||||||
|
ENDIF(WITH_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)
|
||||||
|
|
@ -0,0 +1,752 @@
|
||||||
|
/**
|
||||||
|
*
|
||||||
|
* Copyright (C) Jacobs University Bremen
|
||||||
|
*
|
||||||
|
* @author Vaibhav Kumar Mehta
|
||||||
|
* @file normals.cc
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <fstream>
|
||||||
|
#include <errno.h>
|
||||||
|
|
||||||
|
#include <boost/program_options.hpp>
|
||||||
|
|
||||||
|
#include <slam6d/io_types.h>
|
||||||
|
#include <slam6d/globals.icc>
|
||||||
|
#include <slam6d/scan.h>
|
||||||
|
#include "slam6d/fbr/panorama.h"
|
||||||
|
#include <scanserver/clientInterface.h>
|
||||||
|
|
||||||
|
#include <ANN/ANN.h>
|
||||||
|
#include "newmat/newmat.h"
|
||||||
|
#include "newmat/newmatap.h"
|
||||||
|
|
||||||
|
using namespace NEWMAT;
|
||||||
|
|
||||||
|
#ifdef _MSC_VER
|
||||||
|
#define strcasecmp _stricmp
|
||||||
|
#define strncasecmp _strnicmp
|
||||||
|
#else
|
||||||
|
#include <strings.h>
|
||||||
|
#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<std::string>& 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<std::string>& 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 <options> where options are (default values in brackets)");
|
||||||
|
cmd_options.add_options()
|
||||||
|
("help,?", "Display this help message")
|
||||||
|
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
|
||||||
|
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
|
||||||
|
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
|
||||||
|
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
|
||||||
|
"using shared library <arg> 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<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
|
||||||
|
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
|
||||||
|
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
|
||||||
|
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
|
||||||
|
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
|
||||||
|
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
|
||||||
|
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
|
||||||
|
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
|
||||||
|
;
|
||||||
|
|
||||||
|
po::options_description hidden("Hidden options");
|
||||||
|
hidden.add_options()
|
||||||
|
("input-dir", po::value<string>(&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/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" <<endl;
|
||||||
|
cout << endl << endl;
|
||||||
|
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
|
||||||
|
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// read scan path
|
||||||
|
if (dir[dir.length()-1] != '/') dir = dir + "/";
|
||||||
|
|
||||||
|
}
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
/////////////NORMALS USING AKNN METHOD ////////////////
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, const double _rPos[3] )
|
||||||
|
{
|
||||||
|
cout<<"Total number of points: "<<points.size()<<endl;
|
||||||
|
int nr_neighbors = k;
|
||||||
|
|
||||||
|
ColumnVector rPos(3);
|
||||||
|
for (int i = 0; i < 3; ++i)
|
||||||
|
rPos(i+1) = _rPos[i];
|
||||||
|
|
||||||
|
ANNpointArray pa = annAllocPts(points.size(), 3);
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
pa[i] = new ANNcoord[3];
|
||||||
|
pa[i][0] = points[i].x;
|
||||||
|
pa[i][1] = points[i].y;
|
||||||
|
pa[i][2] = points[i].z;
|
||||||
|
}
|
||||||
|
ANNkd_tree t(pa, points.size(), 3);
|
||||||
|
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||||
|
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||||
|
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
ANNpoint p = pa[i];
|
||||||
|
//ANN search for k nearest neighbors
|
||||||
|
//indexes of the neighbors along with the query point
|
||||||
|
//stored in the array n
|
||||||
|
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||||
|
Point mean(0.0,0.0,0.0);
|
||||||
|
Matrix X(nr_neighbors,3);
|
||||||
|
SymmetricMatrix A(3);
|
||||||
|
Matrix U(3,3);
|
||||||
|
DiagonalMatrix D(3);
|
||||||
|
//calculate mean for all the points
|
||||||
|
for (int j=0; j<nr_neighbors; ++j)
|
||||||
|
{
|
||||||
|
mean.x += points[nidx[j]].x;
|
||||||
|
mean.y += points[nidx[j]].y;
|
||||||
|
mean.z += points[nidx[j]].z;
|
||||||
|
}
|
||||||
|
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) {
|
||||||
|
X(i+1, 1) = points[nidx[i]].x - mean.x;
|
||||||
|
X(i+1, 2) = points[nidx[i]].y - mean.y;
|
||||||
|
X(i+1, 3) = points[nidx[i]].z - 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();
|
||||||
|
normals.push_back(Point(n(1), n(2), n(3)));
|
||||||
|
}
|
||||||
|
delete[] nidx;
|
||||||
|
delete[] d;
|
||||||
|
}
|
||||||
|
////////////////////////////////////////////////////////////////
|
||||||
|
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
|
||||||
|
////////////////////////////////////////////////////////////////
|
||||||
|
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &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: "<<points.size()<<endl;
|
||||||
|
int nr_neighbors;
|
||||||
|
ANNpointArray pa = annAllocPts(points.size(), 3);
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
pa[i] = new ANNcoord[3];
|
||||||
|
pa[i][0] = points[i].x;
|
||||||
|
pa[i][1] = points[i].y;
|
||||||
|
pa[i][2] = points[i].z;
|
||||||
|
}
|
||||||
|
ANNkd_tree t(pa, points.size(), 3);
|
||||||
|
|
||||||
|
Point mean(0.0,0.0,0.0);
|
||||||
|
double temp_n[3],norm_n = 0.0;
|
||||||
|
double e1,e2,e3;
|
||||||
|
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
Matrix U(3,3);
|
||||||
|
ANNpoint p = pa[i];
|
||||||
|
for(int kidx = kmin; kidx < kmax; kidx++)
|
||||||
|
{
|
||||||
|
nr_neighbors=kidx+1;
|
||||||
|
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||||
|
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||||
|
//ANN search for k nearest neighbors
|
||||||
|
//indexes of the neighbors along with the query point
|
||||||
|
//stored in the array n
|
||||||
|
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||||
|
mean.x=0,mean.y=0,mean.z=0;
|
||||||
|
//calculate mean for all the points
|
||||||
|
for (int j=0; j<nr_neighbors; ++j)
|
||||||
|
{
|
||||||
|
mean.x += points[nidx[j]].x;
|
||||||
|
mean.y += points[nidx[j]].y;
|
||||||
|
mean.z += points[nidx[j]].z;
|
||||||
|
}
|
||||||
|
mean.x /= nr_neighbors;
|
||||||
|
mean.y /= nr_neighbors;
|
||||||
|
mean.z /= nr_neighbors;
|
||||||
|
|
||||||
|
Matrix X(nr_neighbors,3);
|
||||||
|
SymmetricMatrix A(3);
|
||||||
|
DiagonalMatrix D(3);
|
||||||
|
|
||||||
|
//calculate covariance = A for all the points
|
||||||
|
for (int j = 0; j < nr_neighbors; ++j) {
|
||||||
|
X(j+1, 1) = points[nidx[j]].x - mean.x;
|
||||||
|
X(j+1, 2) = points[nidx[j]].y - mean.y;
|
||||||
|
X(j+1, 3) = points[nidx[j]].z - mean.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
A << 1.0/nr_neighbors * X.t() * X;
|
||||||
|
|
||||||
|
EigenValues(A, D, U);
|
||||||
|
|
||||||
|
e1 = D(1);
|
||||||
|
e2 = D(2);
|
||||||
|
e3 = D(3);
|
||||||
|
|
||||||
|
delete[] nidx;
|
||||||
|
delete[] d;
|
||||||
|
|
||||||
|
//We take the particular k if the second maximum eigen value
|
||||||
|
//is at least 25 percent of the maximum eigen value
|
||||||
|
if ((e1 > 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)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
void calculateNormalsPANORAMA(vector<Point> &normals,
|
||||||
|
vector<Point> &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: "<<points.size()<<endl;
|
||||||
|
points.clear();
|
||||||
|
int nr_neighbors = 0;
|
||||||
|
cout << "height of Image: "<<extendedMap.size()<<endl;
|
||||||
|
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
||||||
|
|
||||||
|
// as the nearest neighbors and then the same PCA method as done in AKNN
|
||||||
|
//temporary dynamic array for all the neighbors of a given point
|
||||||
|
vector<cv::Vec3f> neighbors;
|
||||||
|
for (size_t i=0; i< extendedMap.size(); i++)
|
||||||
|
{
|
||||||
|
for (size_t j=0; j<extendedMap[i].size(); j++)
|
||||||
|
{
|
||||||
|
if (extendedMap[i][j].size() == 0) continue;
|
||||||
|
neighbors.clear();
|
||||||
|
Point mean(0.0,0.0,0.0);
|
||||||
|
double temp_n[3],norm_n = 0.0;
|
||||||
|
|
||||||
|
// Offset for neighbor computation
|
||||||
|
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
|
||||||
|
|
||||||
|
// Traversing all the cells in the extended map
|
||||||
|
for (int n = 0; n < 5; ++n) {
|
||||||
|
int x = i + offset[0][n];
|
||||||
|
int y = j + offset[1][n];
|
||||||
|
|
||||||
|
// Copy the neighboring buckets into the vector
|
||||||
|
if (x >= 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<Point> &normals,vector<Point> &points,cv::Mat &img,vector<vector<vector<cv::Vec3f>>> extendedMap)
|
||||||
|
{
|
||||||
|
cout<<"Total number of points: "<<points.size()<<endl;
|
||||||
|
points.clear();
|
||||||
|
int nr_points = 0;
|
||||||
|
//int nr_neighbors = 0,nr_neighbors_center = 0;
|
||||||
|
cout << "height of Image: "<<extendedMap.size()<<endl;
|
||||||
|
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
||||||
|
for (size_t i=0; i< extendedMap.size(); ++i)
|
||||||
|
{
|
||||||
|
for (size_t j=0; j<extendedMap[0].size(); j++)
|
||||||
|
{
|
||||||
|
double theta,phi,rho;
|
||||||
|
double x,y,z;
|
||||||
|
double dRdTheta,dRdPhi;
|
||||||
|
double n[3],m;
|
||||||
|
nr_points = extendedMap[i][j].size();
|
||||||
|
if (nr_points == 0 ) continue;
|
||||||
|
|
||||||
|
for (int k = 0; k< nr_points; k++)
|
||||||
|
{
|
||||||
|
cv::Vec3f p = extendedMap[i][j][k];
|
||||||
|
x = p[0];
|
||||||
|
y = p[1];
|
||||||
|
z = p[2];
|
||||||
|
rho = sqrt(x*x + y*y + z*z);
|
||||||
|
theta = atan(y/x);
|
||||||
|
phi = atan(z/x);
|
||||||
|
|
||||||
|
//Sobel Filter for the derivative
|
||||||
|
dRdTheta = dRdPhi = 0.0;
|
||||||
|
|
||||||
|
if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1)
|
||||||
|
{
|
||||||
|
points.push_back(Point(x, y, z));
|
||||||
|
normals.push_back(Point(0.0,0.0,0.0));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
dRdPhi += 10*img.at<uchar>(i-1,j);
|
||||||
|
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
|
||||||
|
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
|
||||||
|
dRdPhi -= 10*img.at<uchar>(i+1,j);
|
||||||
|
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
|
||||||
|
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
|
||||||
|
|
||||||
|
dRdTheta += 10*img.at<uchar>(i,j-1);
|
||||||
|
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
|
||||||
|
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
|
||||||
|
dRdTheta -= 10*img.at<uchar>(i,j+1);
|
||||||
|
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
|
||||||
|
dRdTheta -= 3 *img.at<uchar>(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_<cv::Vec4f> it;
|
||||||
|
|
||||||
|
it = scan.begin<cv::Vec4f>();
|
||||||
|
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_<float> it = source.begin<float>();
|
||||||
|
it != source.end<float>(); ++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 <cutoff> values are larger than it
|
||||||
|
vector<float> sorted(source.cols*source.rows);
|
||||||
|
int i = 0;
|
||||||
|
for (cv::MatIterator_<float> it = source.begin<float>();
|
||||||
|
it != source.end<float>(); ++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_<float> src = source.begin<float>();
|
||||||
|
cv::MatIterator_<uchar> dst = result.begin<uchar>();
|
||||||
|
cv::MatIterator_<float> end = source.end<float>();
|
||||||
|
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<Point> &points, vector<Point> &normals, int scanNumber)
|
||||||
|
{
|
||||||
|
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
|
||||||
|
ofstream normptsout(ofilename.c_str());
|
||||||
|
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
int r,g,b;
|
||||||
|
r = (int)(normals[i].x * (127.5) + 127.5);
|
||||||
|
g = (int)(normals[i].y * (127.5) + 127.5);
|
||||||
|
b = (int)(fabs(normals[i].z) * (255.0));
|
||||||
|
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
|
||||||
|
}
|
||||||
|
normptsout.clear();
|
||||||
|
normptsout.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// =============================================
|
||||||
|
/// Main
|
||||||
|
/// =============================================
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
int start, end;
|
||||||
|
bool scanserver;
|
||||||
|
int max_dist, min_dist;
|
||||||
|
string dir;
|
||||||
|
IOType iotype;
|
||||||
|
int k1, k2;
|
||||||
|
normal_method ntype;
|
||||||
|
int width, height;
|
||||||
|
|
||||||
|
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
|
||||||
|
dir, iotype, k1, k2, ntype, width, height);
|
||||||
|
|
||||||
|
/// ----------------------------------
|
||||||
|
/// Prepare and read scans
|
||||||
|
/// ----------------------------------
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Make directory for saving the scan segments
|
||||||
|
string normdir = dir + "normals";
|
||||||
|
|
||||||
|
#ifdef _MSC_VER
|
||||||
|
int success = mkdir(normdir.c_str());
|
||||||
|
#else
|
||||||
|
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
|
||||||
|
#endif
|
||||||
|
if(success == 0) {
|
||||||
|
cout << "Writing segments to " << normdir << endl;
|
||||||
|
} else if(errno == EEXIST) {
|
||||||
|
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
|
||||||
|
} else {
|
||||||
|
cerr << "Creating directory " << normdir << " failed" << endl;
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Read the scans
|
||||||
|
Scan::openDirectory(scanserver, dir, iotype, start, end);
|
||||||
|
if(Scan::allScans.size() == 0) {
|
||||||
|
cerr << "No scans found. Did you use the correct format?" << endl;
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat img;
|
||||||
|
|
||||||
|
/// --------------------------------------------
|
||||||
|
/// Initialize and perform segmentation
|
||||||
|
/// --------------------------------------------
|
||||||
|
std::vector<Scan*>::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<Point> points;
|
||||||
|
points.reserve(xyz.size());
|
||||||
|
vector<Point> 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,97 @@
|
||||||
|
/**
|
||||||
|
* @file
|
||||||
|
* @brief Representation of a general search trees
|
||||||
|
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef __SEARCHTREE_H__
|
||||||
|
#define __SEARCHTREE_H__
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
using std::vector;
|
||||||
|
|
||||||
|
#include "ptpair.h"
|
||||||
|
#include "data_types.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief The tree structure
|
||||||
|
*
|
||||||
|
* A tree holds the pointer to the data
|
||||||
|
**/
|
||||||
|
class Tree {
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* Destructor - deletes the tree
|
||||||
|
* pure virtual, i.e., must be implented by a derived class
|
||||||
|
*/
|
||||||
|
virtual inline ~Tree() {};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class Scan;
|
||||||
|
/**
|
||||||
|
* @brief The search tree structure
|
||||||
|
*
|
||||||
|
* A search tree holds the pointer to the data.
|
||||||
|
* Furthermore, search functionality must be privided
|
||||||
|
**/
|
||||||
|
class SearchTree : public Tree {
|
||||||
|
friend class Scan;
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* Constructor (default)
|
||||||
|
*/
|
||||||
|
inline SearchTree() {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor - Constructs a tree from the input.
|
||||||
|
* must be implented by a derived class
|
||||||
|
* @param pts 3D array of points
|
||||||
|
* @param n number of points
|
||||||
|
*/
|
||||||
|
SearchTree(double **pts, int n);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Destructor - deletes the tree
|
||||||
|
* virtual, i.e., must be implented by a derived class
|
||||||
|
*/
|
||||||
|
virtual inline ~SearchTree() {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Aquire a lock on this tree, signaling that its resources are in use. Neccessary for cached data in the scanserver.
|
||||||
|
*/
|
||||||
|
virtual inline void lock() {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Release the lock on this tree, signaling that its resources are aren't in use anymore. Neccessary for cached data in the scanserver.
|
||||||
|
*/
|
||||||
|
virtual inline void unlock() {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This Search function returns a pointer to the closest point
|
||||||
|
* of the query point within maxdist2. If there if no such point
|
||||||
|
* a 0-pointer might be returned.
|
||||||
|
*
|
||||||
|
* @param _p Pointer to query point
|
||||||
|
* @param maxdist2 Maximal distance for closest points
|
||||||
|
* @param threadNum If parallel threads share the search tree the thread num must be given
|
||||||
|
* @return Pointer to closest point
|
||||||
|
*/
|
||||||
|
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0 ;
|
||||||
|
|
||||||
|
virtual void getPtPairs(vector <PtPair> *pairs,
|
||||||
|
double *source_alignxf,
|
||||||
|
double * const *q_points, unsigned int startindex, unsigned int endindex,
|
||||||
|
int thread_num,
|
||||||
|
int rnd, double max_dist_match2, double &sum,
|
||||||
|
double *centroid_m, double *centroid_d);
|
||||||
|
|
||||||
|
virtual void getPtPairs(vector <PtPair> *pairs,
|
||||||
|
double *source_alignxf,
|
||||||
|
const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex,
|
||||||
|
int thread_num,
|
||||||
|
int rnd, double max_dist_match2, double &sum,
|
||||||
|
double *centroid_m, double *centroid_d);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,929 @@
|
||||||
|
/**
|
||||||
|
* @file
|
||||||
|
* @brief Representation of an octree for show
|
||||||
|
* @author Jan Elseberg. Jacobs University Bremen gGmbH, Germany
|
||||||
|
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef SHOWBOCTREE_H
|
||||||
|
#define SHOWBOCTREE_H
|
||||||
|
|
||||||
|
#include "slam6d/Boctree.h"
|
||||||
|
#include "show/colormanager.h"
|
||||||
|
#include "show/scancolormanager.h"
|
||||||
|
#include "show/viewcull.h"
|
||||||
|
#include "show/colordisplay.h"
|
||||||
|
#include "slam6d/scan.h"
|
||||||
|
|
||||||
|
using namespace show;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Octree for show
|
||||||
|
*
|
||||||
|
* A cubic bounding box is calculated
|
||||||
|
* from the given 3D points. Then it
|
||||||
|
* is recusivly subdivided into smaller
|
||||||
|
* subboxes
|
||||||
|
*
|
||||||
|
* It contains software culling functionalities
|
||||||
|
*/
|
||||||
|
template <class T>
|
||||||
|
class Show_BOctTree : public colordisplay
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
BOctTree<T>* m_tree;
|
||||||
|
|
||||||
|
unsigned long maxtargetpoints;
|
||||||
|
|
||||||
|
unsigned int current_lod_mode;
|
||||||
|
|
||||||
|
//! A copy of pointdim of the tree initialized from
|
||||||
|
unsigned int POINTDIM;
|
||||||
|
|
||||||
|
DataOcttree* m_cache_access;
|
||||||
|
Scan* m_scan;
|
||||||
|
|
||||||
|
void init(ScanColorManager* scm) {
|
||||||
|
setColorManager(0);
|
||||||
|
if (scm) {
|
||||||
|
scm->registerTree(this);
|
||||||
|
scm->updateRanges(m_tree->getMins());
|
||||||
|
scm->updateRanges(m_tree->getMaxs());
|
||||||
|
}
|
||||||
|
POINTDIM = m_tree->getPointdim();
|
||||||
|
maxtargetpoints = maxTargetPoints(m_tree->getRoot());
|
||||||
|
current_lod_mode = 0;
|
||||||
|
m_cache_access = 0;
|
||||||
|
m_scan = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
//! Create with tree held in cache, lock indefinitely
|
||||||
|
Show_BOctTree(Scan* scan, DataOcttree* cache_access, ScanColorManager* scm = 0)
|
||||||
|
{
|
||||||
|
m_tree = &cache_access->get();
|
||||||
|
init(scm);
|
||||||
|
// save cache access and hold it until an unlock
|
||||||
|
m_cache_access = cache_access;
|
||||||
|
m_scan = scan;
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Retrieve a cache access object and update the tree pointer
|
||||||
|
void lockCachedTree() {
|
||||||
|
if(m_scan != 0 && m_cache_access == 0) {
|
||||||
|
m_cache_access = new DataOcttree(m_scan->get("octtree"));
|
||||||
|
m_tree = &(m_cache_access->get());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Remove the data access
|
||||||
|
void unlockCachedTree() {
|
||||||
|
if(m_scan != 0 && m_cache_access != 0) {
|
||||||
|
delete m_cache_access; m_cache_access = 0;
|
||||||
|
//m_tree = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Create with already constructed tree and take ownership
|
||||||
|
Show_BOctTree(BOctTree<T>* tree, ScanColorManager* scm = 0) :
|
||||||
|
m_tree(tree)
|
||||||
|
{
|
||||||
|
init(scm);
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Create tree by points
|
||||||
|
template <class P>
|
||||||
|
Show_BOctTree(P * const* pts, int n, T voxelSize, PointType pointtype = PointType(), ScanColorManager *scm = 0)
|
||||||
|
{
|
||||||
|
m_tree = new BOctTree<T>(pts, n, voxelSize, pointtype, true);
|
||||||
|
init(scm);
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Create tree by deserializing from file
|
||||||
|
Show_BOctTree(std::string filename, ScanColorManager *scm = 0)
|
||||||
|
{
|
||||||
|
m_tree = new BOctTree<T>(filename);
|
||||||
|
init(scm);
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual ~Show_BOctTree() {
|
||||||
|
// only delete cache access if created via this method
|
||||||
|
if(m_cache_access) {
|
||||||
|
delete m_cache_access;
|
||||||
|
}
|
||||||
|
// if not constructed by a cache-located tree, delete the owned tree
|
||||||
|
if(!m_scan)
|
||||||
|
delete m_tree;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOctTree<T>* getTree() const { return m_tree; }
|
||||||
|
|
||||||
|
void serialize(const std::string& filename) const { m_tree->serialize(filename); }
|
||||||
|
|
||||||
|
unsigned int getMemorySize() const { return m_tree->getMemorySize(); }
|
||||||
|
|
||||||
|
// virtual functions from colordisplay
|
||||||
|
|
||||||
|
void selectRayBrushSize(set<T *> &points, int brushsize) {
|
||||||
|
selectRayBS(points, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), brushsize);
|
||||||
|
}
|
||||||
|
|
||||||
|
void selectRay(set<T *> &points, int depth = INT_MAX) {
|
||||||
|
selectRay(points, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), depth);
|
||||||
|
}
|
||||||
|
|
||||||
|
void selectRay(T * &point) {
|
||||||
|
selectRay(point, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), FLT_MAX);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cycleLOD() {
|
||||||
|
current_lod_mode = (current_lod_mode+1)%3;
|
||||||
|
}
|
||||||
|
|
||||||
|
void drawLOD(float ratio) {
|
||||||
|
switch (current_lod_mode) {
|
||||||
|
case 0:
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
displayOctTreeCulledLOD(maxtargetpoints * ratio, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize());
|
||||||
|
glEnd();
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
displayOctTreeCulledLOD2(ratio, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize());
|
||||||
|
glEnd();
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
#ifdef WITH_GLEE
|
||||||
|
if (GLEE_ARB_point_parameters) {
|
||||||
|
glPointParameterfARB(GL_POINT_SIZE_MIN_ARB, 1.0);
|
||||||
|
glPointParameterfARB(GL_POINT_SIZE_MAX_ARB, 100000.0);
|
||||||
|
GLfloat p[3] = {0.0, 0.0000, 0.0000005};
|
||||||
|
glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
|
||||||
|
displayOctTreeCPAllCulled(m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(),
|
||||||
|
m_tree->getSize() / pow(2, min( (int)(ratio * m_tree->getMaxDepth()), (int)(m_tree->getMaxDepth() - 3))));
|
||||||
|
p[0] = 1.0;
|
||||||
|
p[2] = 0.0;
|
||||||
|
glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void draw() {
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
displayOctTreeAllCulled(m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize());
|
||||||
|
glEnd();
|
||||||
|
}
|
||||||
|
|
||||||
|
// reroute center call (for recast from colordisplay to show_bocttree)
|
||||||
|
void getCenter(double _center[3]) const {
|
||||||
|
m_tree->getCenter(_center);
|
||||||
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
//! ?
|
||||||
|
unsigned long maxTargetPoints(const bitoct &node) {
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
unsigned long max = 0;
|
||||||
|
|
||||||
|
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
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned long length = points[0].length;
|
||||||
|
if (length > max) max = length;
|
||||||
|
} else { // recurse
|
||||||
|
unsigned long tp = maxTargetPoints(children->node);
|
||||||
|
if (tp > max) max = tp;
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return max*POPCOUNT(node.valid);
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeAll(const bitoct &node) {
|
||||||
|
// T ccenter[3];
|
||||||
|
bitunion<T> *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
|
||||||
|
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++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeAll( children->node);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeAllCulled(const bitoct &node, const T* center, T size ) {
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
if (res == 2) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeAll(node);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
// if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
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++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
//}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeAllCulled( children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeLOD2(float ratio, const bitoct &node, const T* center, T size ) {
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
|
||||||
|
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
|
||||||
|
l = max((int)(l*l*ratio), 0);
|
||||||
|
if (l > 1) {
|
||||||
|
if ((int)length > l ) {
|
||||||
|
T each = (T)POINTDIM * (T)((T)length/(T)l);
|
||||||
|
T *p;
|
||||||
|
int index;
|
||||||
|
for(int iterator = 0; iterator < l; iterator++ ) {
|
||||||
|
index = (T)iterator * each;
|
||||||
|
p = point + index - index%POINTDIM;
|
||||||
|
if(cm) cm->setColor(p);
|
||||||
|
glVertex3f( p[0], p[1], p[2]);
|
||||||
|
}
|
||||||
|
} else if ((int)length <= l) {
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
} /* else if (l == 1) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
}*/
|
||||||
|
} else {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
|
||||||
|
l = max((int)(l*l*ratio), 0);
|
||||||
|
if (l > 0) {
|
||||||
|
displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeCulledLOD2(float ratio, const bitoct &node, const T* center, T size ) {
|
||||||
|
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
if (res == 2) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeLOD2(ratio, node, center, size);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
|
||||||
|
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
|
||||||
|
l = max((int)(l*l*ratio), 0);
|
||||||
|
if (l != 0) {
|
||||||
|
if ((int)length > l ) {
|
||||||
|
T each = (T)POINTDIM * (T)((T)length/(T)l);
|
||||||
|
T *p;
|
||||||
|
int index;
|
||||||
|
for(int iterator = 0; iterator < l; iterator++ ) {
|
||||||
|
index = (T)iterator * each;
|
||||||
|
p = point + index - index%POINTDIM;
|
||||||
|
if(cm) cm->setColor(p);
|
||||||
|
glVertex3f( p[0], p[1], p[2]);
|
||||||
|
}
|
||||||
|
} else if ((int)length <= l) {
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
} else if (l == 1) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
//int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
|
||||||
|
//l = max((int)(l*l*ratio), 0);
|
||||||
|
//if (l > 0) {
|
||||||
|
displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0);
|
||||||
|
//}
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeLOD3(long targetpts, const bitoct &node, const T* center, T size ) {
|
||||||
|
if (targetpts <= 0) return; // no need to display anything
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
|
||||||
|
|
||||||
|
if ( l <= targetpts) { // only a single pixel on screen only paint one point
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
} else {
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeLOD3(targetpts, children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeCulledLOD3(long targetpts, const bitoct &node, const T* center, T size ) {
|
||||||
|
if (targetpts <= 0) return; // no need to display anything
|
||||||
|
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
if (res == 2) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeLOD3(targetpts, node, center, size);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
//l = std::min( max((int)(l*l*ratio), 1), targetpts);
|
||||||
|
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
|
||||||
|
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
|
||||||
|
if (targetpts <= 0 ) cout << l << " " << targetpts << endl;
|
||||||
|
if ( l <= targetpts) { // only a single pixel on screen only paint one point
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
} else {
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeCulledLOD3(targetpts, children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeCulledLOD(long targetpts, const bitoct &node, const T* center, T size ) {
|
||||||
|
if (targetpts <= 0) return; // no need to display anything
|
||||||
|
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
if (res == 2) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeLOD(targetpts, node, center, size);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
unsigned short nc = POPCOUNT(node.valid);
|
||||||
|
long newtargetpts = targetpts;
|
||||||
|
if (nc > 0) {
|
||||||
|
newtargetpts = newtargetpts/nc;
|
||||||
|
if (newtargetpts <= 0 ) return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
|
||||||
|
if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { // only a single pixel on screen only paint one point
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
} else if (length <= newtargetpts) { // more points requested than possible, plot all
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
} else { // select points to show
|
||||||
|
// TODO smarter subselection of points here
|
||||||
|
T each = (T)POINTDIM * (T)((T)length/(T)newtargetpts);
|
||||||
|
T *p;
|
||||||
|
int index;
|
||||||
|
for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
|
||||||
|
index = (T)iterator * each;
|
||||||
|
p = point + index - index%POINTDIM;
|
||||||
|
if(cm) cm->setColor(p);
|
||||||
|
glVertex3f( p[0], p[1], p[2]);
|
||||||
|
//point += each;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeCulledLOD(newtargetpts, children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeLOD(long targetpts, const bitoct &node, const T* center, T size ) {
|
||||||
|
if (targetpts <= 0) return; // no need to display anything
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
unsigned short nc = POPCOUNT(node.valid);
|
||||||
|
long newtargetpts = targetpts;
|
||||||
|
if (nc > 0) {
|
||||||
|
newtargetpts = newtargetpts/nc;
|
||||||
|
if (newtargetpts <= 0 ) return;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { // only a single pixel on screen only paint one point
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
} else if (length <= newtargetpts) { // more points requested than possible, plot all
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
} else { // select points to show
|
||||||
|
// TODO smarter subselection of points here
|
||||||
|
T each = (T)POINTDIM * (T)((T)length/(T)newtargetpts);
|
||||||
|
T *p;
|
||||||
|
int index;
|
||||||
|
for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
|
||||||
|
index = (T)iterator * each;
|
||||||
|
p = point + index - index%POINTDIM;
|
||||||
|
if(cm) cm->setColor(p);
|
||||||
|
glVertex3f( p[0], p[1], p[2]);
|
||||||
|
//point += each;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeLOD(newtargetpts, children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void selectRay(set<T *> &selpoints, const bitoct &node, const T* center, T size, int max_depth, int depth = 0) {
|
||||||
|
if (depth < max_depth && !HitBoundingBox(center, size ))return;
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( !(depth+1 < max_depth) || HitBoundingBox(ccenter, size) ) {
|
||||||
|
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++ ) {
|
||||||
|
selpoints.insert(point);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
selectRay( selpoints, children->node, ccenter, size/2.0, max_depth, depth+1);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void selectRayBS(set<T *> &selpoints, const bitoct &node, const T* center, T size, int brushsize) {
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( HitBoundingBox(ccenter, size) ) {
|
||||||
|
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++ ) {
|
||||||
|
if (ScreenDist(point) < brushsize && RayDist(point) > 100.0)
|
||||||
|
selpoints.insert(point);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
selectRayBS( selpoints, children->node, ccenter, size/2.0, brushsize);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void selectRay(T * &selpoint, const bitoct &node, const T* center, T size, float min) {
|
||||||
|
if (!HitBoundingBox(center, size ))return;
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( HitBoundingBox(ccenter, size) ) {
|
||||||
|
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 dist = RayDist(point);
|
||||||
|
if (min > dist && ScreenDist(point) < 5 && dist > 100.0) {
|
||||||
|
selpoint = point;
|
||||||
|
min = dist;
|
||||||
|
}
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
selectRay( selpoint, children->node, ccenter, size/2.0, min);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeCAllCulled(const bitoct &node, const T* center, T size, T minsize ) {
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
if (res == 2) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeCAll(node, center, size, minsize);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf || minsize > size ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
showCube(ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeCAllCulled( children->node, ccenter, size/2.0, minsize);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeCAll(const bitoct &node, const T* center, T size, T minsize ) {
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf || minsize > size ) { // if ith node is leaf get center
|
||||||
|
showCube(ccenter, size/2.0);
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeCAll( children->node, ccenter, size/2.0, minsize);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void displayOctTreeCPAllCulled(const bitoct &node, const T* center, T size, T minsize ) {
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
if (res == 2) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeCPAll(node, center, size, minsize);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( minsize > size ) {
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
if(cm) {
|
||||||
|
if (( 1 << i ) & node.leaf )
|
||||||
|
cm->setColor( &(children->getPoints()[1]) );
|
||||||
|
else
|
||||||
|
cm->setColor( m_tree->pickPoint(children->node) );
|
||||||
|
}
|
||||||
|
|
||||||
|
glPointSize(size/2.0);
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
glVertex3f( ccenter[0], ccenter[1], ccenter[2] );
|
||||||
|
glEnd();
|
||||||
|
}
|
||||||
|
}else if ( ( 1 << i ) & node.leaf ) {
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
glPointSize(1.0);
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
glEnd();
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeCPAllCulled( children->node, ccenter, size/2.0, minsize);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void displayOctTreeCPAll(const bitoct &node, const T* center, T size, T minsize ) {
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( minsize > size ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
if(cm) {
|
||||||
|
if (( 1 << i ) & node.leaf )
|
||||||
|
cm->setColor( &(children->getPoints()[1]) );
|
||||||
|
else
|
||||||
|
cm->setColor( m_tree->pickPoint(children->node) );
|
||||||
|
}
|
||||||
|
|
||||||
|
glPointSize(size/2.0);
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
glVertex3f( ccenter[0], ccenter[1], ccenter[2] );
|
||||||
|
glEnd();
|
||||||
|
}
|
||||||
|
}else if ( ( 1 << i ) & node.leaf ) {
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
T *point = &(points[1].v); // first point
|
||||||
|
glPointSize(1.0);
|
||||||
|
glBegin(GL_POINTS);
|
||||||
|
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
glEnd();
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeCPAll( children->node, ccenter, size/2.0, minsize);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void showCube(const T* center, T size) {
|
||||||
|
glLineWidth(1.0);
|
||||||
|
glBegin(GL_QUADS); // draw a cube with 6 quads
|
||||||
|
glColor3f(0.0f,1.0f,0.0f); // Set The Color To Green
|
||||||
|
glVertex3f(center[0] + size, center[1] + size, center[2] - size);
|
||||||
|
glVertex3f(center[0] - size, center[1] + size, center[2] - size);
|
||||||
|
glVertex3f(center[0] - size, center[1] + size, center[2] + size);
|
||||||
|
glVertex3f(center[0] + size, center[1] + size, center[2] + size);
|
||||||
|
glColor3f(1.0f,0.5f,0.0f); // Set The Color To Orange
|
||||||
|
|
||||||
|
glVertex3f(center[0] + size, center[1] - size, center[2] + size);
|
||||||
|
glVertex3f(center[0] - size, center[1] - size, center[2] + size);
|
||||||
|
glVertex3f(center[0] - size, center[1] - size, center[2] - size);
|
||||||
|
glVertex3f(center[0] + size, center[1] - size, center[2] - size);
|
||||||
|
|
||||||
|
glColor3f(1.0f,0.0f,0.0f); // Set The Color To Red
|
||||||
|
glVertex3f(center[0] + size, center[1] + size, center[2] + size);
|
||||||
|
glVertex3f(center[0] - size, center[1] + size, center[2] + size);
|
||||||
|
glVertex3f(center[0] - size, center[1] - size, center[2] + size);
|
||||||
|
glVertex3f(center[0] + size, center[1] - size, center[2] + size);
|
||||||
|
|
||||||
|
glColor3f(1.0f,1.0f,0.0f); // Set The Color To Yellow
|
||||||
|
glVertex3f(center[0] + size, center[1] - size, center[2] - size);
|
||||||
|
glVertex3f(center[0] - size, center[1] - size, center[2] - size);
|
||||||
|
glVertex3f(center[0] - size, center[1] + size, center[2] - size);
|
||||||
|
glVertex3f(center[0] + size, center[1] + size, center[2] - size);
|
||||||
|
|
||||||
|
glColor3f(0.0f,0.0f,1.0f); // Set The Color To Blue
|
||||||
|
glVertex3f(center[0] - size, center[1] + size, center[2] + size);
|
||||||
|
glVertex3f(center[0] - size, center[1] + size, center[2] - size);
|
||||||
|
glVertex3f(center[0] - size, center[1] - size, center[2] - size);
|
||||||
|
glVertex3f(center[0] - size, center[1] - size, center[2] + size);
|
||||||
|
|
||||||
|
glColor3f(1.0f,0.0f,1.0f); // Set The Color To Violet
|
||||||
|
glVertex3f(center[0] + size, center[1] + size, center[2] - size);
|
||||||
|
glVertex3f(center[0] + size, center[1] + size, center[2] + size);
|
||||||
|
glVertex3f(center[0] + size, center[1] - size, center[2] + size);
|
||||||
|
glVertex3f(center[0] + size, center[1] - size, center[2] - size);
|
||||||
|
|
||||||
|
glEnd();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void displayOctTreeAllCulled(const bitoct &node, const T* center, T size, float *frustum[6], unsigned char frustsize ) {
|
||||||
|
float *new_frustum[6]; unsigned char counter = 0;
|
||||||
|
for (unsigned char p = 0; p < frustsize; p++ ) {
|
||||||
|
char res = PlaneAABB(center[0], center[1], center[2], size, frustum[p]);
|
||||||
|
if (res == 0) { // cube is on the wrong side of the plane (not visible)
|
||||||
|
return;
|
||||||
|
} else if ( res == 1 ) { // plane intersects this volume continue culling with this plane
|
||||||
|
new_frustum[counter++] = frustum[p];
|
||||||
|
} // other case is simply not to continue culling with the respective plane
|
||||||
|
}
|
||||||
|
if (counter == 0) { // if entirely within frustrum discontinue culling
|
||||||
|
displayOctTreeAll(node);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
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++ ) {
|
||||||
|
if(cm) cm->setColor(point);
|
||||||
|
glVertex3f( point[0], point[1], point[2]);
|
||||||
|
point+=POINTDIM;
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
displayOctTreeAllCulled( children->node, ccenter, size/2.0, new_frustum, counter);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned long int countVisiblePoints(const bitoct &node, const T* center, T size ) {
|
||||||
|
unsigned long int result = 0;
|
||||||
|
int res = CubeInFrustum2(center[0], center[1], center[2], size);
|
||||||
|
if (res==0) return 0; // culled do not continue with this branch of the tree
|
||||||
|
|
||||||
|
T ccenter[3];
|
||||||
|
bitunion<T> *children;
|
||||||
|
bitoct::getChildren(node, children);
|
||||||
|
|
||||||
|
for (short i = 0; i < 8; i++) {
|
||||||
|
if ( ( 1 << i ) & node.valid ) { // if ith node exists
|
||||||
|
BOctTree<T>::childcenter(center, ccenter, size, i); // childrens center
|
||||||
|
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
|
||||||
|
// check if leaf is visible
|
||||||
|
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
|
||||||
|
pointrep *points = children->getPointreps();
|
||||||
|
unsigned int length = points[0].length;
|
||||||
|
result += length;
|
||||||
|
}
|
||||||
|
} else { // recurse
|
||||||
|
result += countVisiblePoints( children->node, ccenter, size/2.0);
|
||||||
|
}
|
||||||
|
++children; // next child
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,7 @@
|
||||||
|
IF(WITH_NORMALS)
|
||||||
|
add_executable(normals normals.cc)
|
||||||
|
|
||||||
|
FIND_PACKAGE(OpenCV REQUIRED)
|
||||||
|
|
||||||
|
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS})
|
||||||
|
ENDIF(WITH_NORMALS)
|
||||||
BIN
.svn/wc.db
BIN
.svn/wc.db
Binary file not shown.
|
|
@ -175,7 +175,18 @@ ELSE(WITH_SEGMENTATION)
|
||||||
MESSAGE(STATUS "Without segmentation")
|
MESSAGE(STATUS "Without segmentation")
|
||||||
ENDIF(WITH_SEGMENTATION)
|
ENDIF(WITH_SEGMENTATION)
|
||||||
|
|
||||||
|
## Normals
|
||||||
|
|
||||||
|
OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF)
|
||||||
|
|
||||||
|
IF(WITH_NORMALS)
|
||||||
|
MESSAGE(STATUS "With normals")
|
||||||
|
ELSE(WITH_NORMALS)
|
||||||
|
MESSAGE(STATUS "Without normals")
|
||||||
|
ENDIF(WITH_NORMALS)
|
||||||
|
|
||||||
## CAD matching
|
## CAD matching
|
||||||
|
|
||||||
OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)
|
OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)
|
||||||
|
|
||||||
IF (WITH_CAD)
|
IF (WITH_CAD)
|
||||||
|
|
@ -380,6 +391,7 @@ add_subdirectory(src/slam6d)
|
||||||
add_subdirectory(src/scanio)
|
add_subdirectory(src/scanio)
|
||||||
add_subdirectory(src/scanserver)
|
add_subdirectory(src/scanserver)
|
||||||
add_subdirectory(src/segmentation)
|
add_subdirectory(src/segmentation)
|
||||||
|
add_subdirectory(src/normals)
|
||||||
add_subdirectory(src/veloslam)
|
add_subdirectory(src/veloslam)
|
||||||
add_subdirectory(src/show)
|
add_subdirectory(src/show)
|
||||||
add_subdirectory(src/grid)
|
add_subdirectory(src/grid)
|
||||||
|
|
|
||||||
|
|
@ -94,7 +94,7 @@ public:
|
||||||
//! Create tree by points
|
//! Create tree by points
|
||||||
template <class P>
|
template <class P>
|
||||||
Show_BOctTree(P * const* pts, int n, T voxelSize, PointType pointtype = PointType(), ScanColorManager *scm = 0)
|
Show_BOctTree(P * const* pts, int n, T voxelSize, PointType pointtype = PointType(), ScanColorManager *scm = 0)
|
||||||
{
|
{
|
||||||
m_tree = new BOctTree<T>(pts, n, voxelSize, pointtype, true);
|
m_tree = new BOctTree<T>(pts, n, voxelSize, pointtype, true);
|
||||||
init(scm);
|
init(scm);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -226,6 +226,9 @@ public:
|
||||||
|
|
||||||
this->POINTDIM = pointtype.getPointDim();
|
this->POINTDIM = pointtype.getPointDim();
|
||||||
|
|
||||||
|
//@@@
|
||||||
|
cout << "POINTDIM" << this->POINTDIM << endl;
|
||||||
|
|
||||||
mins = alloc->allocate<T>(POINTDIM);
|
mins = alloc->allocate<T>(POINTDIM);
|
||||||
maxs = alloc->allocate<T>(POINTDIM);
|
maxs = alloc->allocate<T>(POINTDIM);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,42 +0,0 @@
|
||||||
|
|
||||||
inline void D2Tree::calcDivide(double x_size, double y_size, double z_size,
|
|
||||||
double center[3],
|
|
||||||
double &x_sizeNew, double &y_sizeNew, double &z_sizeNew,
|
|
||||||
double newcenter[8][3])
|
|
||||||
{
|
|
||||||
x_sizeNew = x_size * 0.5;
|
|
||||||
y_sizeNew = y_size * 0.5;
|
|
||||||
z_sizeNew = z_size * 0.5;
|
|
||||||
|
|
||||||
newcenter[0][0] = center[0] - x_sizeNew;
|
|
||||||
newcenter[0][1] = center[1] - y_sizeNew;
|
|
||||||
newcenter[0][2] = center[2] - z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[1][0] = center[0] + x_sizeNew;
|
|
||||||
newcenter[1][1] = center[1] - y_sizeNew;
|
|
||||||
newcenter[1][2] = center[2] - z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[2][0] = center[0] - x_sizeNew;
|
|
||||||
newcenter[2][1] = center[1] + y_sizeNew;
|
|
||||||
newcenter[2][2] = center[2] - z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[3][0] = center[0] - x_sizeNew;
|
|
||||||
newcenter[3][1] = center[1] - y_sizeNew;
|
|
||||||
newcenter[3][2] = center[2] + z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[4][0] = center[0] + x_sizeNew;
|
|
||||||
newcenter[4][1] = center[1] + y_sizeNew;
|
|
||||||
newcenter[4][2] = center[2] - z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[5][0] = center[0] + x_sizeNew;
|
|
||||||
newcenter[5][1] = center[1] - y_sizeNew;
|
|
||||||
newcenter[5][2] = center[2] + z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[6][0] = center[0] - x_sizeNew;
|
|
||||||
newcenter[6][1] = center[1] + y_sizeNew;
|
|
||||||
newcenter[6][2] = center[2] + z_sizeNew;
|
|
||||||
|
|
||||||
newcenter[7][0] = center[0] + x_sizeNew;
|
|
||||||
newcenter[7][1] = center[1] + y_sizeNew;
|
|
||||||
newcenter[7][2] = center[2] + z_sizeNew;
|
|
||||||
}
|
|
||||||
|
|
@ -1,50 +0,0 @@
|
||||||
/**
|
|
||||||
* @file
|
|
||||||
* @brief Representation of the cache for cached k-d tree search.
|
|
||||||
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
||||||
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef __KD_CACHE_H__
|
|
||||||
#define __KD_CACHE_H__
|
|
||||||
|
|
||||||
#include "searchCache.h"
|
|
||||||
#include "kdparams.h"
|
|
||||||
|
|
||||||
// just a prototype
|
|
||||||
class KDtree_cache;
|
|
||||||
class Scan;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief cache item
|
|
||||||
*
|
|
||||||
* The return value of the cached k-d tree
|
|
||||||
* is a bundle of the closest point and
|
|
||||||
* a pointer to the leaf.
|
|
||||||
**/
|
|
||||||
class KDCacheItem : public SearchTreeCacheItem {
|
|
||||||
public:
|
|
||||||
KDCacheItem() { node = 0; };
|
|
||||||
KDParams param;
|
|
||||||
KDtree_cache *node;
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief cache
|
|
||||||
*
|
|
||||||
* The cache consists of an array of KDCacheItems
|
|
||||||
* and two Scan numbers.
|
|
||||||
**/
|
|
||||||
class KDCache {
|
|
||||||
public:
|
|
||||||
// KDCache() { SourceScanNr = TargetScanNr = 0; item = 0; };
|
|
||||||
KDCache() { target = 0; item = 0; };
|
|
||||||
KDCacheItem *item; // array of items
|
|
||||||
// unsigned int SourceScanNr;
|
|
||||||
// unsigned int TargetScanNr;
|
|
||||||
Scan const * target;
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,22 +0,0 @@
|
||||||
/**
|
|
||||||
* @file
|
|
||||||
* @brief Representation of a general cache for search trees
|
|
||||||
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef __SEARCHCACHE_H__
|
|
||||||
#define __SEARCHCACHE_H__
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief The general Cache Item
|
|
||||||
*
|
|
||||||
* This class contains a general cache item
|
|
||||||
*/
|
|
||||||
class SearchTreeCacheItem
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
virtual ~SearchTreeCacheItem() {};
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
@ -10,7 +10,6 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
using std::vector;
|
using std::vector;
|
||||||
|
|
||||||
#include "searchCache.h"
|
|
||||||
#include "ptpair.h"
|
#include "ptpair.h"
|
||||||
#include "data_types.h"
|
#include "data_types.h"
|
||||||
|
|
||||||
|
|
@ -95,61 +94,4 @@ public:
|
||||||
double *centroid_m, double *centroid_d);
|
double *centroid_m, double *centroid_d);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief The search tree structure
|
|
||||||
*
|
|
||||||
* A search tree holds the pointer to the data.
|
|
||||||
* Furthermore, search functionality must be privided
|
|
||||||
**/
|
|
||||||
class CachedSearchTree : public SearchTree {
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* Constructor (default)
|
|
||||||
*/
|
|
||||||
inline CachedSearchTree() {};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructor - Constructs a tree from the input.
|
|
||||||
* pure virtual, i.e., must be implented by a derived class
|
|
||||||
* @param pts 3D array of points
|
|
||||||
* @param n number of points
|
|
||||||
*/
|
|
||||||
CachedSearchTree(double **pts, int n, CachedSearchTree *_parent = 0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Destructor - deletes the tree
|
|
||||||
* pure virtual, i.e., must be implented by a derived class
|
|
||||||
*/
|
|
||||||
virtual inline ~CachedSearchTree() {};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This Search function returns a pointer to the closest point
|
|
||||||
* of the query point within maxdist2. This function searches from the
|
|
||||||
* root to the leafs.
|
|
||||||
*
|
|
||||||
* @param _p Pointer to query point
|
|
||||||
* @param maxdist2 Maximal distance for closest points
|
|
||||||
* @param threadNum If parallel threads share the search tree the thread num must be given
|
|
||||||
* @return A new cach Item
|
|
||||||
*/
|
|
||||||
virtual SearchTreeCacheItem* FindClosestCacheInit(double *_p, double maxdist2, int threadNum = 0) = 0;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This Search function returns a pointer to the closest point
|
|
||||||
* of the query point within maxdist2. This function might be started
|
|
||||||
* from the leafs.
|
|
||||||
*
|
|
||||||
* @param _p Pointer to query point
|
|
||||||
* @param maxdist2 Maximal distance for closest points
|
|
||||||
* @param threadNum If parallel threads share the search tree the thread num must be given
|
|
||||||
* @return A new cach Item
|
|
||||||
*/
|
|
||||||
virtual SearchTreeCacheItem* FindClosestCache(double *_p, double maxdist2, int threadNum = 0) = 0;
|
|
||||||
double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
7
src/normals/CMakeLists.txt
Executable file
7
src/normals/CMakeLists.txt
Executable file
|
|
@ -0,0 +1,7 @@
|
||||||
|
IF(WITH_NORMALS)
|
||||||
|
add_executable(normals normals.cc)
|
||||||
|
|
||||||
|
FIND_PACKAGE(OpenCV REQUIRED)
|
||||||
|
|
||||||
|
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS})
|
||||||
|
ENDIF(WITH_NORMALS)
|
||||||
752
src/normals/normals.cc
Normal file
752
src/normals/normals.cc
Normal file
|
|
@ -0,0 +1,752 @@
|
||||||
|
/**
|
||||||
|
*
|
||||||
|
* Copyright (C) Jacobs University Bremen
|
||||||
|
*
|
||||||
|
* @author Vaibhav Kumar Mehta
|
||||||
|
* @file normals.cc
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <fstream>
|
||||||
|
#include <errno.h>
|
||||||
|
|
||||||
|
#include <boost/program_options.hpp>
|
||||||
|
|
||||||
|
#include <slam6d/io_types.h>
|
||||||
|
#include <slam6d/globals.icc>
|
||||||
|
#include <slam6d/scan.h>
|
||||||
|
#include "slam6d/fbr/panorama.h"
|
||||||
|
#include <scanserver/clientInterface.h>
|
||||||
|
|
||||||
|
#include <ANN/ANN.h>
|
||||||
|
#include "newmat/newmat.h"
|
||||||
|
#include "newmat/newmatap.h"
|
||||||
|
|
||||||
|
using namespace NEWMAT;
|
||||||
|
|
||||||
|
#ifdef _MSC_VER
|
||||||
|
#define strcasecmp _stricmp
|
||||||
|
#define strncasecmp _strnicmp
|
||||||
|
#else
|
||||||
|
#include <strings.h>
|
||||||
|
#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<std::string>& 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<std::string>& 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 <options> where options are (default values in brackets)");
|
||||||
|
cmd_options.add_options()
|
||||||
|
("help,?", "Display this help message")
|
||||||
|
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
|
||||||
|
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
|
||||||
|
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
|
||||||
|
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
|
||||||
|
"using shared library <arg> 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<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
|
||||||
|
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
|
||||||
|
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
|
||||||
|
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
|
||||||
|
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
|
||||||
|
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
|
||||||
|
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
|
||||||
|
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
|
||||||
|
;
|
||||||
|
|
||||||
|
po::options_description hidden("Hidden options");
|
||||||
|
hidden.add_options()
|
||||||
|
("input-dir", po::value<string>(&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/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" <<endl;
|
||||||
|
cout << endl << endl;
|
||||||
|
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
|
||||||
|
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// read scan path
|
||||||
|
if (dir[dir.length()-1] != '/') dir = dir + "/";
|
||||||
|
|
||||||
|
}
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
/////////////NORMALS USING AKNN METHOD ////////////////
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, const double _rPos[3] )
|
||||||
|
{
|
||||||
|
cout<<"Total number of points: "<<points.size()<<endl;
|
||||||
|
int nr_neighbors = k;
|
||||||
|
|
||||||
|
ColumnVector rPos(3);
|
||||||
|
for (int i = 0; i < 3; ++i)
|
||||||
|
rPos(i+1) = _rPos[i];
|
||||||
|
|
||||||
|
ANNpointArray pa = annAllocPts(points.size(), 3);
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
pa[i] = new ANNcoord[3];
|
||||||
|
pa[i][0] = points[i].x;
|
||||||
|
pa[i][1] = points[i].y;
|
||||||
|
pa[i][2] = points[i].z;
|
||||||
|
}
|
||||||
|
ANNkd_tree t(pa, points.size(), 3);
|
||||||
|
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||||
|
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||||
|
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
ANNpoint p = pa[i];
|
||||||
|
//ANN search for k nearest neighbors
|
||||||
|
//indexes of the neighbors along with the query point
|
||||||
|
//stored in the array n
|
||||||
|
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||||
|
Point mean(0.0,0.0,0.0);
|
||||||
|
Matrix X(nr_neighbors,3);
|
||||||
|
SymmetricMatrix A(3);
|
||||||
|
Matrix U(3,3);
|
||||||
|
DiagonalMatrix D(3);
|
||||||
|
//calculate mean for all the points
|
||||||
|
for (int j=0; j<nr_neighbors; ++j)
|
||||||
|
{
|
||||||
|
mean.x += points[nidx[j]].x;
|
||||||
|
mean.y += points[nidx[j]].y;
|
||||||
|
mean.z += points[nidx[j]].z;
|
||||||
|
}
|
||||||
|
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) {
|
||||||
|
X(i+1, 1) = points[nidx[i]].x - mean.x;
|
||||||
|
X(i+1, 2) = points[nidx[i]].y - mean.y;
|
||||||
|
X(i+1, 3) = points[nidx[i]].z - 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();
|
||||||
|
normals.push_back(Point(n(1), n(2), n(3)));
|
||||||
|
}
|
||||||
|
delete[] nidx;
|
||||||
|
delete[] d;
|
||||||
|
}
|
||||||
|
////////////////////////////////////////////////////////////////
|
||||||
|
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
|
||||||
|
////////////////////////////////////////////////////////////////
|
||||||
|
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &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: "<<points.size()<<endl;
|
||||||
|
int nr_neighbors;
|
||||||
|
ANNpointArray pa = annAllocPts(points.size(), 3);
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
pa[i] = new ANNcoord[3];
|
||||||
|
pa[i][0] = points[i].x;
|
||||||
|
pa[i][1] = points[i].y;
|
||||||
|
pa[i][2] = points[i].z;
|
||||||
|
}
|
||||||
|
ANNkd_tree t(pa, points.size(), 3);
|
||||||
|
|
||||||
|
Point mean(0.0,0.0,0.0);
|
||||||
|
double temp_n[3],norm_n = 0.0;
|
||||||
|
double e1,e2,e3;
|
||||||
|
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
Matrix U(3,3);
|
||||||
|
ANNpoint p = pa[i];
|
||||||
|
for(int kidx = kmin; kidx < kmax; kidx++)
|
||||||
|
{
|
||||||
|
nr_neighbors=kidx+1;
|
||||||
|
ANNidxArray nidx = new ANNidx[nr_neighbors];
|
||||||
|
ANNdistArray d = new ANNdist[nr_neighbors];
|
||||||
|
//ANN search for k nearest neighbors
|
||||||
|
//indexes of the neighbors along with the query point
|
||||||
|
//stored in the array n
|
||||||
|
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
|
||||||
|
mean.x=0,mean.y=0,mean.z=0;
|
||||||
|
//calculate mean for all the points
|
||||||
|
for (int j=0; j<nr_neighbors; ++j)
|
||||||
|
{
|
||||||
|
mean.x += points[nidx[j]].x;
|
||||||
|
mean.y += points[nidx[j]].y;
|
||||||
|
mean.z += points[nidx[j]].z;
|
||||||
|
}
|
||||||
|
mean.x /= nr_neighbors;
|
||||||
|
mean.y /= nr_neighbors;
|
||||||
|
mean.z /= nr_neighbors;
|
||||||
|
|
||||||
|
Matrix X(nr_neighbors,3);
|
||||||
|
SymmetricMatrix A(3);
|
||||||
|
DiagonalMatrix D(3);
|
||||||
|
|
||||||
|
//calculate covariance = A for all the points
|
||||||
|
for (int j = 0; j < nr_neighbors; ++j) {
|
||||||
|
X(j+1, 1) = points[nidx[j]].x - mean.x;
|
||||||
|
X(j+1, 2) = points[nidx[j]].y - mean.y;
|
||||||
|
X(j+1, 3) = points[nidx[j]].z - mean.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
A << 1.0/nr_neighbors * X.t() * X;
|
||||||
|
|
||||||
|
EigenValues(A, D, U);
|
||||||
|
|
||||||
|
e1 = D(1);
|
||||||
|
e2 = D(2);
|
||||||
|
e3 = D(3);
|
||||||
|
|
||||||
|
delete[] nidx;
|
||||||
|
delete[] d;
|
||||||
|
|
||||||
|
//We take the particular k if the second maximum eigen value
|
||||||
|
//is at least 25 percent of the maximum eigen value
|
||||||
|
if ((e1 > 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)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
|
||||||
|
///////////////////////////////////////////////////////
|
||||||
|
void calculateNormalsPANORAMA(vector<Point> &normals,
|
||||||
|
vector<Point> &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: "<<points.size()<<endl;
|
||||||
|
points.clear();
|
||||||
|
int nr_neighbors = 0;
|
||||||
|
cout << "height of Image: "<<extendedMap.size()<<endl;
|
||||||
|
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
||||||
|
|
||||||
|
// as the nearest neighbors and then the same PCA method as done in AKNN
|
||||||
|
//temporary dynamic array for all the neighbors of a given point
|
||||||
|
vector<cv::Vec3f> neighbors;
|
||||||
|
for (size_t i=0; i< extendedMap.size(); i++)
|
||||||
|
{
|
||||||
|
for (size_t j=0; j<extendedMap[i].size(); j++)
|
||||||
|
{
|
||||||
|
if (extendedMap[i][j].size() == 0) continue;
|
||||||
|
neighbors.clear();
|
||||||
|
Point mean(0.0,0.0,0.0);
|
||||||
|
double temp_n[3],norm_n = 0.0;
|
||||||
|
|
||||||
|
// Offset for neighbor computation
|
||||||
|
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
|
||||||
|
|
||||||
|
// Traversing all the cells in the extended map
|
||||||
|
for (int n = 0; n < 5; ++n) {
|
||||||
|
int x = i + offset[0][n];
|
||||||
|
int y = j + offset[1][n];
|
||||||
|
|
||||||
|
// Copy the neighboring buckets into the vector
|
||||||
|
if (x >= 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<Point> &normals,vector<Point> &points,cv::Mat &img,vector<vector<vector<cv::Vec3f>>> extendedMap)
|
||||||
|
{
|
||||||
|
cout<<"Total number of points: "<<points.size()<<endl;
|
||||||
|
points.clear();
|
||||||
|
int nr_points = 0;
|
||||||
|
//int nr_neighbors = 0,nr_neighbors_center = 0;
|
||||||
|
cout << "height of Image: "<<extendedMap.size()<<endl;
|
||||||
|
cout << "width of Image: "<<extendedMap[0].size()<<endl;
|
||||||
|
for (size_t i=0; i< extendedMap.size(); ++i)
|
||||||
|
{
|
||||||
|
for (size_t j=0; j<extendedMap[0].size(); j++)
|
||||||
|
{
|
||||||
|
double theta,phi,rho;
|
||||||
|
double x,y,z;
|
||||||
|
double dRdTheta,dRdPhi;
|
||||||
|
double n[3],m;
|
||||||
|
nr_points = extendedMap[i][j].size();
|
||||||
|
if (nr_points == 0 ) continue;
|
||||||
|
|
||||||
|
for (int k = 0; k< nr_points; k++)
|
||||||
|
{
|
||||||
|
cv::Vec3f p = extendedMap[i][j][k];
|
||||||
|
x = p[0];
|
||||||
|
y = p[1];
|
||||||
|
z = p[2];
|
||||||
|
rho = sqrt(x*x + y*y + z*z);
|
||||||
|
theta = atan(y/x);
|
||||||
|
phi = atan(z/x);
|
||||||
|
|
||||||
|
//Sobel Filter for the derivative
|
||||||
|
dRdTheta = dRdPhi = 0.0;
|
||||||
|
|
||||||
|
if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1)
|
||||||
|
{
|
||||||
|
points.push_back(Point(x, y, z));
|
||||||
|
normals.push_back(Point(0.0,0.0,0.0));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
dRdPhi += 10*img.at<uchar>(i-1,j);
|
||||||
|
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
|
||||||
|
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
|
||||||
|
dRdPhi -= 10*img.at<uchar>(i+1,j);
|
||||||
|
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
|
||||||
|
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
|
||||||
|
|
||||||
|
dRdTheta += 10*img.at<uchar>(i,j-1);
|
||||||
|
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
|
||||||
|
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
|
||||||
|
dRdTheta -= 10*img.at<uchar>(i,j+1);
|
||||||
|
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
|
||||||
|
dRdTheta -= 3 *img.at<uchar>(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_<cv::Vec4f> it;
|
||||||
|
|
||||||
|
it = scan.begin<cv::Vec4f>();
|
||||||
|
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_<float> it = source.begin<float>();
|
||||||
|
it != source.end<float>(); ++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 <cutoff> values are larger than it
|
||||||
|
vector<float> sorted(source.cols*source.rows);
|
||||||
|
int i = 0;
|
||||||
|
for (cv::MatIterator_<float> it = source.begin<float>();
|
||||||
|
it != source.end<float>(); ++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_<float> src = source.begin<float>();
|
||||||
|
cv::MatIterator_<uchar> dst = result.begin<uchar>();
|
||||||
|
cv::MatIterator_<float> end = source.end<float>();
|
||||||
|
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<Point> &points, vector<Point> &normals, int scanNumber)
|
||||||
|
{
|
||||||
|
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
|
||||||
|
ofstream normptsout(ofilename.c_str());
|
||||||
|
|
||||||
|
for (size_t i=0; i<points.size(); ++i)
|
||||||
|
{
|
||||||
|
int r,g,b;
|
||||||
|
r = (int)(normals[i].x * (127.5) + 127.5);
|
||||||
|
g = (int)(normals[i].y * (127.5) + 127.5);
|
||||||
|
b = (int)(fabs(normals[i].z) * (255.0));
|
||||||
|
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
|
||||||
|
}
|
||||||
|
normptsout.clear();
|
||||||
|
normptsout.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// =============================================
|
||||||
|
/// Main
|
||||||
|
/// =============================================
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
int start, end;
|
||||||
|
bool scanserver;
|
||||||
|
int max_dist, min_dist;
|
||||||
|
string dir;
|
||||||
|
IOType iotype;
|
||||||
|
int k1, k2;
|
||||||
|
normal_method ntype;
|
||||||
|
int width, height;
|
||||||
|
|
||||||
|
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
|
||||||
|
dir, iotype, k1, k2, ntype, width, height);
|
||||||
|
|
||||||
|
/// ----------------------------------
|
||||||
|
/// Prepare and read scans
|
||||||
|
/// ----------------------------------
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Make directory for saving the scan segments
|
||||||
|
string normdir = dir + "normals";
|
||||||
|
|
||||||
|
#ifdef _MSC_VER
|
||||||
|
int success = mkdir(normdir.c_str());
|
||||||
|
#else
|
||||||
|
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
|
||||||
|
#endif
|
||||||
|
if(success == 0) {
|
||||||
|
cout << "Writing segments to " << normdir << endl;
|
||||||
|
} else if(errno == EEXIST) {
|
||||||
|
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
|
||||||
|
} else {
|
||||||
|
cerr << "Creating directory " << normdir << " failed" << endl;
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Read the scans
|
||||||
|
Scan::openDirectory(scanserver, dir, iotype, start, end);
|
||||||
|
if(Scan::allScans.size() == 0) {
|
||||||
|
cerr << "No scans found. Did you use the correct format?" << endl;
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat img;
|
||||||
|
|
||||||
|
/// --------------------------------------------
|
||||||
|
/// Initialize and perform segmentation
|
||||||
|
/// --------------------------------------------
|
||||||
|
std::vector<Scan*>::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<Point> points;
|
||||||
|
points.reserve(xyz.size());
|
||||||
|
vector<Point> 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -1043,6 +1043,7 @@ void initShow(int argc, char **argv){
|
||||||
#if !defined USE_COMPACT_TREE
|
#if !defined USE_COMPACT_TREE
|
||||||
// show structures
|
// show structures
|
||||||
// associate show octtree with the scan and hand over octtree pointer ownership
|
// associate show octtree with the scan and hand over octtree pointer ownership
|
||||||
|
|
||||||
Show_BOctTree<sfloat>* tree = new Show_BOctTree<sfloat>(scan, data_oct, cm);
|
Show_BOctTree<sfloat>* tree = new Show_BOctTree<sfloat>(scan, data_oct, cm);
|
||||||
|
|
||||||
// unlock cached octtree to enable creation of more octtres without blocking the space for full scan points
|
// unlock cached octtree to enable creation of more octtres without blocking the space for full scan points
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue