update to svn rev 735
josch
12 years ago
parent
5255e4f9f0
commit
29567b8de4
File diff suppressed because it is too large
Load Diff
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)
|
||||
Binary file not shown.
@ -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
|
||||
@ -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)
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue