update to svn rev 735

This commit is contained in:
josch 2012-10-29 15:03:12 +01:00
parent 5255e4f9f0
commit 29567b8de4
18 changed files with 5729 additions and 173 deletions

File diff suppressed because it is too large Load diff

File diff suppressed because it is too large Load diff

View file

@ -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)

View file

@ -0,0 +1,752 @@
/**
*
* Copyright (C) Jacobs University Bremen
*
* @author Vaibhav Kumar Mehta
* @file normals.cc
*/
#include <iostream>
#include <string>
#include <fstream>
#include <errno.h>
#include <boost/program_options.hpp>
#include <slam6d/io_types.h>
#include <slam6d/globals.icc>
#include <slam6d/scan.h>
#include "slam6d/fbr/panorama.h"
#include <scanserver/clientInterface.h>
#include <ANN/ANN.h>
#include "newmat/newmat.h"
#include "newmat/newmatap.h"
using namespace NEWMAT;
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
namespace po = boost::program_options;
using namespace std;
enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST};
/*
* validates normal calculation method specification
*/
void validate(boost::any& v, const std::vector<std::string>& values,
normal_method*, int) {
if (values.size() == 0)
throw std::runtime_error("Invalid model specification");
string arg = values.at(0);
if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN;
else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN;
else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA;
else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST;
else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown"));
}
/// validate IO types
void validate(boost::any& v, const std::vector<std::string>& values,
IOType*, int) {
if (values.size() == 0)
throw std::runtime_error("Invalid model specification");
string arg = values.at(0);
try {
v = formatname_to_io_type(arg.c_str());
} catch (...) { // runtime_error
throw std::runtime_error("Format " + arg + " unknown.");
}
}
/// Parse commandline options
void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir,
IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height)
{
/// ----------------------------------
/// set up program commandline options
/// ----------------------------------
po::options_description cmd_options("Usage: calculateNormals <options> where options are (default values in brackets)");
cmd_options.add_options()
("help,?", "Display this help message")
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
"using shared library <arg> for input. (chose format from [uos|uosr|uos_map|"
"uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|"
"riegl_txt|riegl_rgb|riegl_bin|zahn|ply])")
("max,M", po::value<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
;
po::options_description hidden("Hidden options");
hidden.add_options()
("input-dir", po::value<string>(&dir), "input dir");
po::positional_options_description pd;
pd.add("input-dir", 1);
po::options_description all;
all.add(cmd_options).add(hidden);
po::variables_map vmap;
po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap);
po::notify(vmap);
if (vmap.count("help")) {
cout << cmd_options << endl << endl;
cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl;
cout << " bin/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" <<endl;
cout << endl << endl;
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
exit(-1);
}
// read scan path
if (dir[dir.length()-1] != '/') dir = dir + "/";
}
///////////////////////////////////////////////////////
/////////////NORMALS USING AKNN METHOD ////////////////
///////////////////////////////////////////////////////
void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, const double _rPos[3] )
{
cout<<"Total number of points: "<<points.size()<<endl;
int nr_neighbors = k;
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i)
{
pa[i] = new ANNcoord[3];
pa[i][0] = points[i].x;
pa[i][1] = points[i].y;
pa[i][2] = points[i].z;
}
ANNkd_tree t(pa, points.size(), 3);
ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors];
for (size_t i=0; i<points.size(); ++i)
{
ANNpoint p = pa[i];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
Point mean(0.0,0.0,0.0);
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
X(i+1, 1) = points[nidx[i]].x - mean.x;
X(i+1, 2) = points[nidx[i]].y - mean.y;
X(i+1, 3) = points[nidx[i]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
delete[] nidx;
delete[] d;
}
////////////////////////////////////////////////////////////////
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
////////////////////////////////////////////////////////////////
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
int kmin, int kmax, const double _rPos[3])
{
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
cout<<"Total number of points: "<<points.size()<<endl;
int nr_neighbors;
ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i)
{
pa[i] = new ANNcoord[3];
pa[i][0] = points[i].x;
pa[i][1] = points[i].y;
pa[i][2] = points[i].z;
}
ANNkd_tree t(pa, points.size(), 3);
Point mean(0.0,0.0,0.0);
double temp_n[3],norm_n = 0.0;
double e1,e2,e3;
for (size_t i=0; i<points.size(); ++i)
{
Matrix U(3,3);
ANNpoint p = pa[i];
for(int kidx = kmin; kidx < kmax; kidx++)
{
nr_neighbors=kidx+1;
ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
mean.x=0,mean.y=0,mean.z=0;
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
DiagonalMatrix D(3);
//calculate covariance = A for all the points
for (int j = 0; j < nr_neighbors; ++j) {
X(j+1, 1) = points[nidx[j]].x - mean.x;
X(j+1, 2) = points[nidx[j]].y - mean.y;
X(j+1, 3) = points[nidx[j]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
e1 = D(1);
e2 = D(2);
e3 = D(3);
delete[] nidx;
delete[] d;
//We take the particular k if the second maximum eigen value
//is at least 25 percent of the maximum eigen value
if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25))
break;
}
//normal = eigenvector corresponding to lowest
//eigen value that is the 1rd column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
}
///////////////////////////////////////////////////////
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
///////////////////////////////////////////////////////
void calculateNormalsPANORAMA(vector<Point> &normals,
vector<Point> &points,
vector< vector< vector< cv::Vec3f > > > extendedMap,
const double _rPos[3])
{
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
cout<<"Total number of points: "<<points.size()<<endl;
points.clear();
int nr_neighbors = 0;
cout << "height of Image: "<<extendedMap.size()<<endl;
cout << "width of Image: "<<extendedMap[0].size()<<endl;
// as the nearest neighbors and then the same PCA method as done in AKNN
//temporary dynamic array for all the neighbors of a given point
vector<cv::Vec3f> neighbors;
for (size_t i=0; i< extendedMap.size(); i++)
{
for (size_t j=0; j<extendedMap[i].size(); j++)
{
if (extendedMap[i][j].size() == 0) continue;
neighbors.clear();
Point mean(0.0,0.0,0.0);
double temp_n[3],norm_n = 0.0;
// Offset for neighbor computation
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
// Traversing all the cells in the extended map
for (int n = 0; n < 5; ++n) {
int x = i + offset[0][n];
int y = j + offset[1][n];
// Copy the neighboring buckets into the vector
if (x >= 0 && x < (int)extendedMap.size() &&
y >= 0 && y < (int)extendedMap[x].size()) {
for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) {
neighbors.push_back(extendedMap[x][y][k]);
}
}
}
nr_neighbors = neighbors.size();
cv::Vec3f p = extendedMap[i][j][0];
//if no neighbor point is found in the 4-neighboring pixels then normal is set to zero
if (nr_neighbors < 3)
{
points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(0.0,0.0,0.0));
continue;
}
//calculate mean for all the points
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for(int k = 0; k < nr_neighbors; k++)
{
cv::Vec3f pp = neighbors[k];
mean.x += pp[0];
mean.y += pp[1];
mean.z += pp[2];
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
cv::Vec3f pp = neighbors[i];
X(i+1, 1) = pp[0] - mean.x;
X(i+1, 2) = pp[1] - mean.y;
X(i+1, 3) = pp[2] - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) {
cv::Vec3f p = extendedMap[i][j][k];
points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(n(1), n(2), n(3)));
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE //////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
/*
void calculateNormalsFAST(vector<Point> &normals,vector<Point> &points,cv::Mat &img,vector<vector<vector<cv::Vec3f>>> extendedMap)
{
cout<<"Total number of points: "<<points.size()<<endl;
points.clear();
int nr_points = 0;
//int nr_neighbors = 0,nr_neighbors_center = 0;
cout << "height of Image: "<<extendedMap.size()<<endl;
cout << "width of Image: "<<extendedMap[0].size()<<endl;
for (size_t i=0; i< extendedMap.size(); ++i)
{
for (size_t j=0; j<extendedMap[0].size(); j++)
{
double theta,phi,rho;
double x,y,z;
double dRdTheta,dRdPhi;
double n[3],m;
nr_points = extendedMap[i][j].size();
if (nr_points == 0 ) continue;
for (int k = 0; k< nr_points; k++)
{
cv::Vec3f p = extendedMap[i][j][k];
x = p[0];
y = p[1];
z = p[2];
rho = sqrt(x*x + y*y + z*z);
theta = atan(y/x);
phi = atan(z/x);
//Sobel Filter for the derivative
dRdTheta = dRdPhi = 0.0;
if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1)
{
points.push_back(Point(x, y, z));
normals.push_back(Point(0.0,0.0,0.0));
continue;
}
dRdPhi += 10*img.at<uchar>(i-1,j);
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
dRdPhi -= 10*img.at<uchar>(i+1,j);
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
dRdTheta += 10*img.at<uchar>(i,j-1);
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
dRdTheta -= 10*img.at<uchar>(i,j+1);
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
dRdTheta -= 3 *img.at<uchar>(i+1,j+1);
n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) +
cos(theta) * cos(phi) * dRdPhi / rho;
n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) +
sin(theta) * cos(phi) * dRdPhi / rho;
n[2] = cos(phi) - sin(phi) * dRdPhi / rho;
//n[2] = -n[2];
m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
n[0] /= m; n[1] /= m; n[2] /= m;
points.push_back(Point(x, y, z));
normals.push_back(Point(n[0],n[1],n[2]));
}
}
}
}
*/
/*
* retrieve a cv::Mat with x,y,z,r from a scan object
* functionality borrowed from scan_cv::convertScanToMat but this function
* does not allow a scanserver to be used, prints to stdout and can only
* handle a single scan
*/
cv::Mat scan2mat(Scan *source)
{
DataXYZ xyz = source->get("xyz");
DataReflectance xyz_reflectance = source->get("reflectance");
unsigned int nPoints = xyz.size();
cv::Mat scan(nPoints,1,CV_32FC(4));
scan = cv::Scalar::all(0);
cv::MatIterator_<cv::Vec4f> it;
it = scan.begin<cv::Vec4f>();
for(unsigned int i = 0; i < nPoints; i++){
float x, y, z, reflectance;
x = xyz[i][0];
y = xyz[i][1];
z = xyz[i][2];
if(xyz_reflectance.size() != 0)
{
reflectance = xyz_reflectance[i];
//normalize the reflectance
reflectance += 32;
reflectance /= 64;
reflectance -= 0.2;
reflectance /= 0.3;
if (reflectance < 0) reflectance = 0;
if (reflectance > 1) reflectance = 1;
}
(*it)[0] = x;
(*it)[1] = y;
(*it)[2] = z;
if(xyz_reflectance.size() != 0)
(*it)[3] = reflectance;
else
(*it)[3] = 0;
++it;
}
return scan;
}
/*
* convert a matrix of float values (range image) to a matrix of unsigned
* eight bit characters using different techniques
*/
cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff)
{
cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0));
float max = 0;
// find maximum value
if (cutoff == 0.0) {
// without cutoff, just iterate through all values to find the largest
for (cv::MatIterator_<float> it = source.begin<float>();
it != source.end<float>(); ++it) {
float val = *it;
if (val > max) {
max = val;
}
}
} else {
// when a cutoff is specified, sort all the points by value and then
// specify the max so that <cutoff> values are larger than it
vector<float> sorted(source.cols*source.rows);
int i = 0;
for (cv::MatIterator_<float> it = source.begin<float>();
it != source.end<float>(); ++it, ++i) {
sorted[i] = *it;
}
std::sort(sorted.begin(), sorted.end());
max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))];
cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl;
}
cv::MatIterator_<float> src = source.begin<float>();
cv::MatIterator_<uchar> dst = result.begin<uchar>();
cv::MatIterator_<float> end = source.end<float>();
if (logarithm) {
// stretch values from 0 to max logarithmically over 0 to 255
// using the logarithm allows to represent smaller values with more
// precision and larger values with less
max = log(max+1);
for (; src != end; ++src, ++dst) {
float val = (log(*src+1)*255.0)/max;
if (val > 255)
*dst = 255;
else
*dst = (uchar)val;
}
} else {
// stretch values from 0 to max linearly over 0 to 255
for (; src != end; ++src, ++dst) {
float val = (*src*255.0)/max;
if (val > 255)
*dst = 255;
else
*dst = (uchar)val;
}
}
return result;
}
/// Write a pose file with the specofied name
void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber)
{
string poseFileName = dir + "/scan" + to_string(scanNumber, 3) + ".pose";
ofstream posout(poseFileName.c_str());
posout << rPos[0] << " "
<< rPos[1] << " "
<< rPos[2] << endl
<< deg(rPosTheta[0]) << " "
<< deg(rPosTheta[1]) << " "
<< deg(rPosTheta[2]) << endl;
posout.clear();
posout.close();
}
/// write scan files for all segments
void writeScanFiles(string dir, vector<Point> &points, vector<Point> &normals, int scanNumber)
{
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
ofstream normptsout(ofilename.c_str());
for (size_t i=0; i<points.size(); ++i)
{
int r,g,b;
r = (int)(normals[i].x * (127.5) + 127.5);
g = (int)(normals[i].y * (127.5) + 127.5);
b = (int)(fabs(normals[i].z) * (255.0));
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
}
normptsout.clear();
normptsout.close();
}
/// =============================================
/// Main
/// =============================================
int main(int argc, char** argv)
{
int start, end;
bool scanserver;
int max_dist, min_dist;
string dir;
IOType iotype;
int k1, k2;
normal_method ntype;
int width, height;
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
dir, iotype, k1, k2, ntype, width, height);
/// ----------------------------------
/// Prepare and read scans
/// ----------------------------------
if (scanserver) {
try {
ClientInterface::create();
} catch(std::runtime_error& e) {
cerr << "ClientInterface could not be created: " << e.what() << endl;
cerr << "Start the scanserver first." << endl;
exit(-1);
}
}
/// Make directory for saving the scan segments
string normdir = dir + "normals";
#ifdef _MSC_VER
int success = mkdir(normdir.c_str());
#else
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing segments to " << normdir << endl;
} else if(errno == EEXIST) {
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
} else {
cerr << "Creating directory " << normdir << " failed" << endl;
exit(1);
}
/// Read the scans
Scan::openDirectory(scanserver, dir, iotype, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
cv::Mat img;
/// --------------------------------------------
/// Initialize and perform segmentation
/// --------------------------------------------
std::vector<Scan*>::iterator it = Scan::allScans.begin();
int scanNumber = 0;
for( ; it != Scan::allScans.end(); ++it) {
Scan* scan = *it;
// apply optional filtering
scan->setRangeFilter(max_dist, min_dist);
const double* rPos = scan->get_rPos();
const double* rPosTheta = scan->get_rPosTheta();
/// read scan into points
DataXYZ xyz(scan->get("xyz"));
vector<Point> points;
points.reserve(xyz.size());
vector<Point> normals;
normals.reserve(xyz.size());
for(unsigned int j = 0; j < xyz.size(); j++) {
points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2]));
}
if(ntype == AKNN)
calculateNormalsAKNN(normals,points, k1, rPos);
else if(ntype == ADAPTIVE_AKNN)
calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos);
else
{
// create panorama
fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED);
fPanorama.createPanorama(scan2mat(scan));
// the range image has to be converted from float to uchar
img = fPanorama.getRangeImage();
img = float2uchar(img, 0, 0.0);
if(ntype == PANORAMA)
calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos);
else if(ntype == PANORAMA_FAST)
cout << "PANORAMA_FAST is not working yet" << endl;
// calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap());
}
// pose file (repeated for the number of segments
writePoseFiles(normdir, rPos, rPosTheta, scanNumber);
// scan files for all segments
writeScanFiles(normdir, points,normals,scanNumber);
scanNumber++;
}
// shutdown everything
if (scanserver)
ClientInterface::destroy();
else
Scan::closeDirectory();
cout << "Normal program end" << endl;
return 0;
}

View file

@ -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

View file

@ -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

View file

@ -0,0 +1,7 @@
IF(WITH_NORMALS)
add_executable(normals normals.cc)
FIND_PACKAGE(OpenCV REQUIRED)
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS})
ENDIF(WITH_NORMALS)

Binary file not shown.

View file

@ -175,7 +175,18 @@ ELSE(WITH_SEGMENTATION)
MESSAGE(STATUS "Without segmentation") MESSAGE(STATUS "Without segmentation")
ENDIF(WITH_SEGMENTATION) ENDIF(WITH_SEGMENTATION)
## Normals
OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF)
IF(WITH_NORMALS)
MESSAGE(STATUS "With normals")
ELSE(WITH_NORMALS)
MESSAGE(STATUS "Without normals")
ENDIF(WITH_NORMALS)
## CAD matching ## CAD matching
OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF) OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)
IF (WITH_CAD) IF (WITH_CAD)
@ -380,6 +391,7 @@ add_subdirectory(src/slam6d)
add_subdirectory(src/scanio) add_subdirectory(src/scanio)
add_subdirectory(src/scanserver) add_subdirectory(src/scanserver)
add_subdirectory(src/segmentation) add_subdirectory(src/segmentation)
add_subdirectory(src/normals)
add_subdirectory(src/veloslam) add_subdirectory(src/veloslam)
add_subdirectory(src/show) add_subdirectory(src/show)
add_subdirectory(src/grid) add_subdirectory(src/grid)

View file

@ -226,6 +226,9 @@ public:
this->POINTDIM = pointtype.getPointDim(); this->POINTDIM = pointtype.getPointDim();
//@@@
cout << "POINTDIM" << this->POINTDIM << endl;
mins = alloc->allocate<T>(POINTDIM); mins = alloc->allocate<T>(POINTDIM);
maxs = alloc->allocate<T>(POINTDIM); maxs = alloc->allocate<T>(POINTDIM);

View file

@ -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;
}

View file

@ -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

View file

@ -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

View file

@ -10,7 +10,6 @@
#include <vector> #include <vector>
using std::vector; using std::vector;
#include "searchCache.h"
#include "ptpair.h" #include "ptpair.h"
#include "data_types.h" #include "data_types.h"
@ -95,61 +94,4 @@ public:
double *centroid_m, double *centroid_d); double *centroid_m, double *centroid_d);
}; };
/**
* @brief The search tree structure
*
* A search tree holds the pointer to the data.
* Furthermore, search functionality must be privided
**/
class CachedSearchTree : public SearchTree {
public:
/**
* Constructor (default)
*/
inline CachedSearchTree() {};
/**
* Constructor - Constructs a tree from the input.
* pure virtual, i.e., must be implented by a derived class
* @param pts 3D array of points
* @param n number of points
*/
CachedSearchTree(double **pts, int n, CachedSearchTree *_parent = 0);
/**
* Destructor - deletes the tree
* pure virtual, i.e., must be implented by a derived class
*/
virtual inline ~CachedSearchTree() {};
/**
* This Search function returns a pointer to the closest point
* of the query point within maxdist2. This function searches from the
* root to the leafs.
*
* @param _p Pointer to query point
* @param maxdist2 Maximal distance for closest points
* @param threadNum If parallel threads share the search tree the thread num must be given
* @return A new cach Item
*/
virtual SearchTreeCacheItem* FindClosestCacheInit(double *_p, double maxdist2, int threadNum = 0) = 0;
/**
* This Search function returns a pointer to the closest point
* of the query point within maxdist2. This function might be started
* from the leafs.
*
* @param _p Pointer to query point
* @param maxdist2 Maximal distance for closest points
* @param threadNum If parallel threads share the search tree the thread num must be given
* @return A new cach Item
*/
virtual SearchTreeCacheItem* FindClosestCache(double *_p, double maxdist2, int threadNum = 0) = 0;
double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const {
return 0;
}
};
#endif #endif

7
src/normals/CMakeLists.txt Executable file
View file

@ -0,0 +1,7 @@
IF(WITH_NORMALS)
add_executable(normals normals.cc)
FIND_PACKAGE(OpenCV REQUIRED)
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS})
ENDIF(WITH_NORMALS)

752
src/normals/normals.cc Normal file
View file

@ -0,0 +1,752 @@
/**
*
* Copyright (C) Jacobs University Bremen
*
* @author Vaibhav Kumar Mehta
* @file normals.cc
*/
#include <iostream>
#include <string>
#include <fstream>
#include <errno.h>
#include <boost/program_options.hpp>
#include <slam6d/io_types.h>
#include <slam6d/globals.icc>
#include <slam6d/scan.h>
#include "slam6d/fbr/panorama.h"
#include <scanserver/clientInterface.h>
#include <ANN/ANN.h>
#include "newmat/newmat.h"
#include "newmat/newmatap.h"
using namespace NEWMAT;
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
namespace po = boost::program_options;
using namespace std;
enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST};
/*
* validates normal calculation method specification
*/
void validate(boost::any& v, const std::vector<std::string>& values,
normal_method*, int) {
if (values.size() == 0)
throw std::runtime_error("Invalid model specification");
string arg = values.at(0);
if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN;
else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN;
else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA;
else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST;
else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown"));
}
/// validate IO types
void validate(boost::any& v, const std::vector<std::string>& values,
IOType*, int) {
if (values.size() == 0)
throw std::runtime_error("Invalid model specification");
string arg = values.at(0);
try {
v = formatname_to_io_type(arg.c_str());
} catch (...) { // runtime_error
throw std::runtime_error("Format " + arg + " unknown.");
}
}
/// Parse commandline options
void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir,
IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height)
{
/// ----------------------------------
/// set up program commandline options
/// ----------------------------------
po::options_description cmd_options("Usage: calculateNormals <options> where options are (default values in brackets)");
cmd_options.add_options()
("help,?", "Display this help message")
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
"using shared library <arg> for input. (chose format from [uos|uosr|uos_map|"
"uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|"
"riegl_txt|riegl_rgb|riegl_bin|zahn|ply])")
("max,M", po::value<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
;
po::options_description hidden("Hidden options");
hidden.add_options()
("input-dir", po::value<string>(&dir), "input dir");
po::positional_options_description pd;
pd.add("input-dir", 1);
po::options_description all;
all.add(cmd_options).add(hidden);
po::variables_map vmap;
po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap);
po::notify(vmap);
if (vmap.count("help")) {
cout << cmd_options << endl << endl;
cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl;
cout << " bin/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" <<endl;
cout << endl << endl;
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
exit(-1);
}
// read scan path
if (dir[dir.length()-1] != '/') dir = dir + "/";
}
///////////////////////////////////////////////////////
/////////////NORMALS USING AKNN METHOD ////////////////
///////////////////////////////////////////////////////
void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, const double _rPos[3] )
{
cout<<"Total number of points: "<<points.size()<<endl;
int nr_neighbors = k;
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i)
{
pa[i] = new ANNcoord[3];
pa[i][0] = points[i].x;
pa[i][1] = points[i].y;
pa[i][2] = points[i].z;
}
ANNkd_tree t(pa, points.size(), 3);
ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors];
for (size_t i=0; i<points.size(); ++i)
{
ANNpoint p = pa[i];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
Point mean(0.0,0.0,0.0);
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
X(i+1, 1) = points[nidx[i]].x - mean.x;
X(i+1, 2) = points[nidx[i]].y - mean.y;
X(i+1, 3) = points[nidx[i]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
delete[] nidx;
delete[] d;
}
////////////////////////////////////////////////////////////////
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
////////////////////////////////////////////////////////////////
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
int kmin, int kmax, const double _rPos[3])
{
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
cout<<"Total number of points: "<<points.size()<<endl;
int nr_neighbors;
ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i)
{
pa[i] = new ANNcoord[3];
pa[i][0] = points[i].x;
pa[i][1] = points[i].y;
pa[i][2] = points[i].z;
}
ANNkd_tree t(pa, points.size(), 3);
Point mean(0.0,0.0,0.0);
double temp_n[3],norm_n = 0.0;
double e1,e2,e3;
for (size_t i=0; i<points.size(); ++i)
{
Matrix U(3,3);
ANNpoint p = pa[i];
for(int kidx = kmin; kidx < kmax; kidx++)
{
nr_neighbors=kidx+1;
ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
mean.x=0,mean.y=0,mean.z=0;
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
DiagonalMatrix D(3);
//calculate covariance = A for all the points
for (int j = 0; j < nr_neighbors; ++j) {
X(j+1, 1) = points[nidx[j]].x - mean.x;
X(j+1, 2) = points[nidx[j]].y - mean.y;
X(j+1, 3) = points[nidx[j]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
e1 = D(1);
e2 = D(2);
e3 = D(3);
delete[] nidx;
delete[] d;
//We take the particular k if the second maximum eigen value
//is at least 25 percent of the maximum eigen value
if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25))
break;
}
//normal = eigenvector corresponding to lowest
//eigen value that is the 1rd column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
}
///////////////////////////////////////////////////////
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
///////////////////////////////////////////////////////
void calculateNormalsPANORAMA(vector<Point> &normals,
vector<Point> &points,
vector< vector< vector< cv::Vec3f > > > extendedMap,
const double _rPos[3])
{
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
cout<<"Total number of points: "<<points.size()<<endl;
points.clear();
int nr_neighbors = 0;
cout << "height of Image: "<<extendedMap.size()<<endl;
cout << "width of Image: "<<extendedMap[0].size()<<endl;
// as the nearest neighbors and then the same PCA method as done in AKNN
//temporary dynamic array for all the neighbors of a given point
vector<cv::Vec3f> neighbors;
for (size_t i=0; i< extendedMap.size(); i++)
{
for (size_t j=0; j<extendedMap[i].size(); j++)
{
if (extendedMap[i][j].size() == 0) continue;
neighbors.clear();
Point mean(0.0,0.0,0.0);
double temp_n[3],norm_n = 0.0;
// Offset for neighbor computation
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
// Traversing all the cells in the extended map
for (int n = 0; n < 5; ++n) {
int x = i + offset[0][n];
int y = j + offset[1][n];
// Copy the neighboring buckets into the vector
if (x >= 0 && x < (int)extendedMap.size() &&
y >= 0 && y < (int)extendedMap[x].size()) {
for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) {
neighbors.push_back(extendedMap[x][y][k]);
}
}
}
nr_neighbors = neighbors.size();
cv::Vec3f p = extendedMap[i][j][0];
//if no neighbor point is found in the 4-neighboring pixels then normal is set to zero
if (nr_neighbors < 3)
{
points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(0.0,0.0,0.0));
continue;
}
//calculate mean for all the points
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for(int k = 0; k < nr_neighbors; k++)
{
cv::Vec3f pp = neighbors[k];
mean.x += pp[0];
mean.y += pp[1];
mean.z += pp[2];
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
cv::Vec3f pp = neighbors[i];
X(i+1, 1) = pp[0] - mean.x;
X(i+1, 2) = pp[1] - mean.y;
X(i+1, 3) = pp[2] - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) {
cv::Vec3f p = extendedMap[i][j][k];
points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(n(1), n(2), n(3)));
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE //////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
/*
void calculateNormalsFAST(vector<Point> &normals,vector<Point> &points,cv::Mat &img,vector<vector<vector<cv::Vec3f>>> extendedMap)
{
cout<<"Total number of points: "<<points.size()<<endl;
points.clear();
int nr_points = 0;
//int nr_neighbors = 0,nr_neighbors_center = 0;
cout << "height of Image: "<<extendedMap.size()<<endl;
cout << "width of Image: "<<extendedMap[0].size()<<endl;
for (size_t i=0; i< extendedMap.size(); ++i)
{
for (size_t j=0; j<extendedMap[0].size(); j++)
{
double theta,phi,rho;
double x,y,z;
double dRdTheta,dRdPhi;
double n[3],m;
nr_points = extendedMap[i][j].size();
if (nr_points == 0 ) continue;
for (int k = 0; k< nr_points; k++)
{
cv::Vec3f p = extendedMap[i][j][k];
x = p[0];
y = p[1];
z = p[2];
rho = sqrt(x*x + y*y + z*z);
theta = atan(y/x);
phi = atan(z/x);
//Sobel Filter for the derivative
dRdTheta = dRdPhi = 0.0;
if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1)
{
points.push_back(Point(x, y, z));
normals.push_back(Point(0.0,0.0,0.0));
continue;
}
dRdPhi += 10*img.at<uchar>(i-1,j);
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
dRdPhi -= 10*img.at<uchar>(i+1,j);
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
dRdTheta += 10*img.at<uchar>(i,j-1);
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
dRdTheta -= 10*img.at<uchar>(i,j+1);
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
dRdTheta -= 3 *img.at<uchar>(i+1,j+1);
n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) +
cos(theta) * cos(phi) * dRdPhi / rho;
n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) +
sin(theta) * cos(phi) * dRdPhi / rho;
n[2] = cos(phi) - sin(phi) * dRdPhi / rho;
//n[2] = -n[2];
m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
n[0] /= m; n[1] /= m; n[2] /= m;
points.push_back(Point(x, y, z));
normals.push_back(Point(n[0],n[1],n[2]));
}
}
}
}
*/
/*
* retrieve a cv::Mat with x,y,z,r from a scan object
* functionality borrowed from scan_cv::convertScanToMat but this function
* does not allow a scanserver to be used, prints to stdout and can only
* handle a single scan
*/
cv::Mat scan2mat(Scan *source)
{
DataXYZ xyz = source->get("xyz");
DataReflectance xyz_reflectance = source->get("reflectance");
unsigned int nPoints = xyz.size();
cv::Mat scan(nPoints,1,CV_32FC(4));
scan = cv::Scalar::all(0);
cv::MatIterator_<cv::Vec4f> it;
it = scan.begin<cv::Vec4f>();
for(unsigned int i = 0; i < nPoints; i++){
float x, y, z, reflectance;
x = xyz[i][0];
y = xyz[i][1];
z = xyz[i][2];
if(xyz_reflectance.size() != 0)
{
reflectance = xyz_reflectance[i];
//normalize the reflectance
reflectance += 32;
reflectance /= 64;
reflectance -= 0.2;
reflectance /= 0.3;
if (reflectance < 0) reflectance = 0;
if (reflectance > 1) reflectance = 1;
}
(*it)[0] = x;
(*it)[1] = y;
(*it)[2] = z;
if(xyz_reflectance.size() != 0)
(*it)[3] = reflectance;
else
(*it)[3] = 0;
++it;
}
return scan;
}
/*
* convert a matrix of float values (range image) to a matrix of unsigned
* eight bit characters using different techniques
*/
cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff)
{
cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0));
float max = 0;
// find maximum value
if (cutoff == 0.0) {
// without cutoff, just iterate through all values to find the largest
for (cv::MatIterator_<float> it = source.begin<float>();
it != source.end<float>(); ++it) {
float val = *it;
if (val > max) {
max = val;
}
}
} else {
// when a cutoff is specified, sort all the points by value and then
// specify the max so that <cutoff> values are larger than it
vector<float> sorted(source.cols*source.rows);
int i = 0;
for (cv::MatIterator_<float> it = source.begin<float>();
it != source.end<float>(); ++it, ++i) {
sorted[i] = *it;
}
std::sort(sorted.begin(), sorted.end());
max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))];
cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl;
}
cv::MatIterator_<float> src = source.begin<float>();
cv::MatIterator_<uchar> dst = result.begin<uchar>();
cv::MatIterator_<float> end = source.end<float>();
if (logarithm) {
// stretch values from 0 to max logarithmically over 0 to 255
// using the logarithm allows to represent smaller values with more
// precision and larger values with less
max = log(max+1);
for (; src != end; ++src, ++dst) {
float val = (log(*src+1)*255.0)/max;
if (val > 255)
*dst = 255;
else
*dst = (uchar)val;
}
} else {
// stretch values from 0 to max linearly over 0 to 255
for (; src != end; ++src, ++dst) {
float val = (*src*255.0)/max;
if (val > 255)
*dst = 255;
else
*dst = (uchar)val;
}
}
return result;
}
/// Write a pose file with the specofied name
void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber)
{
string poseFileName = dir + "/scan" + to_string(scanNumber, 3) + ".pose";
ofstream posout(poseFileName.c_str());
posout << rPos[0] << " "
<< rPos[1] << " "
<< rPos[2] << endl
<< deg(rPosTheta[0]) << " "
<< deg(rPosTheta[1]) << " "
<< deg(rPosTheta[2]) << endl;
posout.clear();
posout.close();
}
/// write scan files for all segments
void writeScanFiles(string dir, vector<Point> &points, vector<Point> &normals, int scanNumber)
{
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
ofstream normptsout(ofilename.c_str());
for (size_t i=0; i<points.size(); ++i)
{
int r,g,b;
r = (int)(normals[i].x * (127.5) + 127.5);
g = (int)(normals[i].y * (127.5) + 127.5);
b = (int)(fabs(normals[i].z) * (255.0));
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
}
normptsout.clear();
normptsout.close();
}
/// =============================================
/// Main
/// =============================================
int main(int argc, char** argv)
{
int start, end;
bool scanserver;
int max_dist, min_dist;
string dir;
IOType iotype;
int k1, k2;
normal_method ntype;
int width, height;
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
dir, iotype, k1, k2, ntype, width, height);
/// ----------------------------------
/// Prepare and read scans
/// ----------------------------------
if (scanserver) {
try {
ClientInterface::create();
} catch(std::runtime_error& e) {
cerr << "ClientInterface could not be created: " << e.what() << endl;
cerr << "Start the scanserver first." << endl;
exit(-1);
}
}
/// Make directory for saving the scan segments
string normdir = dir + "normals";
#ifdef _MSC_VER
int success = mkdir(normdir.c_str());
#else
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing segments to " << normdir << endl;
} else if(errno == EEXIST) {
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
} else {
cerr << "Creating directory " << normdir << " failed" << endl;
exit(1);
}
/// Read the scans
Scan::openDirectory(scanserver, dir, iotype, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
cv::Mat img;
/// --------------------------------------------
/// Initialize and perform segmentation
/// --------------------------------------------
std::vector<Scan*>::iterator it = Scan::allScans.begin();
int scanNumber = 0;
for( ; it != Scan::allScans.end(); ++it) {
Scan* scan = *it;
// apply optional filtering
scan->setRangeFilter(max_dist, min_dist);
const double* rPos = scan->get_rPos();
const double* rPosTheta = scan->get_rPosTheta();
/// read scan into points
DataXYZ xyz(scan->get("xyz"));
vector<Point> points;
points.reserve(xyz.size());
vector<Point> normals;
normals.reserve(xyz.size());
for(unsigned int j = 0; j < xyz.size(); j++) {
points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2]));
}
if(ntype == AKNN)
calculateNormalsAKNN(normals,points, k1, rPos);
else if(ntype == ADAPTIVE_AKNN)
calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos);
else
{
// create panorama
fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED);
fPanorama.createPanorama(scan2mat(scan));
// the range image has to be converted from float to uchar
img = fPanorama.getRangeImage();
img = float2uchar(img, 0, 0.0);
if(ntype == PANORAMA)
calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos);
else if(ntype == PANORAMA_FAST)
cout << "PANORAMA_FAST is not working yet" << endl;
// calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap());
}
// pose file (repeated for the number of segments
writePoseFiles(normdir, rPos, rPosTheta, scanNumber);
// scan files for all segments
writeScanFiles(normdir, points,normals,scanNumber);
scanNumber++;
}
// shutdown everything
if (scanserver)
ClientInterface::destroy();
else
Scan::closeDirectory();
cout << "Normal program end" << endl;
return 0;
}

View file

@ -1043,6 +1043,7 @@ void initShow(int argc, char **argv){
#if !defined USE_COMPACT_TREE #if !defined USE_COMPACT_TREE
// show structures // show structures
// associate show octtree with the scan and hand over octtree pointer ownership // associate show octtree with the scan and hand over octtree pointer ownership
Show_BOctTree<sfloat>* tree = new Show_BOctTree<sfloat>(scan, data_oct, cm); Show_BOctTree<sfloat>* tree = new Show_BOctTree<sfloat>(scan, data_oct, cm);
// unlock cached octtree to enable creation of more octtres without blocking the space for full scan points // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points