From 83b943ab2bfa14e52ceb20de8f83b12c1e07f747 Mon Sep 17 00:00:00 2001 From: josch Date: Sun, 16 Sep 2012 15:31:02 +0200 Subject: [PATCH] updated svn to 707 --- ...3b3f6526bc7ea6fc90124dc28d38d6566.svn-base | 17 +- ...062e17505dfdc3387c1b4df76dd0e77ae.svn-base | 54 + ...04e48c6a9261fd4cebccb26b573d1a518.svn-base | 130 ++ ...d1a6b6d98176edab38d89aecc832953a9.svn-base | 1101 +++++++++++++++++ ...7a9fa22fdab710646b8a518c83adb8055.svn-base | 29 + ...cb55373715bd55e56bcc6cf64da64e535.svn-base | 845 +++++++++++++ ...ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base | 4 +- ...c7b6de027098b23bd3cb5250702e83988.svn-base | 171 +++ ...55c6dae4125e756d976951b6c7a7e8ed7.svn-base | 413 +++++++ ...630a796c01e97af1666e1308120e13844.svn-base | 53 + .svn/wc.db | Bin 604160 -> 611328 bytes README | 8 +- include/scanio/scan_io_uosr.h | 29 + include/slam6d/io_types.h | 4 +- src/scanio/CMakeLists.txt | 2 +- src/scanio/scan_io_uosr.cc | 171 +++ src/scanserver/CMakeLists.txt | 6 +- src/show/show_common.cc | 4 +- src/show/viewcull.cc | 6 + src/slam6d/fbr/feature_based_registration.cc | 16 +- src/slam6d/io_types.cc | 4 + 21 files changed, 3043 insertions(+), 24 deletions(-) rename src/scanserver/io_types.cc => .svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base (94%) create mode 100644 .svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base create mode 100644 .svn/pristine/60/607ddec04e48c6a9261fd4cebccb26b573d1a518.svn-base create mode 100644 .svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base create mode 100644 .svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base create mode 100644 .svn/pristine/97/97c2b2ecb55373715bd55e56bcc6cf64da64e535.svn-base rename include/scanserver/io_types.h => .svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base (60%) create mode 100644 .svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base create mode 100644 .svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base create mode 100644 .svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base create mode 100644 include/scanio/scan_io_uosr.h create mode 100644 src/scanio/scan_io_uosr.cc diff --git a/src/scanserver/io_types.cc b/.svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base similarity index 94% rename from src/scanserver/io_types.cc rename to .svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base index fcabf17..03f6817 100644 --- a/src/scanserver/io_types.cc +++ b/.svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base @@ -1,13 +1,16 @@ /* - * io_types implementation + * io_tpyes implementation * - * Copyright (C) Thomas Escher, Kai Lingemann + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter * * Released under the GPL version 3. * */ -#include "scanserver/io_types.h" + +#include "slam6d/io_types.h" + +#include "slam6d/globals.icc" #ifdef _MSC_VER //#include // TODO: TEST @@ -19,12 +22,10 @@ #include #include - -#include "slam6d/globals.icc" - IOType formatname_to_io_type(const char * string) { if (strcasecmp(string, "uos") == 0) return UOS; + else if (strcasecmp(string, "uosr") == 0) return UOSR; else if (strcasecmp(string, "uos_map") == 0) return UOS_MAP; else if (strcasecmp(string, "uos_frames") == 0) return UOS_FRAMES; else if (strcasecmp(string, "uos_map_frames") == 0) return UOS_MAP_FRAMES; @@ -69,6 +70,8 @@ const char * io_type_to_libname(IOType type) switch (type) { case UOS: return "scan_io_uos"; + case UOSR: + return "scan_io_uosr"; case UOS_MAP: return "scan_io_uos_map"; case UOS_FRAMES: @@ -140,7 +143,7 @@ const char * io_type_to_libname(IOType type) case VELODYNE: return "scan_io_velodyne"; case VELODYNE_FRAMES: - return "velodyne_frames"; + return "scan_io_velodyne_frames"; default: throw std::runtime_error(std::string("Io type ") + to_string(type) + std::string(" could not be matched to a library name")); } diff --git a/.svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base b/.svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base new file mode 100644 index 0000000..6a897c4 --- /dev/null +++ b/.svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base @@ -0,0 +1,54 @@ +if(WIN32) + add_library(pointfilter STATIC ../slam6d/pointfilter.cc) +else(WIN32) + add_library(pointfilter SHARED ../slam6d/pointfilter.cc) +endif(WIN32) + +set(SCANIO_LIBNAMES + uos uosr uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne +) + +if(WITH_RIVLIB) + set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} rxp) + if(LIBXML2_FOUND) + include_directories(${LIBXML2_INCLUDE_DIR}) +# set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} riegl_project) +# target_link_libraries(scan_io_riegl_project ${RIVLIB} scan_io_rxp ${LIBXML2_LIBRARIES}) + endif(LIBXML2_FOUND) +endif(WITH_RIVLIB) + +#IF (WITH_CAD) +# IF(NOT WIN32) +# add_library(scan_io_cad SHARED scan_io_cad.cc) +# target_link_libraries(scan_io_cad ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) +# ENDIF(NOT WIN32) +#ENDIF (WITH_CAD) + +foreach(libname ${SCANIO_LIBNAMES}) +if(WIN32) + #add_library(scan_io_${libname} STATIC scan_io_${libname}.cc) + add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) +else(WIN32) + add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) +endif(WIN32) + target_link_libraries(scan_io_${libname} pointfilter ${Boost_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) +endforeach(libname) + +if(WITH_RIVLIB) + target_link_libraries(scan_io_rxp ${RIVLIB}) + if(LIBXML2_FOUND) + target_link_libraries(scan_io_rxp ${LIBXML2_LIBRARIES}) #scan_io_riegl_project ${RIVLIB}) + endif(LIBXML2_FOUND) +endif(WITH_RIVLIB) + +if(WIN32) + add_library(scanio STATIC scan_io.cc ../slam6d/io_types.cc) +else(WIN32) + add_library(scanio SHARED scan_io.cc ../slam6d/io_types.cc) +endif(WIN32) + +if(UNIX) + target_link_libraries(scanio dl) +endif(UNIX) + + diff --git a/.svn/pristine/60/607ddec04e48c6a9261fd4cebccb26b573d1a518.svn-base b/.svn/pristine/60/607ddec04e48c6a9261fd4cebccb26b573d1a518.svn-base new file mode 100644 index 0000000..69f732c --- /dev/null +++ b/.svn/pristine/60/607ddec04e48c6a9261fd4cebccb26b573d1a518.svn-base @@ -0,0 +1,130 @@ +------------------------------------------------------------------- + +To compile the project simply call "make". This will configure slam6d +using the default settings. If you wish to configure the project using +custom settings do: "make config". This command requires ccmake be +installed on your system. Alternatively you may change into the build +directory ".build" and configure the project with your preferred cmake +configurator, i.e.: + +cd .build && cmake -i ../ + +For Microsoft Windows, use the cmake-gui application provided by cmake +to configure and generate project files for the appropriate version of +Microsoft Visual Studio C++ of your system. Use the INSTALL target to +built the entire project. Executables (and .dll's) will then reside +in the "windows" folder. For running the binaries you need to install +the proper redistributable package. + +Some Boost libraries (graph, regex, serialization, filesystem, +interprocess) are needed to compile the slam6D program. + +If you are using Debian just do: + + aptitude install libboost-graph-dev libboost-regex-dev libboost-serialization-dev freeglut3-dev libxmu-dev libxi-dev + +or, if you are still using Debian stable (lenny): + + aptitude install libboost-graph1.35-dev libboost-regex1.35-dev libboost-serialization1.35-dev freeglut3-dev libxmu-dev libxi-dev + +for Ubuntu this would be: + + sudo apt-get install libboost-all-dev libcv-dev freeglut3-dev libxmu-dev libxi-dev + +SuSE users please use yast2 for installing the missing packages + +Additionally you need some external tools (exemplarily for Ubuntu): + + sudo apt-get install imagemagick ffmpeg libx264-120 + +------------------------------------------------------------------- + +For a detailed explanation of the programm, its usage, etc., please +refer to the high level documentation doc/documentation_HL.pdf +(esp. sections 4-6, for starters). + +IMPORTANT: +Take care to register scans first (bin/slam6D) before trying to +display them (bin/show), and think about using the point reduction +(see section 6) for a much faster scan matching result. Extremely +large scans might need to be reduced (using bin/scan_red) before +registration. This will write reduced scans in the uos format into a +directory "reduced" in the data directory. + +Three example scans are included in the dat directory, several +larger data sets can be downloaded from the data repository, +http://kos.informatik.uni-osnabrueck.de/3Dscans/ +(Alternatively, click on the "Data Repository" link on this project's +web site on Sourceforge, http://slam6d.sourceforge.net/) + +EXAMPLES: +(using the data set in the slam6d repository) +bin/slam6D -m 500 -R 5 -d 25.0 --metascan dat +bin/show dat + +(using the data set in the slam6d repository) +bin/slam6D --max=500 -r 10.2 -i 20 --metascan dat +bin/show dat + +(using hannover1.tgz from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -s 1 -e 65 -r 10 -i 100 -d 75 -D 250 --epsICP=0.00001 + -I 50 --cldist=750 -L 0 -G 1 /home/nuechter/dat/dat_hannover1 +bin/show -s 1 -e 65 /home/nuechter/dat/dat_hannover1 + +(using hannover2.tgz from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -q -r 10 -f rts -s 23 -d 75 -L 4 --cldist=1500 -G 1 -D -1 + --DlastSLAM 250 --graphDist 200 -I 50 hannover2 +bin/show -f rts -s 23 hannover2 + +(using kvarntorp_mine.tgz (dat_mine1) form http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -s 1 -e 76 -r 10 -m 3000 -d 50 -i 1000 --epsICP=0.000001 + -I 50 -D 75 --clpairs=5000 -f old /home/nuechter/dat/dat_mine1/ +bin/show -s 1 -e 76 -m 3000 -f old /home/nuechter/dat/dat_mine1/ + +(using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/scan_red -s 0 -e 12 -r 10 /home/nuechter/dat/bremen_city +bin/slam6D -a 2 -q /home/nuechter/dat/bremen_city/reduced -f uos -d 150 + -s 0 -e 12 --anim=1 -n /home/nuechter/dat/bremen_city/bremen.net + -G 1 -D 100 -i 0 -I 50 -p --epsSLAM=0.0 +bin/show -s 0 -e 12 /home/nuechter/dat/bremen_city/reduced + +(using UniKoblenz_CampusTour3_OsnabrueckFormat.tar.gz from +http://kos.informatik.uni-osnabrueck.de/3Dscans/) +bin/slam6D -s 1 -e 320 -r 20 -i 300 --epsICP=0.000001 -d 45 -D 45 + -f uos --algo=2 -l 10 -L 4 -I 100 -G 1 + /home/nuechter/dat/UniKoblenz_CampusTour3_OsnabrueckFormat/ +bin/show -s 1 -e 320 -f uos /home/nuechter/dat/UniKoblenz_CampusTour3_OsnabrueckFormat/ +------------------------------------------------------------------- + +For detecting planes compile with the WITH_PLANE option. +Adapt the settings in bin/hough.cfg for your data set. + +EXAMPLE: (using the data set in the slam6d repository, no modification +of bin/hough.cfg necessary) + +bin/planes -s 0 dat +bin/show -s 0 -e 0 dat -l dat/planes/planes.list + +(using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) +adapt these settings in bin/hough.cfg: +RhoNum 500 +RhoMax 5000 +MaxPointPlaneDist 50.0 +MinPlaneSize 50 +PointDist 100.0 +/bin/planes -f riegl_txt -s 0 /home/nuechter/dat/bremen_city/ -r 50 -O 1 -m 5000 +/bin/show -s 0 -e 0 /home/nuechter/dat/bremen_city/ -f riegl_txt -l dat/planes/planes.list -r 10 -O 1 -m 5000 + +------------------------------------------------------------------- +The IO relevant parameters -f(ormat), -s(tart), -e(nd) can be omitted +in slam6D and show if a 'format' file exists in the directory, which +contains key=value lines (spaces are trimmed automatically) for +format, start, end with the same values as in the commandline. These +format-file parameters will be overwritten by commandline parameters +so that the format-file will provide the right IO type and full index +range and the user can overwrite the index range as he sees fit. + +------------------------------------------------------------------- +A reference manual can be found in doc/refman.pdf resp. +doc/html/index.html (type in 'make docu' to compile the doxygen +documentation for the HTML files). diff --git a/.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base b/.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base new file mode 100644 index 0000000..775fa92 --- /dev/null +++ b/.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base @@ -0,0 +1,1101 @@ +/* + * show_common implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#ifdef WITH_GLEE +#include +#endif + +#include "show/show.h" +#include "show/show_Boctree.h" +#include "show/compacttree.h" +#include "show/NurbsPath.h" +#include "show/vertexarray.h" +#ifndef DYNAMIC_OBJECT_REMOVAL +#include "slam6d/scan.h" +#include "slam6d/managedScan.h" +#else +#include "veloslam/veloscan.h" +#endif +#include "glui/glui.h" /* Header File For The glui functions */ +#include +using std::ifstream; +#include +using std::exception; +#include + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif + +#ifdef _MSC_VER +#include "XGetopt.h" +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#include "slam6d/point_type.h" +#include "slam6d/io_utils.h" +#include "show/display.h" + +/** + * This vector contains the pointer to a vertex array for + * all colors (inner vector) and all scans (outer vector) + */ +vector< vector > vvertexArrayList; + +vector< ::SDisplay*> displays; +/** + * the octrees that store the points for each scan + */ +//Show_BOctTree **octpts; +vector octpts; +/** + * Storing the base directory + */ +string scan_dir; + +/** + * Storing the ID of the main windows + */ +int window_id; + +/** + * Size of points + */ +GLfloat pointsize = 1.0; +int anim_delay = 5; + +/** + * Select Color Buffer + */ +GLenum buffermode = GL_BACK; + +/** + * Indicator whether and how the drawing window + * has to be updated. + * + * haveToUpdate == 1 redisplay + * haveToUpdate == 2 reshape + * haveToUpdate == 3 animation scan matching + * haveToUpdate == 4 stop animation scan matching + * haveToUpdate == 6 path animation + * haveToUpdate == 7 force redisplay with all points + */ +int haveToUpdate = 0; + +/** + * Flag for invert the scene + */ +bool invert = true; + +/** + * Flag for indicating brid eyes view + */ +bool showTopView = false; + +/** + * Flag for idicating camera add mode + */ +bool addCameraView = false; //Is the view in add box mode? + +/** + * Storing the apex angle of the camera + */ +GLfloat cangle = 60.0; // Current camera opening mode +GLfloat cangle_old = cangle; + +/** + * Current rotation axis of the scene as quaternion + */ +GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; +GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; + +/** + * Current translation of the scene + */ +GLdouble X = 0.0, Y = 0.0, Z = 0.0; +GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; + +/** + * parallel zoom (similar to apex angle) for parallel projection + */ +GLfloat pzoom = 2000.0; +GLfloat pzoom_old = pzoom; + + +/** + * Mode of the fog (exp, exp2, linear) + */ +GLint fogMode = GL_EXP; + +/** + * Indicates if fog should be shown + */ +int show_fog = 1; + +/** + * Indicates if the points should be shown + */ +int show_points = 1; // Show data points in the viewer? + +/** + * Indicates if camera boxes should be shown + */ +int show_cameras = 1; // Show the camera boxes in the viewer? + +/** + * Indicates if camera path or robot path should be shown + */ +int show_path = 1; // Show the camera movement path ? + +/** + * Camera navigation by mouse or by panel + */ +int cameraNavMouseMode = 1; + +int mouseNavX, mouseNavY; +int mouseNavButton = -1; + +double mouseRotX = 0.0; +double mouseRotY = 0.0; +double mouseRotZ = 0.0; + +bool keymap[256]; + +//@@@ +//int animate_both = 0; // Animate both scan matchin and path? + +int frameNr = 0; + +/** + * Storing of all transformation (frames for animation) of all scans + */ +vector < vector > MetaMatrix; + +/** + * Storing of AlgoType for all frames + */ +vector < vector > MetaAlgoType; + +/** + * Window position + */ +int START_X = 0; +int START_Y = 0; +int START_WIDTH = 960; +int START_HEIGHT = 540; +GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio +bool advanced_controls = false; + +bool fullscreen = false; +int current_width = START_WIDTH; +int current_height = START_HEIGHT; + + +// the following values are scale dependant, i.e. all values are in m +float neardistance = 0.10; +double oldneardistance = 0.10; +float maxfardistance = 400.0; +double fardistance = 400.0; +double oldfardistance = 40000.0; +double movementSpeed = 0.1; +double defaultZoom = 20.0; +GLfloat fogDensity = 0.1; +double voxelSize = 0.20; + + +float adaption_rate = 1.0; +float LevelOfDetail = 0.0001; + + +// Defines for Point Semantic +#define TYPE_UNKNOWN 0x0000 +#define TYPE_OBJECT 0x0001 +#define TYPE_GROUND 0x0002 +#define TYPE_CEILING 0x0003 + +unsigned int cam_choice = 0; + +static unsigned int path_iterator = 0; +static int oldcamNavMode = 0; + +/** + * Animation sould be saved to file + */ +int save_animation = 0; +/** + * If true, interpolation for camera path is based on distance, else always + * the same number of images between two cameras. + */ +int inter_by_dist = 1; + +/**some variables for the camera path**/ +vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; +vector cams; +vector lookats; +vector ups; + +NurbsPath cam_nurbs_path; +char *path_file_name; +char *pose_file_name; + +/** Factor for saved image size */ +int factor = 1; + +/** + * program tries to have this framerate + */ +float idealfps = 20.0; +/** + * value of the listBox fo Color Value and Colormap + */ +int listboxColorVal = 0; +int listboxColorMapVal = 0; +int colorScanVal = 0; +ScanColorManager *cm; +float mincolor_value = 0.0; +float maxcolor_value = 0.0; +//unsigned int types = Point::USE_HEIGHT; +PointType pointtype; + +/** + * Contains the selected points for each scan + */ +set *selected_points; +/** + * Select single points? + */ +int select_voxels = 0; +/** + * Select or unselect points ? + */ +int selectOrunselect = 1; +/** octree depth for selecting groups of points */ +int selection_depth = 1; +int brush_size = 0; +char *selection_file_name; + +int current_frame = 0; +#include "show_menu.cc" +#include "show_animate.cc" +#include "show_gl.cc" + +/** + * Explains the usage of this program's command line parameters + * @param prog name of the program + */ +void usage(char* prog) +{ +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl + << " using shared library F for input" << endl + << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl + << " will attempt to display points with a framerate of NR" << endl + << endl + << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << + "FILE" << endl + << " load objects specified in " << endl + << endl + << endl + << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl + << " neglegt all data points with a distance larger than NR 'units'" << endl + << endl + << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl + << " neglegt all data points with a distance smaller than NR 'units'" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl + << " sets the starting and reset position to: " << endl + << " 0 = the origin of the coordinate system (default)" << endl + << " 1 = the position of the first scan (default if --origin is in argument list)" << endl + << " 2 = the center of the first scan" << endl + << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -C" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl + << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl + << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl + << " " << endl + << endl + + << bold << " -R, --reflectance, --reflectivity" << normal << endl + << " use reflectivity values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -a, --amplitude" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -d, --deviation" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -h, --height" << endl << normal + << " use y-values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -T, --type" << endl << normal + << " use type values for coloring point clouds" << endl + << " only works when using octree display" << endl + << bold << " -c, --color" << endl << normal + << " use color RGB values for coloring point clouds" << endl + << bold << " --saveOct" << endl << normal + << " stores all used scans as octrees in the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl + << " only works when using octree display" << endl + << bold << " --loadOct" << endl << normal + << " only reads octrees from the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are read from file." << endl + << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl + << " only works when using octree display" << endl + << endl << endl; + + exit(1); +} + +/** + * A function that parses the command-line arguments and sets the respective flags. + * + * @param argc the number of arguments + * @param argv the arguments + * @param dir parsing result - the directory + * @param start parsing result - starting at scan number 'start' + * @param end parsing result - stopping at scan number 'end' + * @param maxDist parsing result - maximal distance + * @param minDist parsing result - minimal distance + * @param readInitial parsing result - read a file containing a initial transformation matrix + * @param type parsing result - file format to be read + * @return 0, if the parsing was successful, 1 otherwise + */ +int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, + double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, + bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver) +{ + unsigned int types = PointType::USE_NONE; + start = 0; + end = -1; // -1 indicates no limitation + maxDist = -1; // -1 indicates no limitation + int c; + // from unistd.h + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + cout << endl; + static struct option longopts[] = { + { "origin", optional_argument, 0, 'o' }, + { "format", required_argument, 0, 'f' }, + { "fps", required_argument, 0, 'F' }, + { "scale", required_argument, 0, 'S' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "octree", optional_argument, 0, 'O' }, + { "reflectance", no_argument, 0, 'R' }, + { "reflectivity", no_argument, 0, 'R' }, + { "amplitude", no_argument, 0, 'a' }, + { "deviation", no_argument, 0, 'd' }, + { "height", no_argument, 0, 'h' }, + { "type", no_argument, 0, 'T' }, + { "color", no_argument, 0, 'c' }, + { "loadObj", required_argument, 0, 'l' }, + { "saveOct", no_argument, 0, '0' }, + { "loadOct", no_argument, 0, '1' }, + { "advanced", no_argument, 0, '2' }, + { "scanserver", no_argument, 0, 'S' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRadhTc", longopts, NULL)) != -1) { + switch (c) { + case 's': + w_start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 't': + readInitial = true; + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case '?': + usage(argv[0]); + return 1; + case 'R': + types |= PointType::USE_REFLECTANCE; + break; + case 'a': + types |= PointType::USE_AMPLITUDE; + break; + case 'd': + types |= PointType::USE_DEVIATION; + break; + case 'h': + types |= PointType::USE_HEIGHT; + break; + case 'T': + types |= PointType::USE_TYPE; + break; + case 'c': + types |= PointType::USE_COLOR; + break; + case 'F': + fps = atof(optarg); + break; + case 'C': + scale = atof(optarg); + break; + case 'S': + scanserver = true; + break; + case 'o': + if (optarg) { + origin = atoi(optarg); + } else { + origin = 1; + } + break; + case '0': + saveOct = true; + break; + case '1': + loadOct = true; + break; + case 'l': + loadObj = optarg; + break; + case '2': + advanced_controls = true; + break; + default: + abort (); + } + } + + if (optind != argc-1) { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + parseFormatFile(dir, w_type, w_start, w_end); + + ptype = PointType(types); + return 0; +} + +void setResetView(int origin) { + if (origin == 1) { + double *transmat = MetaMatrix[0].back(); + cout << transmat << endl; + + RVX = -transmat[12]; + RVY = -transmat[13]; + RVZ = -transmat[14]; + Matrix4ToQuat(transmat, Rquat); + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + } else if (origin == 2) { + double center[3]; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[0])->getCenter(center); +#else + ((Show_BOctTree*)octpts[0])->getCenter(center); +#endif + RVX = -center[0]; + RVY = -center[1]; + RVZ = -center[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } +} + +/* + * A function that read the .frame files created by slam6D + * + * @param dir the directory + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param read a file containing a initial transformation matrix and apply it + */ +int readFrames(string dir, int start, int end, bool readInitial, IOType &type) +{ + + // convert to OpenGL coordinate system + double mirror[16]; + M4identity(mirror); + mirror[10] = -1.0; + + double initialTransform[16]; + if (readInitial) { + cout << "Initial Transform:" << endl; + string initialTransformFileName = dir + "initital.frame"; + ifstream initial_in(initialTransformFileName.c_str()); + if (!initial_in.good()) { + cout << "Error opening " << initialTransformFileName << endl; + exit(-1); + } + initial_in >> initialTransform; + cout << initialTransform << endl; + + // update the mirror to apply the initial frame for all frames + double tempxf[16]; + MMult(mirror, initialTransform, tempxf); + memcpy(mirror, tempxf, sizeof(tempxf)); + } + + for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + const double* transformation; + Scan::AlgoType algoType; + vector Matrices; + vector algoTypes; + + // iterate over frames (stop if none were created) and pull/convert the frames into local containers + unsigned int frame_count; + try { + frame_count = (*it)->readFrames(); + } catch(std::ios_base::failure& e) { + break; + } + for(unsigned int i = 0; i < frame_count; ++i) { + (*it)->getFrame(i, transformation, algoType); + double* transMatOpenGL = new double[16]; + + // apply mirror to convert (and initial frame if requested) the frame and save in opengl + MMult(mirror, transformation, transMatOpenGL); + + Matrices.push_back(transMatOpenGL); + algoTypes.push_back(algoType); + } + + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + + if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && it == Scan::allScans.begin()) { + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + } + current_frame = MetaMatrix.back().size() - 1; + } + + if (MetaMatrix.size() == 0) { + cerr << "*****************************************" << endl; + cerr << "** ERROR: No .frames could be found! **" << endl; + cerr << "*****************************************" << endl; + cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; + return -1; + } + return 0; +} + +void generateFrames(int start, int end, bool identity) { + if (identity) { + cout << "using Identity for frames " << endl; + } else { + cout << "using pose information for frames " << endl; + } + int fileCounter = start; + int index = 0; + for (;;) { + if (fileCounter > end) break; // 'nuf read + fileCounter++; + + vector Matrices; + vector algoTypes; + + for (int i = 0; i < 3; i++) { + double *transMat = new double[16]; + + if (identity) { + M4identity(transMat); + transMat[10] = -1.0; + } else { + EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); + } + + Matrices.push_back(transMat); + algoTypes.push_back(Scan::ICP); + + } + index++; + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + current_frame = MetaMatrix.back().size() - 1; + } +} + + + +/* + * create display lists + * @to do general framework for color & type definitions + */ +void createDisplayLists(bool reduced) +{ + for(unsigned int s = 0; s < Scan::allScans.size() ; s++) { + Scan* scan = Scan::allScans[s]; + + vertexArray* myvertexArray1; + vertexArray* myvertexArray2; + + // count points + unsigned int color1 = 0, color2 = 0; + if(!reduced) { + scan->get(DATA_XYZ | DATA_TYPE); + DataType type(scan->get("type")); + + if(type.valid()) { + for(unsigned int i = 0; i < type.size(); ++i) { + if(type[i] & TYPE_GROUND) { + color1 += 3; + } else { + color2 += 3; + } + } + } else { + color2 = 3 * scan->size("xyz"); + } + + myvertexArray1 = new vertexArray(color1); + myvertexArray2 = new vertexArray(color2); + + color1 = 0; color2 = 0; + + DataXYZ xyz(scan->get("xyz")); + for(unsigned int i = 0; i < xyz.size(); ++i) { + if(type[i] & TYPE_GROUND) { + for(unsigned int j = 0; j < 3; ++j) { + myvertexArray1->array[color1++] = xyz[i][j]; + } + } else { + for(unsigned int j = 0; j < 3; ++j) { + myvertexArray2->array[color2++] = xyz[i][j]; + } + } + } + } else { + color2 = 3 * scan->size("xyz reduced"); + + myvertexArray1 = new vertexArray(0); + myvertexArray2 = new vertexArray(color2); + + color2 = 0; + + DataXYZ xyz_r(scan->get("xyz reduced")); + for(unsigned int i = 0; i < xyz_r.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + myvertexArray2->array[color2++] = xyz_r[i][j]; + } + } + } + + + + glNewList(myvertexArray1->name, GL_COMPILE); + //@ + //glColor4d(0.44, 0.44, 0.44, 1.0); + //glColor4d(0.66, 0.66, 0.66, 1.0); + glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender); + glDisableClientState(GL_VERTEX_ARRAY); + glEndList(); + + glNewList(myvertexArray2->name, GL_COMPILE); + //glColor4d(1.0, 1.0, 1.0, 1.0); + //glColor4d(0.0, 0.0, 0.0, 1.0); + glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender); + glDisableClientState(GL_VERTEX_ARRAY); + glEndList(); + + // append to vector + vector vvertexArray; + vvertexArray.push_back(myvertexArray1); + vvertexArray.push_back(myvertexArray2); + vvertexArrayList.push_back(vvertexArray); + } +} + +void cycleLOD() { + LevelOfDetail = 0.00001; + for (unsigned int i = 0; i < octpts.size(); i++) + octpts[i]->cycleLOD(); +} + + +void initShow(int argc, char **argv){ + + /***************/ + /* init OpenGL */ + /***************/ + glutInit(&argc,argv); + + cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; + + if(argc <= 1){ + usage(argv[0]); + } + + double red = -1.0; + int start = 0, end = -1, maxDist = -1, minDist = -1; + string dir; + bool readInitial = false; + IOType type = UOS; + int octree = 0; + bool loadOct = false; + bool saveOct = false; + string loadObj; + int origin = 0; + double scale = 0.01; // in m + bool scanserver = false; + + pose_file_name = new char[1024]; + path_file_name = new char[1024]; + selection_file_name = new char[1024]; + + strncpy(pose_file_name, "pose.dat", 1024); + strncpy(path_file_name, "path.dat", 1024); + strncpy(selection_file_name, "selected.3d", 1024); + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, + octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, type, scanserver); + + // modify all scale dependant variables + scale = 1.0 / scale; + movementSpeed *= scale; + neardistance *= scale; + oldneardistance *= scale; + maxfardistance *= scale; + fardistance *= scale; + fogDensity /= scale; + defaultZoom *= scale; + voxelSize *= scale; +// oldfardistance *= scale; + + //////////////////////// + SDisplay::readDisplays(loadObj, displays); + //////////////////// + + if (type == OCT) { + loadOct = true; + } + + // if we want to load display file get pointtypes from the files first + if(loadOct) { + string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; + cout << "Getting point information from " << scanFileName << endl; + cout << "Attention! All subsequent oct-files must be of the same type!" << endl; + + pointtype = BOctTree::readType(scanFileName); + } + scan_dir = dir; + + // init and create display + M4identity(view_rotate_button); + obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; + + // Loading scans, reducing, loading frames and generation if neccessary + + // load all available scans + Scan::openDirectory(scanserver, dir, type, start, end); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + scan->setRangeFilter(maxDist, minDist); + if(red > 0) { + // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time + if(scanserver) { + dynamic_cast(scan)->setShowReductionParameter(red, octree); + } else { + scan->setReductionParameter(red, octree); + } + } + } + cm = new ScanColorManager(4096, pointtype); + +#ifdef USE_GL_POINTS // use octtrees +#ifdef USE_COMPACT_TREE + cout << "Creating compact display octrees.." << endl; +#else + cout << "Creating display octrees.." << endl; +#endif + + if(loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; + + // for managed scans the input phase needs to know how much it can handle + std::size_t free_mem = 0; + if(scanserver) + free_mem = ManagedScan::getMemorySize(); + + for(unsigned int i = 0; i < Scan::allScans.size(); ++i) { + Scan* scan = Scan::allScans[i]; + + // create data structures +#ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed + compactTree* tree; + try { + if (red > 0) { // with reduction, only xyz points + DataXYZ xyz_r(scan->get("xyz reduced show")); + tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); + } else { // without reduction, xyz + attribute points + sfloat** pts = pointtype.createPointArray(scan); + unsigned int nrpts = scan->size("xyz"); + tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + } + } catch(...) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } +#else // FIXME: remove the case above + scan->setOcttreeParameter(red, voxelSize, pointtype, loadOct, saveOct); + + DataOcttree* data_oct; + try { + data_oct = new DataOcttree(scan->get("octtree")); + } catch(runtime_error& e) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } + BOctTree* btree = &(data_oct->get()); + unsigned int tree_size = btree->getMemorySize(); + + if(scanserver) { + // check if the octtree would actually fit with all the others + if(tree_size > free_mem) { + delete data_oct; + cout << "Stopping at scan " << i << ", no more octtrees could fit in memory." << endl; + break; + } else { + // subtract available memory + free_mem -= tree_size; + } + } +#endif //FIXME: COMPACT_TREE + +#if !defined USE_COMPACT_TREE + // show structures + // associate show octtree with the scan and hand over octtree pointer ownership + Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); + + // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points + tree->unlockCachedTree(); +#endif + + // octtrees have been created successfully + octpts.push_back(tree); + + // print something +#ifdef USE_COMPACT_TREE // TODO: change compact tree for memory footprint output, remove this case + cout << "Scan " << i << " octree finished." << endl; +#else + cout << "Scan " << i << " octree finished ("; + bool space = false; + if(tree_size/1024/1024 > 0) { + cout << tree_size/1024/1024 << "M"; + space = true; + } + if((tree_size/1024)%1024 > 0) { + if(space) cout << " "; + cout << (tree_size/1024)%1024 << "K"; + space = true; + } + if(tree_size%1024 > 0) { + if(space) cout << " "; + cout << tree_size%1024 << "B"; + } + cout << ")." << endl; +#endif + } + +/* +TODO: to maximize space for octtrees, implement a heuristic to remove all +CacheObjects sequentially from the start and pack octtrees one after another +until it reaches the maximum size allowed, resetting the index, but causing the +first to be locked again and stopping by catching the exception +set heuristic, do locking, catch exception, reset heuristic to default or old +*/ +#if !defined USE_COMPACT_TREE + if(scanserver) { + // activate all octtrees until they don't fit anymore + cout << "Locking managed octtrees in memory " << flush; + bool stop = false; + unsigned int loaded = 0; + unsigned int dots = (octpts.size() / 20); + if(dots == 0) dots = 1; + vector::iterator it_remove_first = octpts.end(); + for(vector::iterator it = octpts.begin(); it != octpts.end(); ++it) { + if(!stop) { + // try to lock the octtree in cache + try { + Show_BOctTree* stree = dynamic_cast*>(*it); + stree->lockCachedTree(); + loaded++; + if(loaded % dots == 0) cout << '.' << flush; + } catch(runtime_error& e) { + stop = true; + it_remove_first = it; + } + } + if(stop) { + // delete the octtree, resize after iteration + delete *it; + } + } + // now remove iterators for deleted octtrees + if(stop) octpts.erase(it_remove_first, octpts.end()); + cout << ' ' << loaded << " octtrees loaded." << endl; + } + +#endif // !COMPACT_TREE + +#else // not using octtrees + createDisplayLists(red > 0); +#endif // USE_GL_POINTS + + // load frames now that we know how many scans we actually loaded + unsigned int real_end = min((unsigned int)(end), + (unsigned int)(start + octpts.size() - 1)); + if(readFrames(dir, start, real_end, readInitial, type)) + generateFrames(start, real_end, true); + + cm->setCurrentType(PointType::USE_HEIGHT); + //ColorMap cmap; + //cm->setColorMap(cmap); + resetMinMax(0); + + selected_points = new set[octpts.size()]; + + // sets (and computes if necessary) the pose that is used for the reset button + setResetView(origin); + + for (unsigned int i = 0; i < 256; i++) { + keymap[i] = false; + } +} + +void deinitShow() +{ + static volatile bool done = false; + if(done) return; + done = true; + + cout << "Cleaning up octtrees and scans." << endl; + if(octpts.size()) { + // delete octtrees to release the cache locks within + for(vector::iterator it = octpts.begin(); it!= octpts.end(); ++it) { + delete *it; + } + } + + Scan::closeDirectory(); +} + +/** + * Global program scope destructor workaround to clean up data regardless of + * the way of program exit. + */ +struct Deinit { ~Deinit() { deinitShow(); } } deinit; diff --git a/.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base b/.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base new file mode 100644 index 0000000..462b788 --- /dev/null +++ b/.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base @@ -0,0 +1,29 @@ +/** + * @file scan_io_uosr.h + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * @author Billy Okal + */ + +#ifndef __SCAN_IO_UOSR_H__ +#define __SCAN_IO_UOSR_H__ + +#include "scan_io.h" + + + +/** + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * + * The compiled class is available as shared object file + */ +class ScanIO_uosr : public ScanIO { +public: + virtual std::list readDirectory(const char* dir_path, unsigned int start, unsigned int end); + virtual void readPose(const char* dir_path, const char* identifier, double* pose); + virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/97/97c2b2ecb55373715bd55e56bcc6cf64da64e535.svn-base b/.svn/pristine/97/97c2b2ecb55373715bd55e56bcc6cf64da64e535.svn-base new file mode 100644 index 0000000..bc0966a --- /dev/null +++ b/.svn/pristine/97/97c2b2ecb55373715bd55e56bcc6cf64da64e535.svn-base @@ -0,0 +1,845 @@ +/* + * viewcull implementation + * + * Copyright (C) Jan Elseberg + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Frustrum culling routines + * + * @author Jan Elseberg. Automation Group, Jacobs University Bremen gGmbH, Germany. + */ +#include "show/viewcull.h" +#include +#include +#include + +#ifdef __APPLE__ +#include +#include +#else +#include +#include +#endif + +#include "slam6d/globals.icc" + +/** The 6 planes of the viewing frustum */ +float frustum[6][4]; + +/** the modelview * projection matrix to map a model point to an onscreen coordinate */ +float matrix[16]; +/** some useful variables for faster calculation of the pixel coordinates */ +short VP[4]; +/** a unit vector pointing to the right of the screen */ +float right[3]; +/** how much detail is shown, 0 means everything is plotted */ +short DETAIL; + +double SX, SY, SZ, EX, EY, EZ; +float origin[3], dir[3]; /*ray */ +float dist; +int rayX,rayY; +float rayVP[4]; + +void calcRay(int x, int y, double znear, double zfar) { + + GLdouble modelMatrix[16]; + GLdouble projMatrix[16]; + int viewport[4]; + glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix); + glGetDoublev(GL_PROJECTION_MATRIX,projMatrix); + glGetIntegerv(GL_VIEWPORT,viewport); + + gluUnProject(x, viewport[3]-y, zfar, modelMatrix, projMatrix, viewport, &SX, &SY, &SZ); + gluUnProject(x, viewport[3]-y, znear, modelMatrix, projMatrix, viewport, &EX, &EY, &EZ); + + origin[0] = SX; + origin[1] = SY; + origin[2] = SZ; + dir[0] = EX-SX; + dir[1] = EY-SY; + dir[2] = EZ-SZ; + double t = sqrt( dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2] ); + dir[0] /= t; + dir[1] /= t; + dir[2] /= t; + + dist = SX * dir[0] + SY * dir[1] + SZ * dir[2]; + rayVP[0] = 0.5*viewport[2]; + rayVP[1] = 0.5*viewport[2] + viewport[0]; + + rayVP[2] = 0.5*viewport[3]; + rayVP[3] = 0.5*viewport[3] + viewport[1]; + rayX = x; + rayY = viewport[3]-y; + + remViewport(); +} + +void remViewport() { + GLdouble modelMatrix[16]; + GLdouble projMatrix[16]; + int viewport[4]; + glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix); + glGetDoublev(GL_PROJECTION_MATRIX,projMatrix); + glGetIntegerv(GL_VIEWPORT,viewport); + + MMult( projMatrix, modelMatrix, matrix ); + VP[0] = 0.5*viewport[2]; + VP[1] = 0.5*viewport[2] + viewport[0]; + + VP[2] = 0.5*viewport[3]; + VP[3] = 0.5*viewport[3] + viewport[1]; + + right[0] = modelMatrix[0]; + right[1] = modelMatrix[4]; + right[2] = modelMatrix[8]; +} + +void ExtractFrustum(short detail) +{ + DETAIL = detail + 1; + remViewport(); + + float proj[16]; + float modl[16]; + float clip[16]; + float t; + + /* Get the current PROJECTION matrix from OpenGL */ + glGetFloatv( GL_PROJECTION_MATRIX, proj ); + + /* Get the current MODELVIEW matrix from OpenGL */ + glGetFloatv( GL_MODELVIEW_MATRIX, modl ); + + /* Combine the two matrices (multiply projection by modelview) */ + clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12]; + clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13]; + clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14]; + clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15]; + + clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12]; + clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13]; + clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14]; + clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15]; + + clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12]; + clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13]; + clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14]; + clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15]; + + clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12]; + clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13]; + clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14]; + clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15]; + + /* Extract the numbers for the RIGHT plane */ + frustum[0][0] = clip[ 3] - clip[ 0]; + frustum[0][1] = clip[ 7] - clip[ 4]; + frustum[0][2] = clip[11] - clip[ 8]; + frustum[0][3] = clip[15] - clip[12]; + + /* Normalize the result */ + t = sqrt( frustum[0][0] * frustum[0][0] + frustum[0][1] * frustum[0][1] + frustum[0][2] * frustum[0][2] ); + frustum[0][0] /= t; + frustum[0][1] /= t; + frustum[0][2] /= t; + frustum[0][3] /= t; + + /* Extract the numbers for the LEFT plane */ + frustum[1][0] = clip[ 3] + clip[ 0]; + frustum[1][1] = clip[ 7] + clip[ 4]; + frustum[1][2] = clip[11] + clip[ 8]; + frustum[1][3] = clip[15] + clip[12]; + + /* Normalize the result */ + t = sqrt( frustum[1][0] * frustum[1][0] + frustum[1][1] * frustum[1][1] + frustum[1][2] * frustum[1][2] ); + frustum[1][0] /= t; + frustum[1][1] /= t; + frustum[1][2] /= t; + frustum[1][3] /= t; + + /* Extract the BOTTOM plane */ + frustum[2][0] = clip[ 3] + clip[ 1]; + frustum[2][1] = clip[ 7] + clip[ 5]; + frustum[2][2] = clip[11] + clip[ 9]; + frustum[2][3] = clip[15] + clip[13]; + + /* Normalize the result */ + t = sqrt( frustum[2][0] * frustum[2][0] + frustum[2][1] * frustum[2][1] + frustum[2][2] * frustum[2][2] ); + frustum[2][0] /= t; + frustum[2][1] /= t; + frustum[2][2] /= t; + frustum[2][3] /= t; + + /* Extract the TOP plane */ + frustum[3][0] = clip[ 3] - clip[ 1]; + frustum[3][1] = clip[ 7] - clip[ 5]; + frustum[3][2] = clip[11] - clip[ 9]; + frustum[3][3] = clip[15] - clip[13]; + + /* Normalize the result */ + t = sqrt( frustum[3][0] * frustum[3][0] + frustum[3][1] * frustum[3][1] + frustum[3][2] * frustum[3][2] ); + frustum[3][0] /= t; + frustum[3][1] /= t; + frustum[3][2] /= t; + frustum[3][3] /= t; + + /* Extract the FAR plane */ + frustum[4][0] = clip[ 3] - clip[ 2]; + frustum[4][1] = clip[ 7] - clip[ 6]; + frustum[4][2] = clip[11] - clip[10]; + frustum[4][3] = clip[15] - clip[14]; + + /* Normalize the result */ + t = sqrt( frustum[4][0] * frustum[4][0] + frustum[4][1] * frustum[4][1] + frustum[4][2] * frustum[4][2] ); + frustum[4][0] /= t; + frustum[4][1] /= t; + frustum[4][2] /= t; + frustum[4][3] /= t; + + /* Extract the NEAR plane */ + frustum[5][0] = clip[ 3] + clip[ 2]; + frustum[5][1] = clip[ 7] + clip[ 6]; + frustum[5][2] = clip[11] + clip[10]; + frustum[5][3] = clip[15] + clip[14]; + + /* Normalize the result */ + t = sqrt( frustum[5][0] * frustum[5][0] + frustum[5][1] * frustum[5][1] + frustum[5][2] * frustum[5][2] ); + frustum[5][0] /= t; + frustum[5][1] /= t; + frustum[5][2] /= t; + frustum[5][3] /= t; + +} + + +void ExtractFrustum(float *frust[6]) +{ + remViewport(); + + float proj[16]; + float modl[16]; + float clip[16]; + float t; + + /* Get the current PROJECTION matrix from OpenGL */ + glGetFloatv( GL_PROJECTION_MATRIX, proj ); + + /* Get the current MODELVIEW matrix from OpenGL */ + glGetFloatv( GL_MODELVIEW_MATRIX, modl ); + + /* Combine the two matrices (multiply projection by modelview) */ + clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12]; + clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13]; + clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14]; + clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15]; + + clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12]; + clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13]; + clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14]; + clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15]; + + clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12]; + clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13]; + clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14]; + clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15]; + + clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12]; + clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13]; + clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14]; + clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15]; + + /* Extract the numbers for the RIGHT plane */ + frust[0][0] = clip[ 3] - clip[ 0]; + frust[0][1] = clip[ 7] - clip[ 4]; + frust[0][2] = clip[11] - clip[ 8]; + frust[0][3] = clip[15] - clip[12]; + + /* Normalize the result */ + t = sqrt( frust[0][0] * frust[0][0] + frust[0][1] * frust[0][1] + frust[0][2] * frust[0][2] ); + frust[0][0] /= t; + frust[0][1] /= t; + frust[0][2] /= t; + frust[0][3] /= t; + + /* Extract the numbers for the LEFT plane */ + frust[1][0] = clip[ 3] + clip[ 0]; + frust[1][1] = clip[ 7] + clip[ 4]; + frust[1][2] = clip[11] + clip[ 8]; + frust[1][3] = clip[15] + clip[12]; + + /* Normalize the result */ + t = sqrt( frust[1][0] * frust[1][0] + frust[1][1] * frust[1][1] + frust[1][2] * frust[1][2] ); + frust[1][0] /= t; + frust[1][1] /= t; + frust[1][2] /= t; + frust[1][3] /= t; + + /* Extract the BOTTOM plane */ + frust[2][0] = clip[ 3] + clip[ 1]; + frust[2][1] = clip[ 7] + clip[ 5]; + frust[2][2] = clip[11] + clip[ 9]; + frust[2][3] = clip[15] + clip[13]; + + /* Normalize the result */ + t = sqrt( frust[2][0] * frust[2][0] + frust[2][1] * frust[2][1] + frust[2][2] * frust[2][2] ); + frust[2][0] /= t; + frust[2][1] /= t; + frust[2][2] /= t; + frust[2][3] /= t; + + /* Extract the TOP plane */ + frust[3][0] = clip[ 3] - clip[ 1]; + frust[3][1] = clip[ 7] - clip[ 5]; + frust[3][2] = clip[11] - clip[ 9]; + frust[3][3] = clip[15] - clip[13]; + + /* Normalize the result */ + t = sqrt( frust[3][0] * frust[3][0] + frust[3][1] * frust[3][1] + frust[3][2] * frust[3][2] ); + frust[3][0] /= t; + frust[3][1] /= t; + frust[3][2] /= t; + frust[3][3] /= t; + + /* Extract the FAR plane */ + frust[4][0] = clip[ 3] - clip[ 2]; + frust[4][1] = clip[ 7] - clip[ 6]; + frust[4][2] = clip[11] - clip[10]; + frust[4][3] = clip[15] - clip[14]; + + /* Normalize the result */ + t = sqrt( frust[4][0] * frust[4][0] + frust[4][1] * frust[4][1] + frust[4][2] * frust[4][2] ); + frust[4][0] /= t; + frust[4][1] /= t; + frust[4][2] /= t; + frust[4][3] /= t; + + /* Extract the NEAR plane */ + frust[5][0] = clip[ 3] + clip[ 2]; + frust[5][1] = clip[ 7] + clip[ 6]; + frust[5][2] = clip[11] + clip[10]; + frust[5][3] = clip[15] + clip[14]; + + /* Normalize the result */ + t = sqrt( frust[5][0] * frust[5][0] + frust[5][1] * frust[5][1] + frust[5][2] * frust[5][2] ); + frust[5][0] /= t; + frust[5][1] /= t; + frust[5][2] /= t; + frust[5][3] /= t; + +} + + + +void myProject(float x, float y, float z, short &Xi ) { + float pn[2]; + // x coordinate on screen, not normalized + pn[0] = x * matrix[0] + y * matrix[4] + z * matrix[8] + matrix[12]; + // normalization + pn[1] = x * matrix[3] + y * matrix[7] + z * matrix[11] + matrix[15]; + + // normalized x coordinate on screen + pn[0] /= pn[1]; + + // true x coordinate in viewport coordinate system + Xi = pn[0]*VP[0] + VP[1]; +} + +int LOD2(float x, float y, float z, float size) +{ + size = sqrt(3*size*size); + + short X1; + short X2; + + // onscreen position of the leftmost point + myProject(x - size*right[0], y - size*right[1], z - size*right[2], X1); + // onscreen position of the rightmost point + myProject(x + size*right[0], y + size*right[1], z + size*right[2], X2); + + if (X1 > X2) { + return (X1-X2); + } else { + return (X2-X1); + } +} + +bool LOD(float x, float y, float z, float size) +{ + size = sqrt(3*size*size); + + short X1; + short X2; + + // onscreen position of the leftmost point + myProject(x - size*right[0], y - size*right[1], z - size*right[2], X1); + // onscreen position of the rightmost point + myProject(x + size*right[0], y + size*right[1], z + size*right[2], X2); + + if (X1 > X2) { + return (X1-X2) > DETAIL; + } else { + return (X2-X1) > DETAIL; + } +} + + +/** + * 0 if not in frustrum + * 1 if partial overlap + * 2 if entirely within frustrum + */ +int CubeInFrustum2( float x, float y, float z, float size ) +{ + int p; + + float xm, xp, ym, yp, zm, zp; + float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; + xm = x - size; + xp = x + size; + ym = y - size; + yp = y + size; + zm = z - size; + zp = z + size; + + for( p = 0; p < 6; p++ ) + { + Fxm = frustum[p][0] * xm; + Fym = frustum[p][1] * ym; + Fzm = frustum[p][2] * zm; + if( Fxm + Fym + Fzm + frustum[p][3] > 0 ) + continue; + + Fxp = frustum[p][0] * xp; + if( Fxp + Fym + Fzm + frustum[p][3] > 0 ) + continue; + + Fyp = frustum[p][1] * yp; + if( Fxm + Fyp + Fzm + frustum[p][3] > 0 ) + continue; + + if( Fxp + Fyp + Fzm + frustum[p][3] > 0 ) + continue; + + Fzp = frustum[p][2] * zp; + if( Fxm + Fym + Fzp + frustum[p][3] > 0 ) + continue; + + if( Fxp + Fym + Fzp + frustum[p][3] > 0 ) + continue; + if( Fxm + Fyp + Fzp + frustum[p][3] > 0 ) + continue; + if( Fxp + Fyp + Fzp + frustum[p][3] > 0 ) + continue; + return 0; + } + + // box is in frustrum, now check wether all corners are within. + // If one is outside return 1 otherwise 2 + for( p = 0; p < 6; p++ ) + { + Fxm = frustum[p][0] * xm; + Fym = frustum[p][1] * ym; + Fzm = frustum[p][2] * zm; + if( Fxm + Fym + Fzm + frustum[p][3] < 0 ) + return 1; + + Fxp = frustum[p][0] * xp; + if( Fxp + Fym + Fzm + frustum[p][3] < 0 ) + return 1; + + Fyp = frustum[p][1] * yp; + if( Fxm + Fyp + Fzm + frustum[p][3] < 0 ) + return 1; + + if( Fxp + Fyp + Fzm + frustum[p][3] < 0 ) + return 1; + + Fzp = frustum[p][2] * zp; + if( Fxm + Fym + Fzp + frustum[p][3] < 0 ) + return 1; + + if( Fxp + Fym + Fzp + frustum[p][3] < 0 ) + return 1; + if( Fxm + Fyp + Fzp + frustum[p][3] < 0 ) + return 1; + if( Fxp + Fyp + Fzp + frustum[p][3] < 0 ) + return 1; + } + return 2; + +} + +/* +char PlaneAABB( float x, float y, float z, float size, float *plane ) +{ + float dist = plane[3]; + + float xm, xp, ym, yp, zm, zp; + float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; + + float dmmm, dpmm, dmpm, dppm, dmmp, dpmp, dmpp, dppp; + xm = x - size; + xp = x + size; + ym = y - size; + yp = y + size; + zm = z - size; + zp = z + size; + + { + Fxm = plane[0] * xm; + Fym = plane[1] * ym; + Fzm = plane[2] * zm; + dmmm = Fxm + Fym + Fzm + dist; + if( dmmm > 0 ) + goto intersects; + + Fxp = plane[0] * xp; + dpmm = Fxp + Fym + Fzm + dist; + if( dpmm > 0 ) + goto intersects; + + Fyp = plane[1] * yp; + dmpm = Fxm + Fyp + Fzm + dist; + if( dmpm > 0 ) + goto intersects; + + dppm = Fxp + Fyp + Fzm + dist; + if( dppm > 0 ) + goto intersects; + + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp > 0 ) + goto intersects; + + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp > 0 ) + goto intersects; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp > 0 ) + goto intersects; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp > 0 ) + goto intersects; + return 0; // outside + } + + intersects: + + // cube is inside, determine if plane intersects the cube + { + if( dmmm < 0 ) + return 1; + Fxp = plane[0] * xp; + dpmm = Fxp + Fym + Fzm + dist; + if( dpmm < 0 ) + return 1; + Fyp = plane[1] * yp; + dmpm = Fxm + Fyp + Fzm + dist; + if( dmpm < 0 ) + return 1; + dppm = Fxp + Fyp + Fzm + dist; + if( dppm < 0 ) + return 1; + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp < 0 ) + return 1; + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + } + + // plane is outside + return 2; + +} +*/ +char PlaneAABB( float x, float y, float z, float size, float *plane ) +{ + float dist = plane[3]; + + float xm, xp, ym, yp, zm, zp; + float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; + + float dmmm, dpmm, dmpm, dppm, dmmp, dpmp, dmpp, dppp; + xm = x - size; + xp = x + size; + ym = y - size; + yp = y + size; + zm = z - size; + zp = z + size; + + Fxm = plane[0] * xm; + Fym = plane[1] * ym; + Fzm = plane[2] * zm; + dmmm = Fxm + Fym + Fzm + dist; + if( dmmm > 0 ) + { + Fxp = plane[0] * xp; + dpmm = Fxp + Fym + Fzm + dist; + if( dpmm < 0 ) + return 1; + Fyp = plane[1] * yp; + dmpm = Fxm + Fyp + Fzm + dist; + if( dmpm < 0 ) + return 1; + dppm = Fxp + Fyp + Fzm + dist; + if( dppm < 0 ) + return 1; + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp < 0 ) + return 1; + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + + Fxp = plane[0] * xp; + dpmm = Fxp + Fym + Fzm + dist; + if( dpmm > 0 ) + { + if( dmmm < 0 ) + return 1; + Fyp = plane[1] * yp; + dmpm = Fxm + Fyp + Fzm + dist; + if( dmpm < 0 ) + return 1; + dppm = Fxp + Fyp + Fzm + dist; + if( dppm < 0 ) + return 1; + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp < 0 ) + return 1; + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + + Fyp = plane[1] * yp; + dmpm = Fxm + Fyp + Fzm + dist; + if( dmpm > 0 ) + { + if( dmmm < 0 || dpmm < 0 ) + return 1; + dppm = Fxp + Fyp + Fzm + dist; + if( dppm < 0 ) + return 1; + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp < 0 ) + return 1; + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + + dppm = Fxp + Fyp + Fzm + dist; + if( dppm > 0 ) + { + if( dmmm < 0 || dpmm < 0 || dmpm < 0 ) + return 1; + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp < 0 ) + return 1; + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + + Fzp = plane[2] * zp; + dmmp = Fxm + Fym + Fzp + dist; + if( dmmp > 0 ) + { + if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 ) + return 1; + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + + dpmp = Fxp + Fym + Fzp + dist; + if( dpmp > 0 ) + { + if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 ) + return 1; + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + dmpp = Fxm + Fyp + Fzp + dist; + if( dmpp > 0 ) + { + if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 || dpmp < 0 ) + return 1; + dppp = Fxp + Fyp + Fzp + dist; + if( dppp < 0 ) + return 1; + return 2; + } + dppp = Fxp + Fyp + Fzp + dist; + if( dppp > 0 ) + { + if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 || dpmp < 0 || dmpp < 0) + return 1; + return 2; + } + return 0; // outside +} + +int QuadInFrustrum2old( float x, float y, float z, float size ) +{ + int p; + + for( p = 0; p < 6; p++ ) + { + if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) + continue; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) + continue; + return 0; + } + + // box is in frustrum, now check wether all corners are within. If one is outside return 1 otherwise 2 + for( p = 0; p < 6; p++ ) + { + if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) + return 1; + if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) + return 1; + } + return 2; + +} + +bool CubeInFrustum( float x, float y, float z, float size ) +{ + int p; + + float xm, xp, ym, yp, zm, zp; + float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; + xm = x - size; + xp = x + size; + ym = y - size; + yp = y + size; + zm = z - size; + zp = z + size; + + for( p = 0; p < 6; p++ ) + { + Fxm = frustum[p][0] * xm; + Fym = frustum[p][1] * ym; + Fzm = frustum[p][2] * zm; + if( Fxm + Fym + Fzm + frustum[p][3] > 0 ) + continue; + + Fxp = frustum[p][0] * xp; + if( Fxp + Fym + Fzm + frustum[p][3] > 0 ) + continue; + + Fyp = frustum[p][1] * yp; + if( Fxm + Fyp + Fzm + frustum[p][3] > 0 ) + continue; + + if( Fxp + Fyp + Fzm + frustum[p][3] > 0 ) + continue; + + Fzp = frustum[p][2] * zp; + if( Fxm + Fym + Fzp + frustum[p][3] > 0 ) + continue; + + if( Fxp + Fym + Fzp + frustum[p][3] > 0 ) + continue; + if( Fxm + Fyp + Fzp + frustum[p][3] > 0 ) + continue; + if( Fxp + Fyp + Fzp + frustum[p][3] > 0 ) + continue; + return false; + } + return true; +} + + + + +float minB[NUMDIM], maxB[NUMDIM]; /*box */ +float coord[NUMDIM]; /* hit point */ diff --git a/include/scanserver/io_types.h b/.svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base similarity index 60% rename from include/scanserver/io_types.h rename to .svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base index 27fa470..dc95bb1 100644 --- a/include/scanserver/io_types.h +++ b/.svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base @@ -2,7 +2,7 @@ * @file * @brief Scan types and mapping functions. * - * @author Thomas Escher + * @author Thomas Escher, Billy Okal */ #ifndef IO_TYPES_H @@ -10,7 +10,7 @@ //! IO types for file formats, distinguishing the use of ScanIOs enum IOType { - UOS, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES + UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES }; //! Data channels in the scans diff --git a/.svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base b/.svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base new file mode 100644 index 0000000..088c27a --- /dev/null +++ b/.svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base @@ -0,0 +1,171 @@ +/* + * scan_io_uosr implementation + * + * Copyright (C) Billy Okal + * + * Released under the GPL version 3. + * + */ + + +/** + * @file scan_io_uosr.cc + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * @author Billy Okal. Jacobs University Bremen, Germany. + */ + +#include "scanio/scan_io_uosr.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + + + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".3d" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + + + +std::list ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end) +{ + std::list identifiers; + for(unsigned int i = start; i <= end; ++i) { + // identifier is /d/d/d (000-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.3d) and pose (.pose) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + path pose(dir_path); + pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if(!exists(data) || !exists(pose)) + break; + identifiers.push_back(identifier); + } + return identifiers; +} + +void ScanIO_uosr::readPose(const char* dir_path, const char* identifier, double* pose) +{ + unsigned int i; + + path pose_path(dir_path); + pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + if(!exists(pose_path)) + throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]"); + + // open pose file + ifstream pose_file(pose_path); + + // if the file is open, read contents + if(pose_file.good()) { + // read 6 plain doubles + for(i = 0; i < 6; ++i) pose_file >> pose[i]; + pose_file.close(); + + // convert angles from deg to rad + for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]); + } else { + throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]"); + } +} + +bool ScanIO_uosr::supports(IODataType type) +{ + return !!(type & ( DATA_REFLECTANCE | DATA_XYZ )); +} + +void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation) +{ + unsigned int i; + + // error handling + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]"); + + if(xyz != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // overread the first line ignoring the header information + char dummy[255]; + data_file.getline(dummy, 255); + + // read points and reflectance/intensity/temperature value + double point[3]; + float reflection; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; + data_file >> reflection; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter then insert point and reflectance + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + reflectance->push_back(reflection); + } + } + data_file.close(); + } +} + + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) ScanIO* create() +#else +extern "C" ScanIO* create() +#endif +{ + return new ScanIO_uosr; +} + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) void destroy(ScanIO *sio) +#else +extern "C" void destroy(ScanIO *sio) +#endif +{ + delete sio; +} + +#ifdef _MSC_VER +BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) +{ + return TRUE; +} +#endif + diff --git a/.svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base b/.svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base new file mode 100644 index 0000000..47c23c4 --- /dev/null +++ b/.svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base @@ -0,0 +1,413 @@ +/* + * feature_based_registration implementation + * + * Copyright (C) HamidReza Houshiar + * + * Released under the GPL version 3. + * + */ + +#include +#include +#include "slam6d/fbr/fbr_global.h" +#include "slam6d/fbr/scan_cv.h" +#include "slam6d/fbr/panorama.h" +#include "slam6d/fbr/feature.h" +#include "slam6d/fbr/feature_matcher.h" +#include "slam6d/fbr/registration.h" + +using namespace std; +using namespace fbr; + +struct information{ + string local_time; + string dir, outDir; + int iWidth, iHeight, nImages, minDistance, minError, minInlier, fScanNumber, sScanNumber, verbose; + double pParam, mParam; + IOType sFormat; + projection_method pMethod; + feature_detector_method fMethod; + feature_descriptor_method dMethod; + matcher_method mMethod; + registration_method rMethod; + + int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum; + double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime; +} info; + +/** + * usage : explains how to use the program CMD + */ +void usage(int argc, char** argv){ + printf("\n"); + printf("USAGE: %s dir firstScanNumber secondScanNumber \n", argv[0]); + printf("\n"); + printf("\n"); + printf("\tOptions:\n"); + printf("\t\t-F scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n"); + printf("\t\t-W iWidth\t\t panorama image width\n"); + printf("\t\t-H iHeight\t\t panorama image height\n"); + printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n"); + printf("\t\t-N nImage\t\t number of images used for some projections\n"); + printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n"); + printf("\t\t-f fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n"); + printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n"); + printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n"); + printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n"); + printf("\t\t-E minError \t\t threshold for min error in registration process\n"); + printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n"); + printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n"); + printf("\t\t-r registration \t registration method [ALL|ransac]\n"); + printf("\t\t-V verbose \t\t level of verboseness\n"); + printf("\t\t-O outDir \t\t output directory if not stated same as input\n"); + printf("\n"); + printf("\tExamples:\n"); + printf("\tUsing Bremen City dataset:\n"); + printf("\tLoading scan000.txt and scan001.txt:\n"); + printf("\t\t %s ~/dir/to/bremen_city 0 1\n", argv[0]); + printf("\tLoading scan005.txt and scan006.txt and output panorma images and feature images and match images in ~/dir/to/bremen_city/out dir:\n"); + printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 5 6 \n", argv[0]); + printf("\tLoading scan010.txt and scan011.txt using Mercator projection and SURF feature detector and SIFT descriptor:\n"); + printf("\t\t %s -p MERCATOR -f SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 10 11 \n", argv[0]); + printf("\n"); + exit(1); +} + +void parssArgs(int argc, char** argv, information& info){ + cout<<"parssArgs"<tm_year + 1900, timeinfo->tm_mon + 1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec); + info.local_time = time; + + //default values + info.iWidth = 3600; + info.iHeight = 1000; + info.nImages = 1; + info.minDistance = 50; + info.minError = 50; + info.minInlier = 5; + info.verbose = 0; + //depend on the projection method + info.pParam = 0; + info.mParam = 0; + //=============================== + info.sFormat = RIEGL_TXT; + info.pMethod = EQUIRECTANGULAR; + info.fMethod = SIFT_DET; + info.dMethod = SIFT_DES; + info.mMethod = RATIO; + info.rMethod = RANSAC; + info.outDir = ""; + + int c; + opterr = 0; + //reade the command line and get the options + while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:")) != -1) + switch (c) + { + case 'F': + info.sFormat = stringToScanFormat(optarg); + break; + case 'W': + info.iWidth = atoi(optarg); + break; + case 'H': + info.iHeight = atoi(optarg); + break; + case 'p': + info.pMethod = stringToProjectionMethod(optarg); + break; + case 'N': + info.nImages = atoi(optarg); + break; + case 'P': + info.pParam = atoi(optarg); + break; + case 'f': + info.fMethod = stringToFeatureDetectorMethod(optarg); + break; + case 'd': + info.dMethod = stringToFeatureDescriptorMethod(optarg); + break; + case 'm': + info.mMethod = stringToMatcherMethod(optarg); + break; + case 'D': + info.minDistance = atoi(optarg); + break; + case 'E': + info.minError = atoi(optarg); + break; + case 'I': + info.minInlier = atoi(optarg); + break; + case 'M': + info.mParam = atoi(optarg); + break; + case 'r': + info.rMethod = stringToRegistrationMethod(optarg); + break; + case 'V': + cout<<"verboos"< argc - 3) + { + cout<<"Too few input arguments. At least dir and two scan numbers are required."<(i,0) = bAlign[i]; + + string yml; + yml = info.outDir+"fbr-yml.yml"; + cv::FileStorage fs(yml.c_str(), cv::FileStorage::APPEND); + fs << "feature_bas_registration" << "{"; + + fs << "pair" << "{" << "scan" << to_string(info.fScanNumber, 3); + fs << "scan" << to_string(info.sScanNumber, 3) << "}"; + + fs << "time" << "{" << "local_time" << info.local_time << "}"; + + fs << "param" << "{"; + fs << "DIR" << info.dir; + fs << "sFormat" << scanFormatToString(info.sFormat); + fs << "pMethod" << projectionMethodToString(info.pMethod); + fs << "nImage" << info.nImages; + fs << "pParam" << info.pParam; + fs << "iWidth" << info.iWidth; + fs << "iHeight" << info.iHeight; + fs << "fMethod" << featureDetectorMethodToString(info.fMethod); + fs << "dMethod" << featureDescriptorMethodToString(info.dMethod); + fs << "mMethod" << matcherMethodToString(info.mMethod); + fs << "mParam" << info.mParam; + fs << "rMethod" << registrationMethodToString(info.rMethod); + fs << "minDistance" << info.minDistance; + fs << "minInlier" << info.minInlier; + fs << "minError" << info.minError; + fs << "}"; + + fs << "input" << "{"; + fs << "first_input" << "{"; + fs << "name" << "{" << "scan" << to_string(info.fScanNumber, 3) << "}"; + fs << "point" << "{" << "amount" << info.fSPoints << "time" << info.fSTime << "}"; + fs << "projection" << "{" << "time" << info.fPTime << "}"; + fs << "feature" << "{" << "amount" << info.fFNum << "fTime" << info.fFTime << "dTime" << info.fDTime << "}"; + fs << "}"; + fs << "second_input" << "{"; + fs << "name" << "{" << "scan" << to_string(info.sScanNumber, 3) << "}"; + fs << "point" << "{" << "amount" << info.sSPoints << "time" << info.sSTime << "}"; + fs << "projection" << "{" << "time" << info.sPTime << "}"; + fs << "feature" << "{" << "amount" << info.sFNum << "fTime" << info.sFTime << "dTime" << info.sDTime << "}"; + fs << "}"; + fs << "}"; + + fs << "matches" << "{"; + fs << "amount" << info.mNum << "filteration" << info.filteredMNum << "time" << info.mTime << "}"; + + fs << "reg" << "{"; + fs << "bestError" << bError << "bestErrorIdx" << bErrorIdx << "time" << info.rTime << "bAlign" << align << "}"; + + fs << "}"; +} + +int main(int argc, char** argv){ + string out; + cv::Mat outImage; + parssArgs(argc, argv, info); + cout<<"out of parssArgs"<= 1) informationDescription(info); + + scan_cv fScan (info.dir, info.fScanNumber, info.sFormat); + if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount(); + fScan.convertScanToMat(); + if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency(); + if(info.verbose >= 2) fScan.getDescription(); + panorama fPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam); + if(info.verbose >= 4) info.fPTime = (double)cv::getTickCount(); + fPanorama.createPanorama(fScan.getMatScan()); + if(info.verbose >= 4) info.fPTime = ((double)cv::getTickCount() - info.fPTime)/cv::getTickFrequency(); + if(info.verbose >= 2) fPanorama.getDescription(); + //write panorama to image + if(info.verbose >= 1){ + out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg"; + imwrite(out, fPanorama.getReflectanceImage()); + } + feature fFeature; + if(info.verbose >= 4) info.fFTime = (double)cv::getTickCount(); + fFeature.featureDetection(fPanorama.getReflectanceImage(), info.fMethod); + if(info.verbose >= 4) info.fFTime = ((double)cv::getTickCount() - info.fFTime)/cv::getTickFrequency(); + //write panorama with keypoints to image + if(info.verbose >= 1){ + cv::drawKeypoints(fPanorama.getReflectanceImage(), fFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); + out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg"; + imwrite(out, outImage); + outImage.release(); + } + if(info.verbose >= 4) info.fDTime = (double)cv::getTickCount(); + fFeature.featureDescription(fPanorama.getReflectanceImage(), info.dMethod); + if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency(); + if(info.verbose >= 2) fFeature.getDescription(); + + scan_cv sScan (info.dir, info.sScanNumber, info.sFormat); + if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount(); + sScan.convertScanToMat(); + if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency(); + if(info.verbose >= 2) sScan.getDescription(); + panorama sPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam); + if(info.verbose >= 4) info.sPTime = (double)cv::getTickCount(); + sPanorama.createPanorama(sScan.getMatScan()); + if(info.verbose >= 4) info.sPTime = ((double)cv::getTickCount() - info.sPTime)/cv::getTickFrequency(); + if(info.verbose >= 2) sPanorama.getDescription(); + //write panorama to image + if(info.verbose >= 1){ + out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg"; + imwrite(out, sPanorama.getReflectanceImage()); + } + feature sFeature; + if(info.verbose >= 4) info.sFTime = (double)cv::getTickCount(); + sFeature.featureDetection(sPanorama.getReflectanceImage(), info.fMethod); + if(info.verbose >= 4) info.sFTime = ((double)cv::getTickCount() - info.sFTime)/cv::getTickFrequency(); + //write panorama with keypoints to image + if(info.verbose >= 1){ + cv::drawKeypoints(sPanorama.getReflectanceImage(), sFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); + out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg"; + imwrite(out, outImage); + outImage.release(); + } + if(info.verbose >= 4) info.sDTime = (double)cv::getTickCount(); + sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod); + if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency(); + if(info.verbose >= 2) sFeature.getDescription(); + + feature_matcher matcher (info.mMethod, info.mParam); + if(info.verbose >= 4) info.mTime = (double)cv::getTickCount(); + matcher.match(fFeature, sFeature); + if(info.verbose >= 4) info.mTime = ((double)cv::getTickCount() - info.mTime)/cv::getTickFrequency(); + if(info.verbose >= 2) matcher.getDescription(); + //write matcheed feature to image + if(info.verbose >= 1){ + cv::drawMatches(fPanorama.getReflectanceImage(), fFeature.getFeatures(), sPanorama.getReflectanceImage(), sFeature.getFeatures(), matcher.getMatches(), outImage, cv::Scalar::all(-1), cv::Scalar::all(-1), vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); + out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+".jpg"; + imwrite(out, outImage); + outImage.release(); + } + + registration reg (info.minDistance, info.minError, info.minInlier, info.rMethod); + if(info.verbose >= 4) info.rTime = (double)cv::getTickCount(); + reg.findRegistration(fPanorama.getMap(), fFeature.getFeatures(), sPanorama.getMap(), sFeature.getFeatures(), matcher.getMatches()); + if(info.verbose >= 4) info.rTime = ((double)cv::getTickCount() - info.rTime)/cv::getTickFrequency(); + if(info.verbose >= 2) reg.getDescription(); + + //write .dat and .frames files + if(info.verbose >= 0){ + double *bAlign = reg.getBestAlign(); + + out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".dat"; + ofstream dat(out.c_str()); + dat << bAlign[0] << " " << bAlign[4] << " " << bAlign[8] << " " << bAlign[12] <= 3){ + info.fSPoints = fScan.getNumberOfPoints(); + info.sSPoints = sScan.getNumberOfPoints(); + info.fFNum = fFeature.getNumberOfFeatures(); + info.sFNum = sFeature.getNumberOfFeatures(); + info.mNum = matcher.getNumberOfMatches(); + info.filteredMNum = matcher.getNumberOfFilteredMatches(); + + info_yml(info, reg.getBestError(), reg.getBestErrorIndex(), reg.getBestAlign()); + } + + return 0; +} diff --git a/.svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base b/.svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base new file mode 100644 index 0000000..9a15b60 --- /dev/null +++ b/.svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base @@ -0,0 +1,53 @@ +# CLIENT LIBRARY + +# build by source +set(CLIENT_SRCS + clientInterface.cc sharedScan.cc cache/cacheObject.cc + cache/cacheDataAccess.cc +) + +if(WITH_METRICS) + set(CLIENT_SRCS ${CLIENT_SRCS} ../slam6d/metrics.cc) +endif(WITH_METRICS) + +add_library(scanclient STATIC ${CLIENT_SRCS}) + +# add libraries +# boost::interprocess +set(CLIENT_LIBS ${Boost_LIBRARIES} pointfilter) + +if(UNIX AND NOT APPLE) + # boost::interprocess uses pthread, requiring librt + set(CLIENT_LIBS ${CLIENT_LIBS} rt) +endif(UNIX AND NOT APPLE) + +target_link_libraries(scanclient ${CLIENT_LIBS}) + +# SERVER EXECUTABLE + +# build by source +set(SERVER_SRCS + scanserver.cc serverInterface.cc frame_io.cc serverScan.cc + cache/cacheManager.cc cache/cacheHandler.cc scanHandler.cc + temporaryHandler.cc cacheIO.cc +) + +add_executable(scanserver ${SERVER_SRCS}) + +# add libraries +# boost::interprocess/filesystem +# scanclient basic functionality +# scanio for ScanHandler input +set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio) + +if(UNIX) + # boost::interprocess uses pthread, requiring librt + set(SERVER_LIBS ${SERVER_LIBS} rt) +endif(UNIX) + +if(WIN32) + # 3rd party getopt library + set(SERVER_LIBS ${SERVER_LIBS} XGetopt) +endif(WIN32) + +target_link_libraries(scanserver ${SERVER_LIBS}) diff --git a/.svn/wc.db b/.svn/wc.db index 3547808fc677f1482627feb9766f74e60cd277eb..05742f4135c6bb705e3665f2f642b55472640988 100644 GIT binary patch delta 20690 zcma)k2Xs_L_xPKc*ET>PJwONvC4~~QNp?3uDWUhOl#ssJ(2I1G4q`y63yh#5O|Vh( z!NUdu7DUB@QWR+lDk5T`xBtEOC4t}P|2qc`CwXP&&h2gHW%}iM(_gK>xTZE!QTXFa zVp_17+wz~T%7&JnuNoa{_Ti=iFCOz(uJ=0UW@%7DFQ)P?bFxbV<8CF2AB4ZOa*roP z&=eT;;J@Mipad85EXfREb>Xu1_QxDyEU+|S@U`elFpjeW3ct(_lvXZj%~VBUYhchg zfA_f~*?!rxwFr^v;aYUfMoCmBDT76f@M{VZonp=^W2O{9PB`!$A{7 zNNN6}_i%)BiykTmeIw%h{gxbo`)#%0U90B|dWTbT{Obb&3&H9utUkZMQ@B4Uy*B4M zU~$FYXlV{iTJjSQ@xQe66khHZnetSeqo#a6m<8*@M;7^pCHgNc_rkj;J~YGpzUS+g zmPpG1_s$Uj|H`U(|C4KumERVxy#$wp#h`WV%dsfkzdntwZ-~&+${X;yG;?D+eAIIf z3-!PF>ThrfYs2MhhuMBVyA$f4{rVmIVTc@6#nD7AUaLoyo_^yMnB~vh7_x4~CYtXT zA^uI9Pv4*4sPxd5$Ew(aO5N`X97U*h-HqHM-17H)UIwN#b1%aYK>=E*zwf@AaQVbm z04d*&_s`$|E2~pF{)5*{1TmF`tV_jdG@!KoA(bGv0*2>(afq&`0xgdnjq?xs3S~he z>-9BKK_c@WUZqwgS=3W_rs&M-DM?E*!#S@7vbgQ9cdWQ`9vfkQip|41AGhwGk+z!}F; z5hqtN_6fp7R!!vWE4X&PZH8L8g1xQbNC0Xza zVkBLF9%8i%wr_A*D>sDg#yN<(Ob%nW5EvMm)gqiZ%7Mx32=;1turfeV7AgCcYpfy5 zV}ABAyQK_}k&!IZ|Nhp12F}zpUs{nTwJ^2V?MiVMdOf}Zcd<7k*_o17E>59cwEoU889T$x4riWSibs71Cp0|Md=c8Gi>{p+Chq!@$TJ7R& z3JB5Mc=i#_>j{=4(-YZkxF*s%G+<;bNVLpr$ky8K>jQ-!VIP2sZs)v4X;6a7&#dZSppoRq|-?M!B~D!3t?v=`p&9!g>F!hCWzt5>SEuEHn&)9JkS_ueY|O|capF1~ zySAchU{Z+uwuTySf%kn8r*0|C>K`TZHV zU%HgtSb|)T1>7q`Y2X01vcjM{kwJe7e0q;^uDC?JcxY_0m3qC6%8idaWe$sV8&R=6p% zikQ=yU&JN~2WO`heinZCV=wOIJbYbH0^=-H?jFR75Dld7*ikiJ<_u=X;Q?_OhCQaXjTbdphT?fF>DXq zpm(;Kj3W+Pz-|IV1M()YJt%|NEHzZ7PGR8XEKnjEfX>Ovo5ot=fNfz$J?n$nj7%Re z3AOG#%^KrE^rrhWY(L&m_$-+z;$_#l>?}M$d0MUK*_POI4Hv=Im)mAWSma6DpMU{|S_v;P9p`241bJLt3G)W8 zVn531hSCE0Wb#&iA zI*bla9=RmqthpQ6V&EjnqVXmYDY4yZzuCS)b+Gccu+Na+$o!2hcc5G?z!4p_)zf0% zAq0rUR!eX$xGrE6D*Nna5GE~%+#ldGCv${a4fZnf+~jUL?6a335 z2Uf{pHXq3#ubyz+<~6PEhSlZZ??^dp75tu2EJQt$Yj2qevf~fzD$GR}D2@F*UOst} zQ9gsBDL%z^5);mXr#nY9qL?;zJ5T%|Nf-XimLkR!U;=)%-5~~6tMLV9$76Jq{eNRH zsSNGjzq9LbO~ESv5~4@ak30j^O!KT}AyNTanq<*em3u6shoU*<0|-1rXvnn+?BmrRzEy zUbeb10hsl}byQ3Xm{q?5+nYDp`j`MPhx*E5McJUdt9-5e0?9!rYrtB7U;Q#pUN4z3rZEK%V9hsq7G{sOiPBK&Qh%)NX$KgjROm=}$a7)q`5IpzT--=8j0 ztt^!fVLMG8VsN#p+~a3q^7b+?-PEcaos(@Y>8pU!#z7nekNPb^zgx%&!TcK-fX>Ov z58)Q37pO#5!*Cu=Q_+-jBY1DP0da-OV=wD**2PF3M3`ePAL-!nR(cI?8w0BF1b8B+ z)a0|PVCtuARg|sl20sa0c!sm$C7IQwKZ?(Vr6>j`*Rp|;VoX2C8_U&0M3ihMcW#0x+c-S<^0S)=)+RrGflpV@v z%8$xTu+VC(CW~WE)}HlZIcyMnR3VFg&~kn^@^AgRi9Fqc2L&afz&L~S=Mwx_r>p~! zE#tg1C~hq#*LC1ez&uP#t%Q!8ygNB07px%%cjDuhci|Uc1jZ=qZWkVkNMJ@0nE?a3 zcH?OOq+b2I^H)_>%v8dq_85;}(gLlveSmC?N?~&6 zE>_=qqc0zT$kP%v`rArJ{7%o}l;&baVdV_qJ9Qj}$!vU~t;;m0zZYx;-ViH}77^D_ zv8_tQ+*tv+$3R{}-WApV_hCE%-%#L69AN`vQ?yVkdn7+g5<)xp_H!U|(l{_VSQkyY z%Xki{95TSloxnG!dNpOAqU@6a{Y*

Y!EHIfZAigp7;~Uxv?{?((G+Ia9nIPpZ>d zSXAgNE=qy7y~45pq8+d0Nz{yWgf*} z=7;!g5hJ>YQQ|pqLYx3Ci{V!DReTajygn#jtXfxdw3Vzk50h0wxI^B3neT_iN$rk) z0g8WnHLrrmQlzW3)|M2hX#aKm*K#uCuFnATDX;K-FcgWD5h$%EH}J3F0U?;bk&nUQ z7{z4u6+BJ`zD9(S*lWJdB8%PW(&UY|~Aen7xJ10uGVy zT(%vc*4V+X03bAKxgINnPCGf4;N&OzS^Ui!f$Xlr+wl>+E?>xZ@KeGe8jGG{9J^~p z?c%4IT2FJdm(F*1^~&Q$ju{P!Tu>cI9~akZK}{SIMI$>ZI3?|TP1@HH)7 z_S?<>KrFEC8L)@H77(oTSCj?H`^rDEn#ya*pcqFIm2BQZx3e(W=k*l2i+sr`p5%=5 z!j#m)qEv68E5()UNlx*lr)H$5i(G5L`@9cmAvv(fZNT5?4~aBVl|~=&nj{W%bpt-; z932=D&9^~<^ZX|q%baqj^ch@}6!SmjYp8|5xl7?L(MDv8Y+3C~o)2Due*-#G1v35* zL07iRQD5>rm`BR-1)VJmxD)4@WM^Ur~KR6Wl-J}?M#I?TVVte-ihfKud&>HmHY7}<_NxP{1-$C#VcKx zK;9)cczFRX$KT}VaU|w}qi*q|IFb_F0e46-*-fY?2H&NHNkInRJVs8EP}GEfxA+kv7l zJOHB%wFXrcCs5-^mI}R?V^IRx4N8k-0}`3?c(j-d*W?ri*A{1R!dG7bMS1lFRM-{> zpdzJ~Myey;S`aVxqqG4_m86Zy!{7!2dxhBNum(32{Y0W#J4SP~pcJLa7;nLd!6Tcr z?piR^*JbdSv15|PP8_R7Lur?3a#?MITEkMC2+mp#SCrw(WMvt2y*`G-`Whtd_1UA4 z$_`;u*>h|IlzE3BUBAu)coa|KEqO0q#K-dzzLM|eU-CC)(_N6M4^9@{d7K&@q&Z?A zM1aTr_Y>%G^hgz9)|nLHU^~+Ur7y(Zeld``O>_w~a{GZB4XNZN&w4QA$rZ7$Ug)71zRPw$2rMW1p2cUP;LadP|nu!7BAUih~FUemXg$_j- z-1g1HvOrQJ6qC+((#B{U;gnYcP*}0SJ!qpA`NR_@VE(T+vbtgm=&;_)jqA@^-*iu0qX_%a+LS zP^}$geM8W%%CtIQ`Uj_Rx0)&jbD4CBH(~dz><(frU*1Jvd4?!i0bK@Mu25b+(yDRR*e$o01zCo1|-;|pN1C&!B;2yTMdRV54hu=^^Q zSM61;4HZ7mPZWol^i71qXULP{d9B*Jz~?R~iMN&6%HK*C89W_~^Qz6(?K~+!IRh`< zbiXA=OVZgX80`V+yV*?nfyUF&mDjPF%n%WjzGF6*^)#s@h3S6zfZ6;x;$y@Nt#HsY zVufnw^MAlT1N#GDo#ebaXn|nzm-zz2kR4kW2rN*DdA*(mFeMAc6FV1+w!j;Tll_+n z48-JZv!4?fdSK11c8~25g+0Rm;H~*9us{Z9==d(n_De})A#(i!qsh};tgq4bMS#4# zOjMG8EGG;o5Ee@DvW@Cx1)|(#0P!bh|KKlq>v_=|Ks-^+5h`y?1t&Ilh4>O4kXIYA zQoOYDMbQF?6l6fbf$ z(i*-=Eai3_&=qI{>HOM~%m~iD15!@Ox?4q8BxS2@0Epw1Z9=LuwONMzvn>>q5v3vn z4X9Mq;^0k2?hr?Xt*kQZE%e8>56F9)Bu19udq-R;*KRrDJ%KG4=n;j=WjjHX^LC4m z@WCEiT%2bqJWFIjLrevfu5#9=G$_Gfa@+H2XPLE+SdFnr9^C<~{$-zd70?0kt)?YV z=?nqEc-V&mYH};{BSB4O(u~L*=oCK|8=(BPoL>p5bVxTJ|5{-7NTT*07HcAdAd7?l zS6CucxJ%fF%3i3w8}kAg{ixmyyiXb$QK388Q&i;hCPO{w6{aVBG$e-lqhHIjunnn1fo+U=Tt=5TWNTi?+VtSc8qFq1D_6shw0 z0&K@z5-+9QXRr_0=qz-*Q2#|V<*-XXFPV9rrJg!pj1^b)Fj+$}Se4+MaVz=03 zomSQFd6lnbW`Upma%OM%`L#S?sEb4o-bp?>(O78xVyZsoh@UkMJafHSBdtCG>d!i& zT??Qs4OBrMK`Q14q}cs}Rm=~_i(@d!6rYl#)nI}EnXKGZ85*`zYOAxMC(%@CB(-{KtLimGfM@{KX)e^K zGsRM|LF^I-p-#PI<=0bpLANqg)`(Z5bu|p=3x<9g_^3YdD#cu4XNyFY+&OIbS{)jw zb~waMgS>|7AIMV5ebS~tF*&!1x(l(f#SjZLOm?au21%*u5qvN&MLk%ipMz4>6~VBm zQ0^$~o^prZg&Xqh7i85m^>Lhs>A}P_^&(tT$~noUN_p39zr?6N(4!tA#vlVaHdDz? za4RjVxeD2b1)lgczNY~0cm(>_!`iC0g}`QG21=+M5Nk`w@7WG5>GtY6oI!P4i>{Es z0vIT#Z8wIkWp8&C0;Flp@1>4o6+!71p?;3RHhsw*dFe3~iYeQqb01g#hHHwXzCP+o z%HKG9M}b=L9R*Jz6foPru8T=v>deqto#a=WSfspj1ES!@O`@s{?xxn2Z)_5kINxu9 z>Qgg=Izd?j)UBieu(4s)&QWPo2jh>`BG!U7f1oY#dMJP@ep$+p7JH)Uaw zS_l)!@fMRp8{p>V44oJ#G?fJku4DbYqCx3Lp~pIYZP{vgc6s z(efp*`mqiTRX2gYhsh};)CL?1iq}S{10m6~f}T*L%GK7&9Ia9d7`58U8>5cIRxPRc z-%o;hIi}e35L0_S1>EQ`RmII5+8N85rfyWl38jJ5rmKC}B?&e*6A+D(-QH6JEOmx@ zj5epr?Ckn1^^uB&+doaJX7eOxj*9gc>0sYyR7z~g5mcT_22Ux=gh{Y`|9Rxg$ifCZ zt8UVi?d%|z&gsT0s7b?ZO`2MynzHgDH4}i6brvjAUsh+YEArM_wQo5(#p~3S^5hy7>kf*G{nx5% zcfP2qs>b*a3ilWMRKGMPVGEbSu_3KC8H9j)2HEeZuhyx@8YpZuo6VL(VcLMb3VW+Z*hOd+ zMe;U03wnl0R-Jc2jYuhKj2Tui++Czrf1vQRpG4sMe;{X+qjsxdR{!1VbkJ7H+^kFQ ztMw3VQo{+GKo7d^BP*rqCu_gDf{4Pdh4G<^9a0Ng?sl611(5Txx*n5r&Mqq=27a7V z&H%_jts@3n9ps~ zApJ@|L_Y8S{2einIn4h;orv}XLqe_U$5lH41j$*=zEdk2pfdM+bzN-{r&Qt|g?k_i zS;XJw-wRcA5QD`+Q3@&VQ9v_-OV3I55A3Vg0pz2t4yRNL>9*s~SHP8~XDD`%F*iL+ z>k+QGKdR4{kF)~L5r=p?TfT-($e@S|!?&9EllP$RghMq%p|+9LN>sI@9;l5cORlqC`7 z*KZR^wjD?Q3dnT1tCrQ&p;rHU>Kkf66_%se$+Z=@70lPaXKN+-NN4e(ow6=SGLkr7 zYxPsL)oi(`{anU?Tc(DkwFT#{QjlC^O5aM_HjJ4Vqpey&n!WjrEz)Me8p)L^>)a5{ z&SJ3d7FAudV--5uM`~ykUx`;aHMM7utE9`uHospoch8PkTYuiqkp(e2P9##%Wk9lEcf5 z*Vfr3Yr4YHsbuX7a8enUtg8MpIP%A3W`c&@8LDTtCTLjZ|GRco^|6C|qlb@_ml|kr z-iG{TS{)5Cq=wppAY2ZcjSZS0VUUh*4kJ^UsW0@N+bRybF;HcG9x*wChN7)&}ortUDig!ga@z=kyG1h zx$7mL=V}Ve6Ve7_N?S34L z@PRsX8Lv&1K@+sL2<}5L%%KysWgr9#`XY^x4@jw%In}mS@|j{EPq5}r(_3v4Flt^=)!s;2B!sI!pG7Q;!H>%0-q=S%qkep}fq zV-AQlSn4Kac)TfvDL&XAa;17wJx*6)k;~2i}nK2kHe(r5OhBqZzGy$o_o7SAqXX6<&H0D3TJ?>f2hSi$Zpf>{js(K zpU~W>Pc$m(Fa?s4Zvlt14w6|A;#r?+#W2)@0;&~G!Mw+cJfvYjw3Dt#;IcLK3#~mq zAqtLvX%kB~!Cz}J_=c+YeuuT^vH1fAjj_`9DhVfTO3*H?KBu(JYE&ps!0pw?_z*rF zj*Ps?_wi%=5@Y~1KtZE_(jfT3d6iJE2FdB?wJ@vqd2KHyM?g@hmH6ue0jT2z4f1M0 zwL4IE^tcvhmHejV(i2?n=0%MPXyR|oCEGDkv668Jj)+Y8L(2l~Bfb3XAFTn*BbGL} zYHKi2IN+MLmjGh-IQfQV;tl1XU2fW}CCa?FAQr%z8TH}V)v?=UOJOJsysKeH2v?D= zzaUIJc~6^&N>B39HJunps?${0DQO~ioNMUo)DZASf`uqN1KP16aPS7QIvMnsUIW6_ zIT=)0hvVwjjLJHaNr7l%75xmrrs(%lpkA=Enx0m+y44{_C*7lPJu+CQpo|{OS`wlc zlO3X!RjRIIP_~>kbUa@}VAF5HKK8ttI;jDfUt}1VU*%dlrsQ<0DkfT|td%&>zqW24 zPQWJTQ9K3{RbPja-Iif|oPOWgXT|G(VcJL>$am_LH;}}prRbEt6N<}H^#Z`g9v0xz z$sf`g%$#TROgYi5e{D0_%Jt|^(MSwOzo*++Axq_i#(;B|rh55aww3jWjV3L3wPhJr zSUNOlrIQzcvMk+T%LqzZl`(WirVrR7tKZ5EQx&KsyxDuOC0#}xpV z5(ev-zL1VK8A3#oon#Ny>39a<7`k(~PT4Zfm5u@8Dq90v`Desgq8!%-0| zPy`wZeb7+LG1mr OX;1dupkaTC=aZFE4fnND9x36g3Mo`w~*@Zkbo4upuZ>^ z;cc%xGd`K$!{ty{T3A zWxX0YPm)XfHM*S!VR{G_1<0?}r`U3a9r$&6N7yu@oYk^6>6orrpxHAqMIDYmSJS%i#(cb{REM1s*#a3LB%j&E8 zuQ&mZtpjW9qz(htC)afktY|?JOfnsV-%_|0F&F-TmyR zrW{t%z#!wKyQT3GnqMs3Et7{^8gG^@C%n+tifGE0bK!(dkLE@r z>t1W)wfm%WG_oo%=+fDsz(I9El`^LY=WYsF2AUIre@K_oKe$Q4?W9JSd9@#lYHSoeAx`%trqN#}F!v)t;LXA}_o za5LuEV53(Bxb8!Z(dA;2o-8xL3LIfzdSGk8xBXF8BaQU(G1ieM9+=&AjPX>(o8QM7 znH3XzPcXKbgV{W=SN^!HJkdB1z}B)Cp}V%2%>x#1j^e#bYQf81CDq|)&l1?l;5|wp z5#rrSs#vd1GOAQAC*`}Rjh^Lztsc)9+vrFX9dW+KR`6^9_ZMe3;1_H&%s1wjBfn*V zu}NNAXiSF(B(wa%dMz2#%BU}!EHX6f-Xi0}3c>xl%xGP{i1bfIvZaw(LENboM(2vx z-CrSGnkn?_`?B{osYr?54k}*BlP%27_(tO39G?&Zebr%)*(wr$S zpT`Bem>zeA3-;W+o&t|A-B;uh+2r%L+%}t7S6?)?;DG>=<-s+^fbzwyPV0=36=(VW;t^Q{A`Rw*%|KR-OpRVAejoxKrGUeiE*z+efzP!h9Shp(ae)r?p!m5B5$Dxm0^6r5Sm!*--nMrY!LMMDWd<$mK=E1JZMm+5Pn)cy*jN-{v zOsb426DbB#?hC6Z2; zCg!(@6y`8;QZ1O5?V=S3<*sh1OUvd-SF{PuNRR2qClJs>EoX*JD@FGXO>G`g-J1W1 z`2oom&x&Vc+E9>+jon(oO&PxnTbm!3-^gZd&5F=mJofpGzBI* z3Qfuch`n9B<_EB*1qmtkne6YUj*Lk6h z8JXTXJSkeT$b1%uqRg%MWj5Claew-bcsXRbc?MpRa_32NHz9*nm~(|~Wn}4{SK6A3 z#fF?X7ZHEKB%ei^bhsBB?CSTD`5i8U+L3+%STcN-c@jY*!}4A>cOw!c2RRBl5&d2< zPn8*T=M6S=QiZ?QBGWgT?a%Kk!!J_(6;k7{5B=;9n%7=$Rjk^Yf_znT$G*_Ac2L7%~8|jgGYFWjVto0 z)&B2ouod&ExfSTOU~^|6Qb&%b%VBdCGJ!Z6=>)a}d}k)%YvN*;@68=_LO_|XDD&A> zVZirNj6tj_lJ!rJo{>}f{)CxO8y?i#Bs4JZ~*8g*kSV&5+*@epPiH@h>n#6Z9(Xpg_%&Z0uN??$C!x}oC zj|@;cC}Oq9^bx?c{%uh{au1-|Kkr61Xs4<7dTyB zPkKsWagp2WEX*kK`Cwxs!;|jHNGbLdIbGRvvK~bLVYfu`y4v8Ar@@CdUT*=QT;O&h zCB=naX9kdx>hh$)XE-@?`U8AXj{n0R%~Ym$uorN-Q!=~-#VMZDwDjV1S6Yz^{vJa{ zMp|leQF@`%R}B2`_aFgoSMtLh;@o4yFG}|nru$OesqTVQXF+-v_}LI{)h{xDNm8GdPzGKQ-OsNiRrAD<~-P!okvHrz<1F>n=!3D=hXU7rE22 z=k&4R%iPIuYUJ%CM<*5@z-}r~BNp&wxMd}hMUi-(-7Jl{*c>}z)Pih~0yUxAlbY;K z@i{%NWLFVjn+mIel+%ieJ?^5SWS_@V;1hXPQi|gUe5=g73T&>lgI^7oWY$#Ko60KC z;1@ZY0>75aPtqLC%1xk#%i)*p-3~e^1bK;E;C6flFDcv(_c;7JKQrCja_ib5R#{cr zDf~D1Dqx_}4j{SV&{0Vc9y*GYpCt0ivdX8tl8PUI$v^w^#-{x#7zbcm^svl_5*FXD zsGq#Dy5RiP5#xN`L1TTR#U6O_)%saK9q_NY9Nlt4br?n9^fOfqk6pyQi z$SUmx@(Y7~n}Ey`T59&561nDa73p@%Gwnrfbw*T4h%7qH8~+cPz_&{^ z!$JN{#*Q90VrY}H90x@ijG~N{`Cs$c|1q~ho~S#)gGW*aSyfsqB8&G@S~LF>J=3CQ zb}osGbok6f#}f~-nCmkJDYC^T(MTT6hNGL^Hot3^s&4$&+w9Gy5*Es2n^`EV8^X|HI4S_7GG>xcXUIRDn*;K~>ZAi{ zY4ERreWSf10Bo#INx01258pWqY%9Xc1yi9AXlO0!g7nMe2_LsN8H0aGsS2DDRh2?T zJf;+~*}NsFe=o<(=#rX|a8N2yE?KOH&}k_d(+)l|Y`Q{4g~4pP%|jkm|NrQ}Hp(XD zlY}xalO|U`p@z%Am1?!{w2~+kK*;|QKrNZJ5>Ar+%hx>UKHLZ-%#jF1mO8Uq3H*hf z(*jPnzM%$^mt6j~8s{Jv=JkR;VO7%lAwO;Xw=jYnM8O>6Rk@^wBZGUI$k?@NlmpM5 zCC*4FsUdqLLN>m=Sglcx^ndw0_+qD4-E!+9Y0%k4+f?{$MXZ4@;C|$@OM?=|a)`$H z4~EN!{dyy!t{p84dxcZP=HGrg zqOPq^zCm(ZT{!FX&q1|j*nMm5)i$$D*S3i>!b|GNtu9A2=+qZ3M}5bAouZ%{<90+> zF4L*R8P!T+WL^U}Pf}(z|1}uTR0ZF6@SuXIxV3G?U0fdymi%iTSsN;D#Q#xT`T#X? oW|fj?d3Obv+^Jn^bDRu`e(Q(Krv7bVx~#CUih#ZOEj9N40a=BB+W-In delta 18437 zcmZvE2Y6LQ^Y_`^Q`5_Z1OhRkqyPy72n3K4klu?B5>g;iLNC%(5CTZA7g&mP1W{?` zg+)|AP*GG+cxi$ls3@plq1SI_PHxcu`#lflp0Z_j>ThQEIO)yGlm4i*Aj)WCEVt-NaJ0vMo(F00qMbLO(pyEL37M=YB<^0K$P;-*0i9kF3p-hCj5jo) z66*|qtFSouTjAYqPAL{#H?h``OH4Y`^SWB*Kf@&eB=& zwe0UP;=QwGkI`Ya@i0g;mI{AU7ELvZj5*2JEOvtJ@wS@#CtRkCVd38P^PXVmu`p2H z{1353%68M^t-0_aT<$CnkDD&C=rOj=_HGPfe`-RjrfJpqHCB&%L(|H8D=*20)(d~+ z;ohZ7F5=}OkuERBIZ@t_r4f1}P$St~YuUn1L98nf)X{rwX+AU?@rhl^+jm*zq8U;- zaGx?1=D4*w-uu$JGrrq?>#xHlQ8HjdOCKdghc?!x>wCge6tD@ei_$l@z(<|;GmrQ6 zH-CjoAOkM9oM8LC?2*Si^{q$na71&%Bd?A$lH}T5tepHllZAP|Ul&yLyElDv z_=e?MX&|o%_io+x{nLS}6&>B)EzoULROds1t%&CK`;d!7WZwR-iiasm|A=9WP(PTk zUKj4SuXd*>Xw$^whVe7wcRaVE0x4YjZ@H7II%l*F3S0Ys2{>?ln& z5mwgibewma=%N~FtiT+_N#$5ZseiB)>A&_FtsG?vcW!cX$TbdR^v*?u@Eh}gUO zEVk~P2$$Qx{SGdPL+j4vLN^A2*&w&vwK73nyvF&X*fI`g|M`3*&0c|p7lmJlhifmp z9A4Dn;wmB*n>_g*S6aj_*TYBY9dwVk;}0KL^R%VfYHgD@;KxXBl^^qC11-Y|v^fiL ze|b{zZ*uW(^`dP*b_{_Lkdxk9f5m&#Zv76ov7hSUMfGk6m(17e&NjS8jVp@2=SRMGaqb%=SA#MC45b?srNXDnt}j|vYzN1o&zsD(wT0@$rX%!j$?w($Z?!~ z1J@~gZI8+kY_EYWVNg}gVIepKN?mn=$$aCfzW(e#)a;z_sN_)Q8YK{=T7(tfz_cnS zobAItNV=?1iao%=K#;0&Y3BF_lf5F?n^NDw%FBw8EX^B}P{(^jY1;uN4pszsbj2+fxh&pxI3K+9!XB13ye z%~Y+bFj8#P5}8|-t@r15HLiW2v7_1tpd&lv$4M*?#{`X4X~~RuiXxJk)!8O+J@iLQtd{0# zD_*`*hk+@RHb~is#j9C$*-G*oz`)J*(p_P|=$qR&rEB0#xI9@_k5}L`Uq|V{(mkqP zYj}&3qYOH3yn$7tiA<1()&15ZhpvI9$yb2EWy#U%C5Vk?5l8GfPh zHlnTeh4uxo=Y3fzn+=6Vs4sF;HrvR-`-Q|fZ5MV5kZp2UH`jm2cXLQGsBdarE?bBc zfe<~iWp5NLP1?N=yN8d#Y0DS#*?PUJrgfEmFR<2Dc9vNHK|yA{z)~1jb6#NMgd?;s zHU1k2${YLJe))X?>yCsVN7@YZE!iV;2eEacFm$NA)LxI24{x$CxqSh%WK3fx*=-NA zy#UIS(vOF;igI#qR!Qy%)yv7R_pwO%&I0BqpEPz1r4MG|KIy8eLzn?K3glIm*7B+D zQF+7IfA9t+u{mLcD;2bzXGgMt;#T3R*C@6hZqWCt>Z3_G3MAzlCS2u?W&2Tlv6;F@ z)|$Y;iz!e)YJfgU<-W?AV1u2dtV-(Rsf=tMDDkLA(^wKtL~lAyXNT|x1E-u(DB@-N znd~w=Kvhu9W)+*@eh^9T*=((DLgXr=?UB|3R+)>6GI;?j0BejjV-2U7%_N)6qSV<1 ztPPh2t%h5><8#B8I-%Yt?6G_WLny~|XzOqtplm3aeO zhx3DZdSs(Fka4a-b=*X1PaZCRGusW!Blah4B`Ff8Rm*Me4Qi0e+s?j379+nmH`#@v zR6rg&Wz|`+Z*c^OvZ@JqAe<72@yPD`*nNDzpM8waQkdgWRX$?mrD>t94!Tp2sU9zb zrh14?L!$;6d1ReWVd<^DVic=TM)i);j1*&3_Av%@z_7U9$WIuE^BeXPJRom079R{b z&M<(}gH!N8%{am4;4;XAC!A#_4SXu%tM>erL8oG}yZ+AZ!Zih^yz59F2|n@?tY*Ru_BrP~NwxZmeZc&6i23|5yQNiP zGBm*{CBMJP5NLrf^2m%^Y_&+6oGM$@VzIJYBPaGBqgIpUTR743t?Qt| z_ict)gILh(4tpb5tOkIQs_{{*2mBzstSseSHpHb$Mc#9TOAM@YpM{kyqt^qr5piiB z=#sOjwqCZ9wn>A3A85z5U$n=}!>X_*;IIa>LK*Xr4F@}mlnWk$ne2VY+6X_Py{PeX zY`yj(4A4(5XMA+A&Q{>O0#1XPDKi9rQ!k92Twm@?Q85hsUe5vfMQ~G?lUNL{5m}Qv>-&AWg5PdD(e>0r)yi?kLA+A}Q( zvg}cX|4~f7%&N+_J+nyLq8-pqYuCXAgs>s6UhF0+9+O)O5$U5oEY)iqZHl<89|}GV=ex! zKmP#0OpwNUX+g46DxZ%O0S{$lEO4o89sUe55(1HIUzZmln0c-!G@%Jwl%^{^jb9@V zi0+U#;tg?RP;`&#)|k&G(I8ey3$tOQrZkcZyxKS8ANht|Tp#9X-JDbOBQ6)T;H8L0 zM4rIqYTg~ktIJ7scxY&1AioF#+MtPg>?`=$03!0rk6ZH*Ff&40)w&HQ4@M3y6~>mc z+QRxlM48aGd&J~slQSZn6g$dk$jfM2cu0B3L(M2Tv7u8J`2uqWDNXkW8}wq| zQTYSBJGYmY``#7N^6TTmmfuJ4F!`h{$9xLIrh43tdvGy`k0R^AgzYeDly1FV5JsH~@7aAgNZp#1ysl#-D<_2)Clk8;YZe zL*2pH1jD0x4da(dx@a>ezk-F09}VUSqoZ4CH--Zk#YL-}v3!$Wfjq%&mI%7KkbT1L z@g&|`o5z>%&-o*f0zTrPjOoN`BW{;PsZN4C@+uFMQ>r-e^6*5sI{hk7)N6_YCf8)M zWOefuFg00kA|D`6PUJ`B7(>LH%%2=das{3&Z?+P3c2DBJqH0s@dNP%}_y^gN^jh&v zW6;K4(?PAF*`L4Zp4~G!r3Vynl4rR$)IV!Bf0a0aaP0aVUfIVM895mi-(em{=t<(p zn$M?}u*Qlo;$_(-95E^aw#20_6DhuBN#G5rnC8zLX<#D5`5JzdKNK;dofs|_2_?>h zImK|*Xf+?t^h$=)QnIeRRLE05tL?35IL+y)3=HRve7uGq0!m23Pagx@dbpNT+{7oU z{Cb{%l#?~|+`zB;)+G0Q3Co!920sXIam6wMh5FJaejFYUwRxK%jKK{CU|D84kCQ=L zXw{@0QE%}$d`r>3?m;lySKj9Nz9;JMcigB$%uL+oGJzDf*LFSy*hTJm$-6LW*XhUj!(hBsz=HDtZt9oZpKlSN+FWTJ;FV;O z$EXft){+Za8UAY70p1q0ikx2LPM|LO6QYh(Ch1dGu?VbX9R{r7qnqypc(v?vjwzWh zKzs?;B#68(_&Pn9KhSh-xW*f3!`Z7MPlkNW^JMCAo(s)LoNpfIm;ce7Ku9?mlf%NI zUmVbPZ^!S*Ut)f;R){5+AysbYm4phRjnW zy-R=CrKB@TR93T2^Rap$7vP!OYg&8$kY&oC^SrTa@g46)au1igzvCy}dD7)q7kG6j zuuu)`dDaUE%3`hH?{o6u1x^_n`mcT$`BGFaf?6lfazLAxXpBzbVw5~^2YkSR%ltKH zL3*G01Fr+k6(};MU9Jd*T!9$9|6P$NzYi0JobwZyPw*Aq`XBERu3}c9GJocTZYhoT zzse(U6!LYaPlNVkUxP%lc&_YUd88lQP@tT7i~kMRByyT>-)-*2mk3Mx-r>K{IGFsV zUI%5GagX~75OU0Yeg#`1#2x;CpT?Gi-LoE%BD4FDB^5lTiOKO4JmJf5Vs_eUE8F`C ztOtzf3&xt~FV+R}b0C4Mng+D3R}!u0a; zVUXwnFTid+YCy0!j~Y!zk`X5E07rq}6+tl>Nl#FkB>2&#x|G!U7a#E zdjd&nI^2b9(#tBny5Ou)fu^;nh$fU6&SiOpl{*=}|O5{#SN zZ9^|4=or5ey&q3QA*vZ zDI7+ZENFLaK_H3Pmk|S~a9pZDC*eM*gZy^=Yw1{l^&HWZbEl`Q(S;?;hxNof0LioK ziwVpGwK}i;0z&>)1~e9Jecc8%7E6{k5?Q_>hcyzb!dfA35K6*3i*?^9Q`xcVx~JP5=Ds6Q5%v$-)SOxB4LPG%A|``!Yu42SEoDA z`yP7hi}+Fig7r_zq#FI!`DWK9wUTQRMPnRJ&S~mImUYveUYgcRn-AJ@hgD@cY!TGI z?#r^R^rjd){M3NfVi_kG<6UqnnN^V4hH-3ZJ3(18(4hR<3k;$P*onwmb?zvx6O=## z2lTWdAU`j@K-M6?dv+HDB@vSJ?E%(?R72F<+@UAfgo`=HbOxEs%%o2EP7TNs>#!lY zmGaqwz%&Kzj84#WNRGIKZ*j~%x#9@;6T&S`27;dA_(*2gX_$gS3V{)_!#ppI5tu21X77>CXDq<_gx%%Uv`krUoMDjtN({RrSdtf|4Cn#LQ`~Jpof>Mjntf=QVK{NkSJfV7geY`}2E{ z{rsY_`(#V!<&C)lgE6_kKJx@7Y-9-o=8I)81G9=k6NFpIT_BnOS13C6TqqD(kjQ#1 z5;t)Kuh_hwtcn^dt5xK87N(by=1$QbI);uwFuDpBFmI>W z2M=h5ymy7f#ryz^xu_Q!Q!Kfo2{@LBB1jFvtcpZ=4$gAeE^%78dMq>F!*Jrdg513% zTe6qF?~7YL#g{`r6j&dF0-i@M*$p}~YoGWOAM6Kt@&HXRUE`S|6AG0&Onp-?)Te~X z@ALFH`AQ3dV5Bdnqo8Zet2~T{z9r{>3Z>Guw7FJCFD=)$aQsNV9bkYqGV>q_4!U{d z>0PkQUk-{lVF}>X))HB$%9a2KGWZh##SfMKsh}+eQn$!m0^-DBu?dO|D&-qNX(BF5 zK7U*wrY4QaKOxoygn{SsIWK8XwcC-)^43_GAmwRMAUl;*YC1i%lU4RKdox%18A0V@ zV*07CU{!<9iqkMI8GVm$1tdkP^Et3%-&|?m3EDdW8dR$buInIYlY3EIC|OXKpJAwR zm&7;4!pXi&W`kfx_4`2_0licySO2ll#aE!iOV^MCZnPXQ0npUUU&YIidS+?G9yynJ z*xE&|nx-#}+%Mjw!2(shWJ zkFSeO^2`m9;MT2Xbup4ncAcEiv3fk2YkPP90T>6>>re3tsFzzz54k01M{Z{stFmf- z8;k>4LY1mJV2Ze)d%`V4VZC(hPq3f~_knZ3zz55;gL15)|77t^ofHRftL zT>l%cL9d~tb=ry#(|fXRhk{6rXq`%*w6Hw^P}iRDj6Ri0 zBT3Szq&F?&C(4Q{qCO|ADjy_a0K` z1>jq|c>N;jRz5h3CW$VuVB;Hh?{o@Ylq2L;)o&no32oI50wQKs*Y{9Al1MBp=9Ls3 zB;q2OQ}|$3P5nr*k`Ab)FAwA);HQE$7ODmF$MRuqoiY{--~Cc`#KweO%ftdt=ho3j zP@>bjl0F$?*+BWYj!wt}jiY~E{V1^ti|wr&>cu;b9+}xl2fU!bV}6hCDegKApzuGq zx$fEv?8w$bnYGYQLIYfzx9+=8%WtV~z#f!xHf|5t7>0pl-gOsPlh5y{L;P~ooG$uk z=Fgu%47q_Zxdl5v~#<=OvG%tUk`l+MnBvVe+1i@jy6PtJokNV2t3G* zC~PyOZKTuJPIN|KPFgAiKs7hZ%gpDTkuV&o(c&(8yoc`rkhiPC$Qg+~nANP3oYY*e zC>uA29QErrAo`u}pohtqn>!JDp=U}P`OQ|?VS98>_{+^(MX(I(0FB??D*QR$ufVpb zNQ0{uv;ufZt=5&xHO$PPV`+!Km%ei|Si1AkS)7OiHDH$@Oty9N0Y6%)JN0ExlhW+(+ z?n|nPCdoB{fEU&t)Pv>G+rkekqX;&RYN9i{0zXGVGW(E)xh@Nj&e4NQivLUJ>2ZN2 z6{h@rtox&!$NnimNdJSxLzl?w19jN)ydstb*i|XhgeJj{?cR69GUWKzWYRgc)KcG$pA-|l@ zDpmSOohtQcv?_O$J`Bt1WWj&G1QzT}&?yHb>2`S;47Sxo9k=ypZ#45&eKW#@s?wMQ zhI(FtFDV2|EGMr|(Li30E^n_EqDvrMcP{s5}x_V?2EdejG3>)ZZq}o zGhbh=7nYwgRIUp#V&wI?dP`h0`k2=9^bNu@Wk`usv6eyLzUo|k&F%&IHsELvwPTUq z0Rl?5-HY{Wh=YDzsz))uhZ_H{_K=s?wgDCEWbi@J7N;3eGL0qowQ_>w#U@ZJS=)*# zLwNj2-dw83dH8%y|M1T-$8RqkIBNLFg8V^!_4nke`JfoIJp{TKCL0gZ-7z8)C)b)X z5^W~@UzMsTxw5rW6==q!?SCun(UR=x=y9<1X9wwykyw~8Ww<;u3mAH1gl@`x&AFfa zeVLwy%SN58w_JZ?_v^Z@!-m-V8dPB~Xz#=7w#x6;>w|C}tPS+qpl_5vtk5a5MSz;K zQeQ7GEZ2MZxYv4xzGC-A*DOh7)!xuCpP?#F<4vv#lGL*|>z|Tppg8JoAzeebs9L|J zb9@1~3^u3U*1tr?CvDSDJ3!h#jep0EY5Rb*9eQEllvm}48^M*8?&{!52wkOaLl^lz zWTKu=+b3-Gxkz)7?0pcL9>^8Zte(8s)rpnI-qW+88y2+Xl5G$P`tQ{*;W(I2_j+G{ zGr9unps|r`DqG3`Ov0Y!Z&0d?1n}}4&*Z~Z+y`JaUbA-t}qPi`}{x@_BtR0Uuyl)+94 zP{IH4O%ZG5z9Yb$Uphem*>R2gnO&NvKi9sweC7-CHpp2u>kWBQJ){tqV%8C9!`x_KeVMPN^81MxkFG=2y`s|*6k2!A&-x<30}Aw{Dy|iI-}Teris3=RIxk$wkwEN(ehrSo6|O=BsP z+Fmznt7`R~Gfe8X@k8;tAJ_&KLKK`ntcj~cUi9!cb|PLv#HPxJ8ng$AX_aaiW{{w% z7?KlixR@3zy5-6kF0{aK)S#?E*os(`9c4^MW|C#xEoTrmfn?1iH(F2(%!oFqoPZ~; za-T7RsIb@jq)1k)VvYVhNJmh0k(Xn`;i8HRt?fjy7GMjFWtqwbfku#+EUav-6bVyd zH>0gnUcS)9DFZX2FLm~Tc3iz7!59aJ5RKwht6z@a$`ceN`RfW)gBb3>6dtSnb) zXkfNONFt??fys!^Ms3xnv9TNZ?0aI#&glj~HBelr88@Y#G>X*#HW%kMGhPgaTB)wZ zX<8ir8V=N5mZ5je2$}Spk%^OFJXrahfk+yTc#Oa^PVk?++}wB$Un54V-@?GsLCK8j zg_g#Lm_L!MQ#%;6r$}1Rr=u|jb&7QPw{ET)QQ!#dZd6CQAzSvy6J?+hGPZ|NNNrK& z0<#S|wLxv0<&a|00iu4n#(3(7M>C95;O)!y#`VfS&%-utLLUPQiEj3lm2YgKoGMHj z1B@7z_@nWJE$n2gY6FeIOisEDrnQ%T)E9$bdDzbj>$6XTnY9^WVETfa4yx@?gVI-u zo~NPaF6Rt0Mu+ktnxRE$qKWp9oz5kR*ngBMb#&rH?cQqEBxG zT2f!WJ<>>ohrR>H>+t%p<&kSf8GVBJ5r`4vHAs+G!4DqRE|3Ra1gc$?P)$Y~gqbNg z4<2K@f#YIXxtR`=#@%bBklQjD1lEi-2sn4=Q}>^0lS$2sv3 z8P*TtHh z<|QiYx;Z>6h%WXWS46CA>N*S}z0H)jLrfgP;ff@&X2lpzOrly8%B4V6K{J42j{ z-(nPDI!tU!AN zg-@t&xz7zs=s^S?8TlUYDf0;FEfJpih0zb1D#)3eVi!b6D)Oj-;m^(KBY{_H;xVHo zKA{zy{n}kD-Gm)CV(<+WaWYOA%K!){Ff@#euC*sza92XL!|oT2ZF<>ocXPiRAIK-c z*3dirAV0&e0}?2!VtzDi4nT{JZUxKvR}52iy<&WX01MXRQHfXo5q4T%Gbq)7T>&Tt zo;Bjsj9-l$q8*ud|2Kn*D8$~F>#n~bT}(R)tD11b$OIK4P5t$zQ3d)DN2}a6%HtTc zz^psQM>HUo`D@%WY`h_q+wQ*0Sz1{B18~pqW^!dXn0Mx3@l+T9gB}}rmIP;!seggz ze(8xZ4mFp=V;Uy$k2I&gX%coKFPUSR8}u^#F_?L%#_IBB@Dr*PR+ttN35Xy?E?i*7 z$>bo@Pqqs*F%c$szckPU7*1hgOOSa99%HbRYl2POz+~{fMZ#)EL5Pl5hnVPMi36=d zO;<5-p~J#V3e@O()S_^+FPRz|l7AT!1GP#iYvKVinknr*oT``=Ws;7Ng+-PE3kxWZ zw7AC;W1>yM#l)YU&zSCs8Vn_;@vv99$|jU>TnWd=nNK}*X1w_qVoTyrUW(~r3Ghp5 zQcdu<@PjNMQT)bD97RmgBTHVJ5M zSKq{nhHD%RTrx?0ZZ|2W3R9OVO-<4es-$E!Gv_0FNL?PcGRZB_@X2jV3Urtgs$OkP z7lI;x?e;{Xg7Y4P{i$zuM{@?~IYF&^oy`_RCbGypA`;ZgUCm$d2{wgIFB9D;g@y^; z&0}y)Gv@U$U%^I1*dG~Y9Ny4&ebvlj!VutRWtm>&Dup3$A6R!{UlW`sO+c#EZ%*NB4%72?Zu%4btA~18ISzKfMN-^L_NGCxcB25qRi9RU2xOFLRkX%v?eO zMZYV?n+P&#+G;PktzhTCqq1H$SDU1W%^w5l{g-?6rOfhB2Vx4Iw9xHQCN{<^2 zwvq9gNpKtvzbnuJY6>NCk8)MR;!%=v0rZMoTB9$s#4mZw&j2dlvs0I=IP1^#k>`1xCV zA2w6-*$Q@?`|+lHJm1N$idfjfUm!jM1J7h@WtG;n#mAb;{jZyK*$G+AG-|7=tIU~9 z{{&AV*$++E zccWa|?l*VCe&ZUbxW;H&44Vy~BFV|0m>Yd_)%(<}kAsnaTYbl@COh^tp(uLTtbv}c zH}F531qJe{;6O`jT50w=zaR&EX1))72nnWr?izo|0B~-yoUF0UtVFv_bpBCZIARi{ zf&im(zc81g>XNZKCr!!=i5ay{xi_Su*{979Nr_h1Cq{qbD6|S?RKLX)9Vb39}F(6E^4)ZsouT3Yxs= z>#W*DSpDc>8EZ_*!*)^D=#sgvl()za(#S757R>dGRUO%jPt;eHEXol87dMfUI4>tV zmA0zLDRI_%Uz0ZR)^OBXYJV}sYEv>15ZV(}m)?9?&&nuy)4G90h1H!6pVB4UHMVvI z^DI!v6B@q>e@C)IkHW&@@z?LewW!h1N{nTXM2cYfe+9Cc(yCYI?M|B-V zgIy2pM0NXhYdfC1Af_K#XVE!kx@oh)8dfsPg*U8rGz&_g(`IXkPhH`NJWEi1+bkD+ zqwqU!f2ynM+77FUk37}!T??_TyXFOz;cyVHSv@SW?iaVItjg`Sx)!%dmkXkeVtxB# zzvZY0%KFpC7a6eySW$1ERomBEZQuV7K?fYP2#ArQHE9leE~K|Oj{N!~3)Jn91t-ah zS04YV)f&c8(3&cU#DSo1tWL#)&z9GYTeDo!(_!b!X9h5w4d*Sbh1hxFNoyGXLIOle zNun)Tro4UjslcB;z4z_^=c#&l&bn11meQ9jLYw3a>4V<|tL*A%IAL<*WovfvBBY65 z_Pp|gH7!W2(>SZD!6D14@Z$|__pvr(MJs?)F5V&oD4mwcUEf(y!SSesCE zPp58h%OV#~Phb!miWx?N+XDdoQx?>HZnQ+hdm3!8;f3;hk zs8hSL^1^*J%3p7uzhd4--lovtr2KbPD`<|WspsyQ%NB&*xrNdr3|WCDI3uYVYU{f=~_}r zW57ooLT`k<1PP(oS)&YmR$Saq_Rv9~L(AIV;R8$)a--}$;3A2mRV%wK)A94Po6&Zh z91v$;#&IyZ=EU2h&_;=}pOS2{DH2h;>h?*b455b{Ump7PN~IZy?)G&cuPp8-Q=@ID zxYV`1L=o0MQ|h@3rdZsnzRM`eJM$XYACrLb6NGx{?kKqW%^jM;O)=B^G_w!;Ze+ve zcFA|K9b4F#q9G6D;zl@aOM5j|q#4kbJqgCUkcC*c7*kz3uYtk6}&~+SkTnkBcvx^s`A%T(GBI5jkLtW6;0sV$|AcXa^;yt%_m)Sl38>PTeJ z%Qog53er#)eqmD4Zlc=-6+}jk0ODu9YL5#RVrYz7kFTz4*!OoD4W&n1k^Zv&nho-yB^U1Ce|OI)TS-QKia@p9l&`w~h7v3jnw z_aVE<^i!6*mPXdzc7?0Bm=MTuGeI;nSK8#aNRdv!QC69;%KjERqjIEO1C9(?ZC}8l zkzcuM?0rZC-bgtdN(C8j*cXe9y6q-+bkc;s*CWq2+bz)k@QV!f<5s&Hk%hZ$?Y7%r z(G9XPX{T#KB&@OPV9vqs;;63Q$SwMZ6RG+hxfcc5unV;CtEt@gZ3R@3L#XnRO`x8* z*!-Y-LykD_kR5`d4hpRZ*q_Fc9iQ5-!vkV<-eLO}oBu7i2o*7^(`WV$9;59FGMp%z z7YxlGJo<&c+L?ddvH%tHg}noosbC|fKQ4;gO}i8J6kIzoGBO3Y;rFebjIW7#?atY| z0{L3VhAL}XWj0?}3)3Uyob#k$6l#{e0vTDsclOg1V8I2O4kYvkEz6hIMf*h<4cDO- zTy(`q=9zZMu2jP4;g{`_(vgH_$#Ov{7A5nN?TYHj5B52`Sa{cevkANqxqn{)8MMAm zPM?%L}X+3Sw| zUvS+D!VT4gNC>d3B=6d9${|tZ7SP zj?s{3K-VZ4wjrMb$LD5G3ALsr*AO@?SdQ% zM5G(yUD#%r8SMOwbA&ip&7h&yMmQ!bBj5Hozq!vPJZ~nimvVL$_mR*iUEVG2c#DTuFg)_!#q#aTIKG0t>{!;pEjdDI z{i2-yXaZ27k&B~Y!^4hsmbxP}k+6R?1Fp%uUasJvWs##8TG2s8p-z$Iq0?8DoFaG$ zRYA&De}r#rUyXClVtcH!4T*>46nB<|3Ccb{8Xo*JLx7-dPblu)7goJ`!v|o k`@frcGvSq0yp<=L*Kuk?JLCo&cY~pUOIUmyQrGeRKVt&v#Q*>R diff --git a/README b/README index c12ec30..69f732c 100644 --- a/README +++ b/README @@ -29,15 +29,13 @@ or, if you are still using Debian stable (lenny): for Ubuntu this would be: - sudo aptitude install libboost-graph-dev libboost-regex-dev libboost-serialization-dev libboost1.46-dev libboost-filesystem1.46-dev freeglut3-dev libxmu-dev libxi-dev + sudo apt-get install libboost-all-dev libcv-dev freeglut3-dev libxmu-dev libxi-dev SuSE users please use yast2 for installing the missing packages +Additionally you need some external tools (exemplarily for Ubuntu): -Additionally for CAD matching support the following dependencies must -be satisfied (exemplarily for Ubuntu): - - sudo aptitude install libboost-program-options-dev + sudo apt-get install imagemagick ffmpeg libx264-120 ------------------------------------------------------------------- diff --git a/include/scanio/scan_io_uosr.h b/include/scanio/scan_io_uosr.h new file mode 100644 index 0000000..462b788 --- /dev/null +++ b/include/scanio/scan_io_uosr.h @@ -0,0 +1,29 @@ +/** + * @file scan_io_uosr.h + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * @author Billy Okal + */ + +#ifndef __SCAN_IO_UOSR_H__ +#define __SCAN_IO_UOSR_H__ + +#include "scan_io.h" + + + +/** + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * + * The compiled class is available as shared object file + */ +class ScanIO_uosr : public ScanIO { +public: + virtual std::list readDirectory(const char* dir_path, unsigned int start, unsigned int end); + virtual void readPose(const char* dir_path, const char* identifier, double* pose); + virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/include/slam6d/io_types.h b/include/slam6d/io_types.h index 27fa470..dc95bb1 100644 --- a/include/slam6d/io_types.h +++ b/include/slam6d/io_types.h @@ -2,7 +2,7 @@ * @file * @brief Scan types and mapping functions. * - * @author Thomas Escher + * @author Thomas Escher, Billy Okal */ #ifndef IO_TYPES_H @@ -10,7 +10,7 @@ //! IO types for file formats, distinguishing the use of ScanIOs enum IOType { - UOS, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES + UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES }; //! Data channels in the scans diff --git a/src/scanio/CMakeLists.txt b/src/scanio/CMakeLists.txt index e7d1d27..6a897c4 100644 --- a/src/scanio/CMakeLists.txt +++ b/src/scanio/CMakeLists.txt @@ -5,7 +5,7 @@ else(WIN32) endif(WIN32) set(SCANIO_LIBNAMES - uos uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne + uos uosr uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne ) if(WITH_RIVLIB) diff --git a/src/scanio/scan_io_uosr.cc b/src/scanio/scan_io_uosr.cc new file mode 100644 index 0000000..088c27a --- /dev/null +++ b/src/scanio/scan_io_uosr.cc @@ -0,0 +1,171 @@ +/* + * scan_io_uosr implementation + * + * Copyright (C) Billy Okal + * + * Released under the GPL version 3. + * + */ + + +/** + * @file scan_io_uosr.cc + * @brief IO of a 3D scan in uos file format plus a + * reflectance/intensity/temperature value + * @author Billy Okal. Jacobs University Bremen, Germany. + */ + +#include "scanio/scan_io_uosr.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + + + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".3d" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + + + +std::list ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end) +{ + std::list identifiers; + for(unsigned int i = start; i <= end; ++i) { + // identifier is /d/d/d (000-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.3d) and pose (.pose) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + path pose(dir_path); + pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if(!exists(data) || !exists(pose)) + break; + identifiers.push_back(identifier); + } + return identifiers; +} + +void ScanIO_uosr::readPose(const char* dir_path, const char* identifier, double* pose) +{ + unsigned int i; + + path pose_path(dir_path); + pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + if(!exists(pose_path)) + throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]"); + + // open pose file + ifstream pose_file(pose_path); + + // if the file is open, read contents + if(pose_file.good()) { + // read 6 plain doubles + for(i = 0; i < 6; ++i) pose_file >> pose[i]; + pose_file.close(); + + // convert angles from deg to rad + for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]); + } else { + throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]"); + } +} + +bool ScanIO_uosr::supports(IODataType type) +{ + return !!(type & ( DATA_REFLECTANCE | DATA_XYZ )); +} + +void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation) +{ + unsigned int i; + + // error handling + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]"); + + if(xyz != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // overread the first line ignoring the header information + char dummy[255]; + data_file.getline(dummy, 255); + + // read points and reflectance/intensity/temperature value + double point[3]; + float reflection; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; + data_file >> reflection; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter then insert point and reflectance + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + reflectance->push_back(reflection); + } + } + data_file.close(); + } +} + + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) ScanIO* create() +#else +extern "C" ScanIO* create() +#endif +{ + return new ScanIO_uosr; +} + + +/** + * class factory for object construction + * + * @return Pointer to new object + */ +#ifdef _MSC_VER +extern "C" __declspec(dllexport) void destroy(ScanIO *sio) +#else +extern "C" void destroy(ScanIO *sio) +#endif +{ + delete sio; +} + +#ifdef _MSC_VER +BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) +{ + return TRUE; +} +#endif + diff --git a/src/scanserver/CMakeLists.txt b/src/scanserver/CMakeLists.txt index 5d7dd30..9a15b60 100644 --- a/src/scanserver/CMakeLists.txt +++ b/src/scanserver/CMakeLists.txt @@ -16,10 +16,10 @@ add_library(scanclient STATIC ${CLIENT_SRCS}) # boost::interprocess set(CLIENT_LIBS ${Boost_LIBRARIES} pointfilter) -if(UNIX) - # boost::interprocess uses pthread, requiring librt +if(UNIX AND NOT APPLE) + # boost::interprocess uses pthread, requiring librt set(CLIENT_LIBS ${CLIENT_LIBS} rt) -endif(UNIX) +endif(UNIX AND NOT APPLE) target_link_libraries(scanclient ${CLIENT_LIBS}) diff --git a/src/show/show_common.cc b/src/show/show_common.cc index d5a48c3..775fa92 100644 --- a/src/show/show_common.cc +++ b/src/show/show_common.cc @@ -206,8 +206,8 @@ vector < vector > MetaAlgoType; */ int START_X = 0; int START_Y = 0; -int START_WIDTH = 720; -int START_HEIGHT = 576; +int START_WIDTH = 960; +int START_HEIGHT = 540; GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio bool advanced_controls = false; diff --git a/src/show/viewcull.cc b/src/show/viewcull.cc index bda709e..bc0966a 100644 --- a/src/show/viewcull.cc +++ b/src/show/viewcull.cc @@ -18,8 +18,14 @@ #include #include +#ifdef __APPLE__ +#include +#include +#else #include #include +#endif + #include "slam6d/globals.icc" /** The 6 planes of the viewing frustum */ diff --git a/src/slam6d/fbr/feature_based_registration.cc b/src/slam6d/fbr/feature_based_registration.cc index 2077dc1..47c23c4 100644 --- a/src/slam6d/fbr/feature_based_registration.cc +++ b/src/slam6d/fbr/feature_based_registration.cc @@ -58,13 +58,23 @@ void usage(int argc, char** argv){ printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n"); printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n"); printf("\t\t-r registration \t registration method [ALL|ransac]\n"); - printf("\t\t-V verbose \t\t level verboseness\n"); - printf("\t\t-O outDir \t\t level output directory if not stated same as input\n"); + printf("\t\t-V verbose \t\t level of verboseness\n"); + printf("\t\t-O outDir \t\t output directory if not stated same as input\n"); + printf("\n"); + printf("\tExamples:\n"); + printf("\tUsing Bremen City dataset:\n"); + printf("\tLoading scan000.txt and scan001.txt:\n"); + printf("\t\t %s ~/dir/to/bremen_city 0 1\n", argv[0]); + printf("\tLoading scan005.txt and scan006.txt and output panorma images and feature images and match images in ~/dir/to/bremen_city/out dir:\n"); + printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 5 6 \n", argv[0]); + printf("\tLoading scan010.txt and scan011.txt using Mercator projection and SURF feature detector and SIFT descriptor:\n"); + printf("\t\t %s -p MERCATOR -f SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 10 11 \n", argv[0]); printf("\n"); exit(1); } void parssArgs(int argc, char** argv, information& info){ + cout<<"parssArgs"<= 1) informationDescription(info); scan_cv fScan (info.dir, info.fScanNumber, info.sFormat); diff --git a/src/slam6d/io_types.cc b/src/slam6d/io_types.cc index d2c0a35..03f6817 100644 --- a/src/slam6d/io_types.cc +++ b/src/slam6d/io_types.cc @@ -7,6 +7,7 @@ * */ + #include "slam6d/io_types.h" #include "slam6d/globals.icc" @@ -24,6 +25,7 @@ IOType formatname_to_io_type(const char * string) { if (strcasecmp(string, "uos") == 0) return UOS; + else if (strcasecmp(string, "uosr") == 0) return UOSR; else if (strcasecmp(string, "uos_map") == 0) return UOS_MAP; else if (strcasecmp(string, "uos_frames") == 0) return UOS_FRAMES; else if (strcasecmp(string, "uos_map_frames") == 0) return UOS_MAP_FRAMES; @@ -68,6 +70,8 @@ const char * io_type_to_libname(IOType type) switch (type) { case UOS: return "scan_io_uos"; + case UOSR: + return "scan_io_uosr"; case UOS_MAP: return "scan_io_uos_map"; case UOS_FRAMES: