updated svn to 707
josch
12 years ago
parent
0281b65175
commit
83b943ab2b
@ -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)
|
||||
|
||||
|
||||
@ -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).
|
||||
File diff suppressed because it is too large
Load Diff
@ -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<std::string> 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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
|
||||
virtual bool supports(IODataType type);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -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 <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef __APPLE__
|
||||
#include <GLUT/glut.h>
|
||||
#include <OpenGL/glu.h>
|
||||
#else
|
||||
#include <GL/glut.h>
|
||||
#include <GL/glu.h>
|
||||
#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 */
|
||||
@ -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 <iostream>
|
||||
using std::cout;
|
||||
using std::cerr;
|
||||
using std::endl;
|
||||
#include <vector>
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#include <windows.h>
|
||||
#endif
|
||||
|
||||
#include <boost/filesystem/operations.hpp>
|
||||
#include <boost/filesystem/fstream.hpp>
|
||||
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<std::string> ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
|
||||
{
|
||||
std::list<std::string> 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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* 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
|
||||
|
||||
@ -0,0 +1,413 @@
|
||||
/*
|
||||
* feature_based_registration implementation
|
||||
*
|
||||
* Copyright (C) HamidReza Houshiar
|
||||
*
|
||||
* Released under the GPL version 3.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <fstream>
|
||||
#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"<<endl;
|
||||
time_t rawtime;
|
||||
struct tm *timeinfo;
|
||||
time(&rawtime);
|
||||
char time[50];
|
||||
timeinfo = localtime (&rawtime);
|
||||
sprintf(time, "%d-%d-%d-%d:%d:%d", timeinfo->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"<<endl;
|
||||
info.verbose = atoi(optarg);
|
||||
break;
|
||||
case 'O':
|
||||
info.outDir = optarg;
|
||||
break;
|
||||
case '?':
|
||||
cout<<"Unknown option character "<<optopt<<endl;
|
||||
usage(argc, argv);
|
||||
break;
|
||||
default:
|
||||
usage(argc, argv);
|
||||
}
|
||||
if(info.pMethod == PANNINI && info.pParam == 0){
|
||||
info.pParam = 1;
|
||||
if(info.nImages < 3) info.nImages = 3;
|
||||
}
|
||||
if(info.pMethod == STEREOGRAPHIC && info.pParam == 0){
|
||||
info.pParam = 2;
|
||||
if(info.nImages < 3) info.nImages = 3;
|
||||
}
|
||||
if(info.pMethod == RECTILINEAR && info.nImages < 3)
|
||||
info.nImages = 3;
|
||||
if(info.mMethod == KNN && info.mParam == 0)
|
||||
info.mParam = 3;
|
||||
if(info.mMethod == RADIUS && info.mParam == 0)
|
||||
info.mParam = 100;
|
||||
if(info.dMethod == ORB_DES && info.fMethod == SIFT_DET){
|
||||
cout<<"Error: SIFT feature doesn't work with ORB descriptor."<<endl;
|
||||
usage(argc, argv);
|
||||
}
|
||||
if(info.mMethod == FLANN && info.dMethod == ORB_DES){
|
||||
cout<<"Error: ORB descriptoronly works with BRUTEFORCE matcher."<<endl;
|
||||
usage(argc, argv);
|
||||
}
|
||||
|
||||
if (optind > argc - 3)
|
||||
{
|
||||
cout<<"Too few input arguments. At least dir and two scan numbers are required."<<endl;
|
||||
usage(argc, argv);
|
||||
}
|
||||
|
||||
info.dir = argv[optind];
|
||||
info.fScanNumber = atoi(argv[optind+1]);
|
||||
info.sScanNumber = atoi(argv[optind+2]);
|
||||
if(info.outDir.empty()) info.outDir = info.dir;
|
||||
else if(info.outDir.compare(info.outDir.size()-1, 1, "/") != 0) info.outDir += "/";
|
||||
}
|
||||
|
||||
void informationDescription(information info){
|
||||
cout<<"program parameters are:"<<endl;
|
||||
cout<<endl;
|
||||
cout<<"local time: "<<info.local_time<<endl;
|
||||
cout<<"input dir: "<<info.dir<<endl;
|
||||
cout<<"output dir: "<<info.outDir<<endl;
|
||||
cout<<"first scan number: "<<info.fScanNumber<<endl;
|
||||
cout<<"second scan number: "<<info.sScanNumber<<endl;
|
||||
cout<<"scan format: "<<scanFormatToString(info.sFormat)<<endl;
|
||||
cout<<endl;
|
||||
cout<<"image width: "<<info.iWidth<<endl;
|
||||
cout<<"image height: "<<info.iHeight<<endl;
|
||||
cout<<"number of images: "<<info.nImages<<endl;
|
||||
cout<<"projection parameter: "<<info.pParam<<endl;
|
||||
cout<<"projection method: "<<projectionMethodToString(info.pMethod)<<endl;
|
||||
cout<<endl;
|
||||
cout<<"feature detector method: "<<featureDetectorMethodToString(info.fMethod)<<endl;
|
||||
cout<<"feature descriptor method: "<<featureDescriptorMethodToString(info.dMethod)<<endl;
|
||||
cout<<endl;
|
||||
cout<<"matcher parameter: "<<info.mParam<<endl;
|
||||
cout<<"matcher method: "<<matcherMethodToString(info.mMethod)<<endl;
|
||||
cout<<endl;
|
||||
cout<<"min distacne: "<<info.minDistance<<endl;
|
||||
cout<<"min error: "<<info.minError<<endl;
|
||||
cout<<"min inlier: "<<info.minInlier<<endl;
|
||||
cout<<"registration method: "<<registrationMethodToString(info.rMethod)<<endl;
|
||||
cout<<endl;
|
||||
}
|
||||
|
||||
void info_yml(information info, double bError, double bErrorIdx, double* bAlign){
|
||||
cv::Mat align(16, 1, CV_32FC(1), cv::Scalar::all(0));
|
||||
for(int i = 0 ; i < 16 ; i++)
|
||||
align.at<float>(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"<<endl;
|
||||
if(info.verbose >= 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<char>(), 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] <<endl;
|
||||
dat << bAlign[1] << " " << bAlign[5] << " " << bAlign[9] << " " << bAlign[13] <<endl;
|
||||
dat << bAlign[2] << " " << bAlign[6] << " " << bAlign[10] << " " << bAlign[14] <<endl;
|
||||
dat << bAlign[3] << " " << bAlign[7] << " " << bAlign[11] << " " << bAlign[15] <<endl;
|
||||
dat.close();
|
||||
|
||||
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)+".frames";
|
||||
ofstream frames(out.c_str());
|
||||
for (int n = 0 ; n < 2 ; n++)
|
||||
{
|
||||
for(int i = 0; i < 16; i++)
|
||||
frames << bAlign[i] <<" ";
|
||||
frames << "2" << endl;
|
||||
}
|
||||
frames.close();
|
||||
}
|
||||
|
||||
if(info.verbose >= 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;
|
||||
}
|
||||
@ -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})
|
||||
Binary file not shown.
@ -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<std::string> 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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
|
||||
virtual bool supports(IODataType type);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -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 <iostream>
|
||||
using std::cout;
|
||||
using std::cerr;
|
||||
using std::endl;
|
||||
#include <vector>
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#include <windows.h>
|
||||
#endif
|
||||
|
||||
#include <boost/filesystem/operations.hpp>
|
||||
#include <boost/filesystem/fstream.hpp>
|
||||
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<std::string> ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
|
||||
{
|
||||
std::list<std::string> 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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* 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
|
||||
|
||||
Loading…
Reference in New Issue