diff --git a/.svn/pristine/03/03075b55ebfb6f87c17388bd580e470b377222b3.svn-base b/.svn/pristine/03/03075b55ebfb6f87c17388bd580e470b377222b3.svn-base new file mode 100644 index 0000000..a86e317 --- /dev/null +++ b/.svn/pristine/03/03075b55ebfb6f87c17388bd580e470b377222b3.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* temperature, 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/08/088a2028a56b46e0edd257d15d96be51a35bd9a1.svn-base b/.svn/pristine/08/088a2028a56b46e0edd257d15d96be51a35bd9a1.svn-base new file mode 100644 index 0000000..e8c97a3 --- /dev/null +++ b/.svn/pristine/08/088a2028a56b46e0edd257d15d96be51a35bd9a1.svn-base @@ -0,0 +1,322 @@ +#ifndef __COLORMANAGER_H__ +#define __COLORMANAGER_H__ + +#ifdef _MSC_VER +#define _USE_MATH_DEFINES +#include +#endif +#ifdef __APPLE__ +#include +#else +#include +#endif +#include +#include + +class ColorMap { + public: + enum CM { + SOLID = 0, + GREY = 1, + HSV = 2, + JET = 3, + HOT = 4, + SHSV = 5, + TEMP = 6 + }; + + virtual ~ColorMap() {}; + + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + d[0] = d[1] = d[2] = 1.0; + } + + static ColorMap getColorMap(CM map); + + /** + * hue is in [0,360], all others in [0,1] + */ + static void convert_hsv_to_rgb(float hue, float s, float v, + float &r, float &g, float &b); +}; + +class GreyMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + d[0] = (float)i/(float)buckets; + d[1] = (float)i/(float)buckets; + d[2] = (float)i/(float)buckets; + } +}; + +class HSVMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + float t = (float)i/(float)buckets; + convert_hsv_to_rgb(360.0*t, 1.0, 1.0, d[0], d[1], d[2]); + } +}; + +class SHSVMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + float t = (float)i/(float)buckets; + convert_hsv_to_rgb(240.0*(1.0-t), 1.0, 1.0, d[0], d[1], d[2]); + } +}; + +class JetMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + float t = (float)i/(float)buckets; + if (t <= 0.125) { + d[0] = d[1] = 0.0; d[2] = 0.5 + 0.5*(t/0.125); + } else if (t < 0.375) { + d[0] = 0.0; d[2] = 1.0; d[1] = (t-0.125)/0.25; + } else if (t < 0.625) { + d[1] = 1.0; d[0] = (t-0.375)/0.25;; d[2] = 1.0 - d[0]; + } else if (t < 0.875) { + d[0] = 1.0; d[2] = 0.0; d[1] = 1.0 - (t-0.625)/0.25; + } else { + d[1] = d[2] = 0.0; d[0] = 1.0 - 0.5*((t - 0.875)/0.125); + } + } +}; + +class HotMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + float t = (float)i/(float)buckets; + if (t <= 1.0/3.0) { + d[1] = d[2] = 0.0; d[0] = t/(1.0/3.0); + } else if (t <= 2.0/3.0) { + d[0] = 1.0; d[2] = 0.0; d[1] = (t-(1.0/3.0))/(1.0/3.0); + } else { + d[0] = 1.0; d[1] = 1.0; d[2] = (t-(2.0/3.0))/(1.0/3.0); + } + } +}; + +class TempMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + float t = 1.0 - (float)i/(float)buckets; + + if(t <= 1.0/5.0) { + d[1] = d[2] = 0.0; + d[0] = t/(1.0/5.0); + } else if(t <=2.0/5.0) { + d[0] = 1.0; + d[1] = (t-(1.0/5.0))/(1.0/5.0); + d[2] = 0.0; + } else if(t <=3.0/5.0) { + d[0] = 1.0 - ((t-(2.0/5.0))/(1.0/5.0)); + d[1] = 1.0; + d[2] = (t-(2.0/5.0))/(1.0/5.0); + } else if(t <=4.0/5.0) { + d[0] = 0.0; + d[1] = 1.0 - ((t-(3.0/5.0))/(1.0/5.0)); + d[2] = 1.0; + } else { + d[0] = 0.0; + d[1] = 0.0; + d[2] = 1.0 - ((t-(4.0/5.0))/(1.3/5.0)); + } + } +}; + +class DiffMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets); + private: + static const float cmap[7][3]; +}; + +class ColorManager { + + public: + + ColorManager(unsigned int _buckets, unsigned int pointdim, float *_mins, float *_maxs, const float *_color = 0) : buckets(_buckets) { + if (_color) { + color[0] = _color[0]; + color[1] = _color[1]; + color[2] = _color[2]; + } else { + color[0] = 1; + color[1] = 1; + color[2] = 1; + } + + colormap = new float*[buckets + 1]; // allow a color more for values equal to max + for (unsigned int i = 0; i < buckets; i++) { + colormap[i] = new float[3]; + } + colormap[buckets] = new float[3]; + + mins = new float[pointdim]; + maxs = new float[pointdim]; + for (unsigned int i = 0; i < pointdim; i++) { + mins[i] = _mins[i]; + maxs[i] = _maxs[i]; + } + + setCurrentDim(0); + } + + virtual ~ColorManager() { + for (unsigned int i = 0; i < buckets; i++) { + delete[] colormap[i]; + } + delete[] colormap[buckets]; + delete[] colormap; + + delete[] mins; + delete[] maxs; + } + + virtual void load() { + glColor3f(color[0], color[1], color[2] ); + glEnable (GL_TEXTURE_1D); + glBindTexture (GL_TEXTURE_1D, 0); + } + + virtual void unload() { + glDisable (GL_TEXTURE_1D); + } + + virtual void setColor(float *val) { + glTexCoord1f( (float)((val[currentdim]-min)/extent) ); + } + virtual void setColor(double *val) { + glTexCoord1f( (float)((val[currentdim]-min)/extent) ); + } + virtual void setColor(short int *val) { + glTexCoord1f( (float)((val[currentdim]-min)/extent) ); + } + virtual void setColor(signed char *val) { + glTexCoord1f( (float)((val[currentdim]-min)/extent) ); + } + + virtual void setColorMap(ColorMap &cm) { + for (unsigned int i = 0; i < buckets; i++) { + cm.calcColor(colormap[i], i, buckets); + } + cm.calcColor(colormap[buckets], buckets-1, buckets); + convertToTexture1D(); + } + + + void setCurrentDim(unsigned int cdim) { + currentdim = cdim; + makeValid(); + } + + + void setMinMax(float _min, float _max) { + if (_min < _max) { + min = _min; + max = _max; + } + extent = max - min; + } + + protected: + + + void convertToTexture1D() { + unsigned char *imageData = new unsigned char[(buckets+1) * 3]; + for (unsigned int i = 0; i < buckets; i++) { + imageData[3*i+0] = colormap[i][0]*255; + imageData[3*i+1] = colormap[i][1]*255; + imageData[3*i+2] = colormap[i][2]*255; + } + + imageData[3*buckets+0] = colormap[buckets][0]*255; + imageData[3*buckets+1] = colormap[buckets][1]*255; + imageData[3*buckets+2] = colormap[buckets][2]*255; + + glBindTexture (GL_TEXTURE_1D, 0); + glPixelStorei (GL_UNPACK_ALIGNMENT, 1); + glTexParameteri (GL_TEXTURE_1D, GL_TEXTURE_WRAP_S, GL_CLAMP); + glTexParameteri (GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri (GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexEnvf (GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); + glTexImage1D (GL_TEXTURE_1D, 0, GL_RGB, buckets+1, 0, GL_RGB, GL_UNSIGNED_BYTE, imageData); + delete[] imageData; + } + + void makeValid() { + min = mins[currentdim]; + max = maxs[currentdim]; + + extent = max - min; + } + + unsigned int buckets; + + unsigned int currentdim; + + /** stores minima and maxima for each point dimension */ + float *mins; + float *maxs; + /** maps color to value */ + float **colormap; + + float min; + float max; + + float extent; + + float color[3]; + +}; + +class CColorManager : public ColorManager { + public: + CColorManager(unsigned int buckets, unsigned int pointdim, float *mins, float *maxs, unsigned int _colordim) : ColorManager(buckets, pointdim, mins, maxs) { + colordim = _colordim; + } + + virtual void load() { + glGetBooleanv(GL_COLOR_LOGIC_OP, &color_state); + glDisable(GL_COLOR_LOGIC_OP); // this disables inversion of color, but also messes with fog behaviour + glColor3f(color[0], color[1], color[2] ); + glEnable (GL_TEXTURE_1D); + glBindTexture (GL_TEXTURE_1D, 0); + } + + virtual void unload() { + glDisable (GL_TEXTURE_1D); + if (color_state) { + glEnable(GL_COLOR_LOGIC_OP); + } + } + + void setColor(double *val) { + GLubyte color[3]; + memcpy(color, &val[colordim], 3); + glColor3ubv(color); + } + void setColor(float *val) { + GLubyte color[3]; + memcpy(color, &val[colordim], 3); + glColor3ubv(color); + } + void setColor(short *val) { + GLubyte color[3]; + memcpy(color, &val[colordim], 3); + glColor3ubv(color); + } + virtual void setColor(signed char *val) { + GLubyte color[3]; + memcpy(color, &val[colordim], 3); + glColor3ubv(color); + } + + private: + unsigned int colordim; + GLboolean color_state; +}; + + +#endif diff --git a/.svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base b/.svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base new file mode 100644 index 0000000..b9193ac --- /dev/null +++ b/.svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base @@ -0,0 +1,101 @@ +/** + * @file fbr_global.h + * @brief Globally used headers, functions, structures + * @author HamidReza Houshiar. Jacobs University Bremen gGmbH, Germany. + * @date 2012/05/9 14:00 + */ + +#ifndef FBR_GLOBAL_H_ +#define FBR_GLOBAL_H_ + +#include +#include +#include +#include +#include +#include +#include "slam6d/io_types.h" +#include "slam6d/globals.icc" + +using namespace std; + +namespace fbr{ + + //Vertical angle of view of scanner +#define MAX_ANGLE 60.0 +#define MIN_ANGLE -40.0 + + /** + * @enum projection_method + */ + enum projection_method{ + EQUIRECTANGULAR, + CYLINDRICAL, + MERCATOR, + RECTILINEAR, + PANNINI, + STEREOGRAPHIC, + ZAXIS, + CONIC + }; + /** + * @enum panorama_map_method + */ + enum panorama_map_method{ + FARTHEST, + EXTENDED, + }; + /** + * @enum feature_method + */ + enum feature_detector_method{ + SIFT_DET, + SURF_DET, + ORB_DET, + FAST_DET, + STAR_DET, + }; + enum feature_descriptor_method{ + SIFT_DES, + SURF_DES, + ORB_DES, + }; + /** + * @enum matching_method + */ + enum matcher_method{ + BRUTEFORCE, + FLANN, + KNN, + RADIUS, + RATIO, + }; + /** + * @enum registration_method + */ + enum registration_method{ + ALL, + RANSAC, + DISABLE, + }; + //RANSAC iteration +#define RANSACITR 20000 + //Inlier influence +#define iInfluence 0.5 + + string scanFormatToString(IOType format); + IOType stringToScanFormat(string format); + string projectionMethodToString(projection_method method); + projection_method stringToProjectionMethod(string method); + string panoramaMapMethodToString(panorama_map_method method); + panorama_map_method stringToPanoramaMapMethod(string method); + string featureDetectorMethodToString(feature_detector_method method); + feature_detector_method stringToFeatureDetectorMethod(string method); + string featureDescriptorMethodToString(feature_descriptor_method method); + feature_descriptor_method stringToFeatureDescriptorMethod(string method); + string matcherMethodToString(matcher_method method); + matcher_method stringToMatcherMethod(string method); + string registrationMethodToString(registration_method method); + registration_method stringToRegistrationMethod(string method); +} +#endif /* FBR_GLOBAL_H_ */ diff --git a/.svn/pristine/15/15833af3aa316bca80246ea5be32252af93605f2.svn-base b/.svn/pristine/15/15833af3aa316bca80246ea5be32252af93605f2.svn-base new file mode 100644 index 0000000..a828cd2 --- /dev/null +++ b/.svn/pristine/15/15833af3aa316bca80246ea5be32252af93605f2.svn-base @@ -0,0 +1,27 @@ +/** + * @file + * @brief IO of a 3D scan in riegl_txt file format with added color information + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_RIEGL_RGB_H__ +#define __SCAN_IO_RIEGL_RGB_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for Riegl scans + * + * The compiled class is available as shared object file + */ +class ScanIO_riegl_rgb : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/16/16e86bc6017730cb99fa4313a99581e6712cb05d.svn-base b/.svn/pristine/16/16e86bc6017730cb99fa4313a99581e6712cb05d.svn-base new file mode 100644 index 0000000..5c05f84 --- /dev/null +++ b/.svn/pristine/16/16e86bc6017730cb99fa4313a99581e6712cb05d.svn-base @@ -0,0 +1,466 @@ +/* + * scan_red implementation + * + * Copyright (C) Dorit Borrmann, Razvan-George Mihalyi, Remus Dumitru + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Main program for reducing 3D scans. + * + * Program to reduce scans for use with slam6d + * Usage: bin/scan_red -r 'dir', + * Use -r for octree based reduction (voxel size=) + * and 'dir' the directory of a set of scans + * Reduced scans will be written to 'dir/reduced' + * + * @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany. + */ +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#define WANT_STREAM ///< define the WANT stream :) +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::ofstream; +#include + +#include "slam6d/metaScan.h" +#include "slam6d/io_utils.h" +#include "slam6d/scan.h" +#include "slam6d/fbr/fbr_global.h" +#include "slam6d/fbr/panorama.h" +#include "slam6d/fbr/scan_cv.h" + +#include "scanserver/clientInterface.h" + +#include "slam6d/globals.icc" + +#ifdef _OPENMP +#include +#endif + + +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#include +#include +#else +#include +#include +#include +#include +#endif + +//Vertical angle of view of scanner +#define MAX_ANGLE 60.0 +#define MIN_ANGLE -40.0 + +#define IMAGE_HEIGHT 1000 +#define IMAGE_WIDTH 3600 + +using namespace fbr; + +projection_method strToPMethod(string method){ + if(strcasecmp(method.c_str(), "EQUIRECTANGULAR") == 0) return EQUIRECTANGULAR; + else if(strcasecmp(method.c_str(), "CYLINDRICAL") == 0) return CYLINDRICAL; + else if(strcasecmp(method.c_str(), "MERCATOR") == 0) return MERCATOR; + else if(strcasecmp(method.c_str(), "CONIC") == 0) return CONIC; + else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown")); +} + +/** + * Explains the usage of this program's command line parameters + */ +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] -r directory" << endl << endl; + cout << bold << "OPTIONS" << normal << 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 << " -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 + << " (choose 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})" << 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 << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " if NR >= 0, turns on octree based point reduction (voxel size=)" << endl + << " if NR < 0, turns on rescaling based reduction" << endl + << endl + << bold << " -I" << normal << " NR," << bold << "--rangeimage=" << normal << "NR" << endl + << " use rescaling of the range image as reduction method" << endl + << " if NR = 1 recovers ranges from range image" << endl + << " if NR = 2 interpolates 3D points in the image map" << endl + << " if NR is omitted, then NR=1 is selected" << endl + << endl + << bold << " -p" << normal << " MET," << bold << "--projection=" << normal << "MET" << endl + << " create range image using the MET projection method" << endl + << " (choose MET from [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|CONIC])" << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl << endl; + + cout << bold << "EXAMPLES " << normal << endl + << " " << prog << " -m 500 -r 5 dat" << endl + << " " << prog << " --max=5000 -r 10.2 dat" << endl + << " " << prog << " -s 2 -e 10 -r dat" << endl + << " " << prog << " -s 0 -e 1 -r 10 -I=1 dat " << 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 the directory + * @param red using point reduction? + * @param rand use randomized point reduction? + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param maxDist - maximal distance of points being loaded + * @param minDist - minimal distance of points being loaded + * @param projection - projection method for building range image + * @param quiet switches on/off the quiet mode + * @param veryQuiet switches on/off the 'very quiet' mode + * @return 0, if the parsing was successful. 1 otherwise + */ +int parseArgs(int argc, char **argv, string &dir, double &red, + int &start, int &end, int &maxDist, int &minDist, + string &projection, int &octree, IOType &type, + int &rangeimage, bool &scanserver) +{ + bool reduced = false; + int c; + // from unistd.h: + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + /* options descriptor */ + // 0: no arguments, 1: required argument, 2: optional argument + static struct option longopts[] = { + { "format", required_argument, 0, 'f' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "octree", optional_argument, 0, 'O' }, + { "rangeimage", optional_argument, 0, 'I' }, + { "projection", required_argument, 0, 'p' }, + { "scanserver", no_argument, 0, 'S' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + cout << endl; + while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:O:p:", longopts, NULL)) != -1) + switch (c) + { + case 'r': + red = atof(optarg); + reduced = true; + break; + case 's': + w_start = atoi(optarg); + if (w_start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (w_end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (w_end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'M': + minDist = atoi(optarg); + break; + case 'I': + if (optarg) { + rangeimage = atoi(optarg); + } else { + rangeimage = 1; + } + break; + case 'p': + projection = optarg; + break; + case 'S': + scanserver = true; + break; + case '?': + usage(argv[0]); + return 1; + default: + abort (); + } + + if(!reduced) { + cerr << "\n*** Reduction method missed ***" << endl; + usage(argv[0]); + } + 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); + + return 0; +} + + +/** + * Main program for reducing scans. + * Usage: bin/scan_red -r 'dir', + * Use -r for octree based reduction (voxel size=) + * and 'dir' the directory of a set of scans + * Reduced scans will be written to 'dir/reduced' + * + */ +int main(int argc, char **argv) +{ + + cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl; + + if (argc <= 1) { + usage(argv[0]); + } + + // parsing the command line parameters + // init, default values if not specified + string dir; + double red = -1.0; + int start = 0, end = -1; + int maxDist = -1; + int minDist = -1; + string projection = "EQUIRECTANGULAR"; + int octree = 0; + IOType type = RIEGL_TXT; + int rangeimage = 0; + bool scanserver = false; + + parseArgs(argc, argv, dir, red, start, end, maxDist, minDist, projection, + octree, type, rangeimage, scanserver); + + if (scanserver) { + try { + ClientInterface::create(); + } catch(std::runtime_error& e) { + cerr << "ClientInterface could not be created: " << e.what() << endl; + cerr << "Start the scanserver first." << endl; + exit(-1); + } + } + + // Get Scans + string reddir = dir + "reduced"; + +#ifdef _MSC_VER + int success = mkdir(reddir.c_str()); +#else + int success = mkdir(reddir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << reddir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << reddir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << reddir << " failed" << endl; + exit(1); + } + + 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); + } + + string scanFileName; + string poseFileName; + + /// Use the OCTREE based reduction + if (rangeimage == 0) { + cout << endl << "Reducing point cloud using octrees" << endl; + int scan_number = start; + for(std::vector::iterator it = Scan::allScans.begin(); + it != Scan::allScans.end(); + ++it, ++scan_number) { + Scan* scan = *it; + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + scan->setRangeFilter(maxDist, minDist); + scan->setReductionParameter(red, octree); + // get reduced points + DataXYZ xyz_r(scan->get("xyz reduced")); + unsigned int nPoints = xyz_r.size(); + + const char* id = scan->getIdentifier(); + cout << "Writing Scan No. " << id; + cout << " with " << xyz_r.size() << " points" << endl; + scanFileName = reddir + "/scan" + id + ".3d"; + poseFileName = reddir + "/scan" + id + ".pose"; + + ofstream redptsout(scanFileName.c_str()); + for(unsigned int j = 0; j < nPoints; j++) { + redptsout << xyz_r[j][0] << " " + << xyz_r[j][1] << " " + << xyz_r[j][2] << endl; + } + redptsout.close(); + redptsout.clear(); + + ofstream posout(poseFileName.c_str()); + posout << rPos[0] << " " + << rPos[1] << " " + << rPos[2] << endl + << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + + posout.close(); + posout.clear(); + if (scanserver) { + scan->clear("xyz reduced"); + } + } + } else { /// use the RESIZE based reduction + cout << endl << "Reducing point cloud by rescaling the range image" << endl; + + Scan::openDirectory(false, dir, type, start, end); + if (Scan::allScans.size() <= 0) { + cerr << "No scans found!" << endl; + exit(-1); + } + + for (int scan_number = start; scan_number <= end; scan_number++) { + + Scan* scan = Scan::allScans[scan_number]; + scan->setRangeFilter(maxDist, minDist); + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + scanFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".3d"; + poseFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".pose"; + // Create a panorama. The iMap inside does all the tricks for us. + scan_cv sScan(dir, scan_number, type); + sScan.convertScanToMat(); + + /// Project point cloud using the selected projection method + panorama image(IMAGE_WIDTH, IMAGE_HEIGHT, strToPMethod(projection)); + image.createPanorama(sScan.getMatScan()); + image.getDescription(); + + /// Resize the range image, specify desired interpolation method + double scale = 1.0/red; + cv::Mat range_image_resized; // reflectance_image_resized; + string ofilename; + stringstream ss; + ss << setw(3) << setfill('0') << (scan_number); + ofilename = reddir + "/scan" + ss.str() + ".3d"; + if (rangeimage == 1) { + resize(image.getRangeImage(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + // Recover point cloud from image and write scan to file + stringstream ss; + ss << setw(3) << setfill('0') << (scan_number); + image.recoverPointCloud(range_image_resized, ofilename); + } else { + resize(image.getMap(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + ofstream redptsout(ofilename.c_str()); + // Convert back to 3D. + for(int i = 0; i < range_image_resized.rows; i++) { + for(int j = 0; j < range_image_resized.cols; j++) { + cv::Vec3f vec = range_image_resized.at(i, j); + double x = vec[0]; + double y = vec[1]; + double z = vec[2]; + redptsout << x << " " << y << " " << z << endl; + } + } + } + + ofstream posout(poseFileName.c_str()); + posout << rPos[0] << " " + << rPos[1] << " " + << rPos[2] << endl + << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + posout.clear(); + posout.close(); + } + } + + cout << endl << endl; + cout << "Normal program end." << endl << endl; + + if (scanserver) { + Scan::closeDirectory(); + ClientInterface::destroy(); + } +} + diff --git a/.svn/pristine/27/273b9c7a4add695164edbbd786206862ef4ae722.svn-base b/.svn/pristine/27/273b9c7a4add695164edbbd786206862ef4ae722.svn-base new file mode 100644 index 0000000..a38145b --- /dev/null +++ b/.svn/pristine/27/273b9c7a4add695164edbbd786206862ef4ae722.svn-base @@ -0,0 +1,27 @@ +/** + * @file + * @brief IO of a 3D scan in uos file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_UOS_H__ +#define __SCAN_IO_UOS_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for UOS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_uos : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/2c/2cf4e891f6f5a378f076d29d0b9feb822ab8c7d8.svn-base b/.svn/pristine/2c/2cf4e891f6f5a378f076d29d0b9feb822ab8c7d8.svn-base new file mode 100644 index 0000000..db8b480 --- /dev/null +++ b/.svn/pristine/2c/2cf4e891f6f5a378f076d29d0b9feb822ab8c7d8.svn-base @@ -0,0 +1,265 @@ +/* + * scanHandler implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann + * + * Released under the GPL version 3. + * + */ + +#include "scanserver/scanHandler.h" + +#include +#include +#include +using namespace std; + +#include +using boost::scoped_ptr; + +#include "scanserver/cache/cacheManager.h" +#include "scanio/scan_io.h" + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif //WITH_METRICS + + + +bool ScanHandler::binary_caching = false; +std::map* > ScanHandler::m_prefetch_xyz; +std::map* > ScanHandler::m_prefetch_rgb; +std::map* > ScanHandler::m_prefetch_reflectance; +std::map* > ScanHandler::m_prefetch_temperature; +std::map* > ScanHandler::m_prefetch_amplitude; +std::map* > ScanHandler::m_prefetch_type; +std::map* > ScanHandler::m_prefetch_deviation; + + + +//! Abstract class for merging calls to the main vector +class PrefetchVectorBase { +public: + virtual bool prefetch() = 0; + virtual void create() = 0; + virtual unsigned int size() const = 0; + virtual void write(void* data_ptr) = 0; +}; + +/** Class for handling a vector and its special case for prefetching. + * + * Outline: prefetch() main vector, otherwise create() on main+prefetched + * vectors and use of ScanIO, cache allocation with size(), write() for main, + * save() for prefetched vectors. + */ +template +class PrefetchVector : public PrefetchVectorBase { +public: + PrefetchVector(SharedScan* scan, map*>& prefetches) : + m_scan(scan), m_prefetches(&prefetches), m_vector(0) + { + } + + ~PrefetchVector() + { + // remove vectors that are still here (RAII/exceptions) + if(m_vector) + delete m_vector; + } + + inline vector* get() const { return m_vector; } + + //! If a prefetch is found, take ownership and signal true for a successful prefetch + virtual bool prefetch() + { + // check if a prefetch is available + typename map*>::iterator it = m_prefetches->find(m_scan); + if(it != m_prefetches->end()) { + // take ownership of the vector and remove it from the map + m_vector = it->second; + m_prefetches->erase(it); + return true; + } + return false; + } + + //! Create an empty vector + virtual void create() { + if(m_vector == 0) { + m_vector = new vector; + } + } + + //! Size of vector contents in bytes + virtual unsigned int size() const { + return m_vector->size()*sizeof(T); + } + + //! Write vector contents into the cache object via \a data_ptr and clean up the vector + virtual void write(void* data_ptr) { + // write vector contents + for(unsigned int i = 0; i < m_vector->size(); ++i) { + reinterpret_cast(data_ptr)[i] = (*m_vector)[i]; + } + // remove so it won't get saved for prefetches + delete m_vector; + m_vector = 0; + } + + //! Save vector for prefetching + void save() { + if(m_vector != 0 && m_vector->size() != 0) { + // create map entry and assign the vector + (*m_prefetches)[m_scan] = m_vector; + // ownership transferred + m_vector = 0; + } + } +private: + SharedScan* m_scan; + map*>* m_prefetches; + + vector* m_vector; +}; + + + +ScanHandler::ScanHandler(CacheObject* obj, CacheManager* cm, SharedScan* scan, IODataType data) : + TemporaryHandler(obj, cm, scan, true), + m_data(data) +{ +} + +bool ScanHandler::load() +{ + ScanIO* sio = ScanIO::getScanIO(m_scan->getIOType()); + + // avoid loading of a non-supported type + if(!sio->supports(m_data)) return false; + +#ifdef WITH_METRICS + Timer t = ServerMetric::scan_loading.start(); +#endif //WITH_METRICS + + // INFO + //cout << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanHandler::load" << endl; + + // if binary scan caching is enabled try to read it via TemporaryHandler first, if written-flag wasn't set or file didn't exist, parse scan + if(binary_caching) { + if(TemporaryHandler::load()) { + // INFO + //cout << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanHandler::load successful via TemporaryHandler" << endl << endl; +#ifdef WITH_METRICS + ServerMetric::scan_loading.end(t); +#endif //WITH_METRICS + return true; + } + } + + PrefetchVector xyz(m_scan, m_prefetch_xyz); + PrefetchVector rgb(m_scan, m_prefetch_rgb); + PrefetchVector reflectance(m_scan, m_prefetch_reflectance); + PrefetchVector temperature(m_scan, m_prefetch_temperature); + PrefetchVector amplitude(m_scan, m_prefetch_amplitude); + PrefetchVector type(m_scan, m_prefetch_type); + PrefetchVector deviation(m_scan, m_prefetch_deviation); + + // assign vector for this particular ScanHandler + PrefetchVectorBase* vec = 0; + if(m_data == DATA_XYZ) vec = &xyz; + else if(m_data == DATA_RGB) vec = &rgb; + else if(m_data == DATA_REFLECTANCE) vec = &reflectance; + else if(m_data == DATA_TEMPERATURE) vec = &temperature; + else if(m_data == DATA_AMPLITUDE) vec = &litude; + else if(m_data == DATA_TYPE) vec = &type; + else if(m_data == DATA_DEVIATION) vec = &deviation; + + unsigned int prefetch = m_scan->getPrefetch(); + // try to prefetch only the requested type from its handler + if(vec->prefetch()) + { + // nothing to do if prefetch was successful, vector is initialized + // reset prefetch flags, nothing needs to be saved + prefetch = 0; + } else { + // create vector and exclude it from prefetch handling + vec->create(); + prefetch &= ~m_data; + + // create vectors which are to be prefetched + if(prefetch & DATA_XYZ) xyz.create(); + if(prefetch & DATA_RGB) rgb.create(); + if(prefetch & DATA_REFLECTANCE) reflectance.create(); + if(prefetch & DATA_TEMPERATURE) temperature.create(); + if(prefetch & DATA_AMPLITUDE) amplitude.create(); + if(prefetch & DATA_TYPE) type.create(); + if(prefetch & DATA_DEVIATION) deviation.create(); + + // request data from the ScanIO + try { + PointFilter filter(m_scan->getPointFilter()); + sio->readScan(m_scan->getDirPath(), m_scan->getIdentifier(), + filter, + xyz.get(), rgb.get(), reflectance.get(), temperature.get(), amplitude.get(), type.get(), deviation.get()); + } catch(std::runtime_error& e) { + // INFO + // cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanIO runtime_error: " << e.what() << endl; + // rethrow + throw e; + } catch(std::bad_alloc& e) { + // INFO + // cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanIO bad_alloc: " << e.what() << endl; + // rethrow + throw e; + } + } + + // after successful loading, allocate enough cache space + unsigned int size = vec->size(); + void* data_ptr; + try { + data_ptr = m_manager->allocateCacheObject(m_object, size); + } catch(runtime_error& e) { + // INFO + // cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] CacheManager error: " << e.what() << endl; + // rethrow + throw e; + } + + // write data into the cache object and clean up the vector + vec->write(data_ptr); + + // save all vectors that still hold their data for prefetching + xyz.save(); + rgb.save(); + reflectance.save(); + temperature.save(); + amplitude.save(); + type.save(); + deviation.save(); + + // INFO + //cout << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanHandler::load successful" << endl << endl; + +#ifdef WITH_METRICS + ServerMetric::scan_loading.end(t); +#endif //WITH_METRICS + return true; +} + +void ScanHandler::save(unsigned char* data, unsigned int size) +{ + // INFO + //cout << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanHandler::save" << endl; + + // if global binary scan caching is enabled, save to file for later faster reloading + if(binary_caching) { + // duplicate writes of static data are handled in TemporaryHandler already + TemporaryHandler::save(data, size); + } +} + +void ScanHandler::setBinaryCaching() +{ + binary_caching = true; +} diff --git a/.svn/pristine/2d/2d589cc71a969f101c026cc2c3bca3ce32594578.svn-base b/.svn/pristine/2d/2d589cc71a969f101c026cc2c3bca3ce32594578.svn-base new file mode 100644 index 0000000..19112b6 --- /dev/null +++ b/.svn/pristine/2d/2d589cc71a969f101c026cc2c3bca3ce32594578.svn-base @@ -0,0 +1,454 @@ +/* + * show_menu implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Functions for the menu panels of the viewer software + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ +#include "show/colormanager.h" + +// GUI variables + +GLUI *glui1, ///< pointer to the glui window(s) + *glui2; ///< pointer to the glui window(s) + +/** GLUI spinner for the fog */ +GLUI_Spinner *fog_spinner; + +/** GLUI spinner for the point size */ +GLUI_Spinner *ps_spinner; + +/** GLUI spinner for the current angle */ +GLUI_Spinner *cangle_spinner; +/** GLUI spinner for the current angle */ +GLUI_Spinner *pzoom_spinner; +/** GLUI spinner for the factor for the image size */ +GLUI_Spinner *image_spinner; +/** GLUI_Spinner for the depth to select groups of points */ +GLUI_Spinner *depth_spinner; +GLUI_Spinner *brushsize_spinner; +GLUI_Spinner *frame_spinner; +GLUI_Spinner *fps_spinner; +GLUI_Spinner *farplane_spinner; +GLUI_Spinner *nearplane_spinner; +GLUI_Spinner *lod_spinner; + +int window_id_menu1, ///< menue window ids + window_id_menu2; ///< menue window ids + +/** User IDs for callbacks */ +#define BOX_ID 201 +/** User IDs for callbacks */ +#define ENABLE_ID 300 +/** User IDs for callbacks */ +#define DISABLE_ID 301 +/** User IDs for callbacks */ +#define SHOW_ID 302 +/** User IDs for callbacks */ +#define HIDE_ID 303 + +/** Pointer to the panels */ +GLUI_Panel *ground_panel; +/** Pointer to the panels */ +GLUI_Panel *points_panel; +/** Pointer to the panels */ +GLUI_Panel *wireframe_panel; +/** Pointer to the panels */ +GLUI_Panel *path_panel; +/** Pointer to the panels */ +GLUI_Panel *pose_panel; +/** Pointer to the panels */ +GLUI_Panel *selection_panel; +/** Pointer to the panels */ +GLUI_Panel *color_panel; +/** Pointer to the panels */ +GLUI_Panel *camera_panel; +/** Pointer to the panels */ +GLUI_Panel *nav_panel; +/** Pointer to the panels */ +GLUI_Panel *mode_panel; +/** Pointer to the panels */ +GLUI_Panel *settings_panel; +/** Pointer to the panels */ +GLUI_Panel *advanced_panel; +/** Pointer to the button */ +GLUI_Button *button1; +/** Pointer to the edit text box*/ +GLUI_EditText *path_filename_edit; +/** Pointer to the edit text box*/ +GLUI_EditText *pose_filename_edit; +/** Pointer to the edit text box*/ +GLUI_EditText *selection_filename_edit; + +/** Pointer to the rotation button */ +GLUI_Rotation *rotButton; + +/** used for GLUI menue */ +float obj_pos_button[3]; +/** used for GLUI menue */ +GLfloat view_rotate_button[16]; +/** used for GLUI menue */ +GLfloat obj_pos_button_old[3]; + +/** used for GLUI menue */ +GLfloat X_button; +/** used for GLUI menue */ +GLfloat Y_button; +/** used for GLUI menue */ +GLfloat Z_button; +/** GLUI spinner for choosing the camera */ +GLUI_Spinner *cam_spinner; +/** GLUI spinner for choosing the animation delay */ +GLUI_Spinner *anim_spinner; +/** Panel for the camera controls **/ +GLUI_Panel *cam_panel; +/** ListBox for choosing which value to map to a color */ +GLUI_Listbox *value_listbox; +/** ListBox for choosing which color map to use */ +GLUI_Listbox *colormap_listbox; +GLUI_Spinner *mincol_spinner; +GLUI_Spinner *maxcol_spinner; + +/** Checkboxes for changing point display mode **/ +GLUI_Checkbox *always_box; +GLUI_Checkbox *never_box; +/** Checkbox for changing interpolation mode **/ +GLUI_Checkbox *interpol_box; + +/** + * Generate the menu for the application. + * It consists of control and selection menus. + */ +void newMenu() +{ + /*** Create the bottom subwindow ***/ + glui2 = GLUI_Master.create_glui("3D_Viewer - Controls"); + window_id_menu2 = glui2->get_glut_window_id(); + glutSetWindow(window_id_menu2); + + glutPositionWindow(START_X, START_Y + START_HEIGHT + 65); + glutSetWindow(window_id); + glui2->set_main_gfx_window( window_id ); + + settings_panel = glui2->add_panel("Settings: "); + + cangle_spinner = glui2->add_spinner_to_panel(settings_panel, "Field of View : ", GLUI_SPINNER_FLOAT, &cangle); + cangle_spinner->set_float_limits( 1.0, 180.0 ); + cangle_spinner->set_speed( 20.0 ); + cangle_spinner->set_float_val(60.0); + cangle_spinner->set_alignment( GLUI_ALIGN_RIGHT ); + + pzoom_spinner = glui2->add_spinner_to_panel(settings_panel, "Parallel Zoom :", GLUI_SPINNER_FLOAT, &pzoom); + pzoom_spinner->set_float_limits( 10.0, 50000.0 ); + pzoom_spinner->set_speed( 5.0 ); + pzoom_spinner->set_float_val(2000.0); + pzoom_spinner->set_alignment( GLUI_ALIGN_RIGHT ); + pzoom_spinner->disable(); + + glui2->add_column( true ); + + mode_panel = glui2->add_panel("Mode: "); + + /****** Top view *****/ + glui2->add_button_to_panel(mode_panel, "Top view", 0, callTopView )->set_alignment( GLUI_ALIGN_CENTER ); + + + /****** Reset button *****/ + glui2->add_button_to_panel(mode_panel, "Reset position", 0, resetView )->set_alignment( GLUI_ALIGN_CENTER ); + glui2->add_column( true ); + + /****** Add Camera View *****/ + + + camera_panel = glui2->add_panel("Camera: "); + cam_spinner = glui2->add_spinner_to_panel(camera_panel, "Choose Camera", GLUI_SPINNER_INT, &cam_choice); + cam_spinner->set_int_limits( 0, 0 ); + cam_spinner->set_speed( 1 ); + cam_spinner->set_alignment( GLUI_ALIGN_LEFT ); + + glui2->add_button_to_panel(camera_panel, "Add Camera", 1, callAddCamera )->set_alignment( GLUI_ALIGN_CENTER ); + glui2->add_button_to_panel(camera_panel, "Delete Camera", 0, callDeleteCamera )->set_alignment( GLUI_ALIGN_CENTER ); + + /******* Other navigation controls**********/ + glui2->add_column(true); + + nav_panel = glui2->add_panel("Navigation: "); + rotButton = glui2->add_rotation_to_panel(nav_panel, "Rotation", view_rotate_button, -1, update_view_rotate); + + glui2->add_column_to_panel(nav_panel, true ); + glui2->add_translation_to_panel(nav_panel, "Move XY", GLUI_TRANSLATION_XY, + obj_pos_button, -1, update_view_translation); + + glui2->add_column_to_panel(nav_panel, false ); + glui2->add_translation_to_panel(nav_panel, "Move X", GLUI_TRANSLATION_X, + &obj_pos_button[0], -1, update_view_translation); + + glui2->add_column_to_panel(nav_panel, false ); + glui2->add_translation_to_panel(nav_panel, "Move Y", GLUI_TRANSLATION_Y, + &obj_pos_button[1], -1, update_view_translation); + + glui2->add_column_to_panel(nav_panel, false ); + glui2->add_translation_to_panel(nav_panel, "Move Z", GLUI_TRANSLATION_Z, + &obj_pos_button[2], -1, update_view_translation); + + glui2->add_column_to_panel(nav_panel, false); + glui2->add_checkbox_to_panel(nav_panel, "MouseNav", &cameraNavMouseMode ); + + static int dummy4; + always_box = glui2->add_checkbox_to_panel(nav_panel, "Always all Points", &dummy4, 0, &changePointMode); + glui2->set_glutMouseFunc(CallBackMouseFuncMoving); + static int dummy5 = 1; + never_box = glui2->add_checkbox_to_panel(nav_panel, "Always reduce Points", &dummy5, 1, &changePointMode ); + + /*** Create the right subwindow ***/ + glui1 = GLUI_Master.create_glui("3D_Viewer - Selection"); + window_id_menu1 = glui1->get_glut_window_id(); + glutSetWindow(window_id_menu1); + glutPositionWindow(START_X + START_WIDTH + 50, START_Y + 30); + glutSetWindow(window_id); + glui1->set_main_gfx_window( window_id ); + + glui1->add_checkbox( "Draw Points", &show_points ); + glui1->add_checkbox( "Draw Camera", &show_cameras); + glui1->add_checkbox( "Draw Path", &show_path); + ps_spinner = glui1->add_spinner( "Point Size:", GLUI_SPINNER_FLOAT, &pointsize); + ps_spinner->set_float_limits( 0.0000001, 10.0 ); + ps_spinner->set_speed( 25.0 ); + ps_spinner->set_float_val(pointsize); + ps_spinner->set_alignment( GLUI_ALIGN_LEFT ); + + + /**** Fog Panel *****/ + + GLUI_Panel *fogt_panel = glui1->add_rollout("Fog :", false ); + fogt_panel ->set_alignment( GLUI_ALIGN_LEFT ); + GLUI_RadioGroup *fogt = glui1-> add_radiogroup_to_panel( fogt_panel, &show_fog ); + glui1->add_radiobutton_to_group( fogt, "No Fog" ); + glui1->add_radiobutton_to_group( fogt, "Fog Exp" ); + glui1->add_radiobutton_to_group( fogt, "Fog Exp2" ); + glui1->add_radiobutton_to_group( fogt, "Fog Linear" ); + glui1->add_radiobutton_to_group( fogt, "inverted Fog Exp" ); + glui1->add_radiobutton_to_group( fogt, "inverted Fog Exp2" ); + glui1->add_radiobutton_to_group( fogt, "inverted Fog Linear" ); + + + + + fog_spinner = glui1->add_spinner( "Fog Density:", GLUI_SPINNER_FLOAT, &fogDensity); + fog_spinner->set_float_limits( 0.0, 1.0 ); + fog_spinner->set_speed( 0.5 ); + fog_spinner->set_float_val(0.001); + fog_spinner->set_alignment( GLUI_ALIGN_LEFT ); + + + /****** Color Controls *****/ + color_panel = glui1->add_rollout("Color :", false ); + color_panel ->set_alignment( GLUI_ALIGN_LEFT ); + + GLUI_Panel *color_ro = glui1->add_rollout_to_panel(color_panel, "Color values:", false); + color_ro->set_alignment(GLUI_ALIGN_LEFT); + + GLUI_RadioGroup *color_rog = glui1->add_radiogroup_to_panel( color_ro, &listboxColorVal, 0, &mapColorToValue ); + glui1->add_radiobutton_to_group( color_rog, "height"); + GLUI_RadioButton *rbrefl = glui1->add_radiobutton_to_group( color_rog, "reflectance"); + GLUI_RadioButton *rbtemp = glui1->add_radiobutton_to_group( color_rog, "temperature"); + GLUI_RadioButton *rbampl = glui1->add_radiobutton_to_group( color_rog, "amplitude"); + GLUI_RadioButton *rbdevi = glui1->add_radiobutton_to_group( color_rog, "deviation"); + GLUI_RadioButton *rbtype = glui1->add_radiobutton_to_group( color_rog, "type"); + //if (!(types & PointType::USE_REFLECTANCE)) rbrefl->disable(); + //if (!(types & PointType::USE_AMPLITUDE)) rbampl->disable(); + //if (!(types & PointType::USE_DEVIATION)) rbdevi->disable(); + //if (!(types & PointType::USE_TYPE)) rbtype->disable(); + if (!(pointtype.hasReflectance())) rbrefl->disable(); + if (!(pointtype.hasTemperature())) rbtemp->disable(); + if (!(pointtype.hasAmplitude())) rbampl->disable(); + if (!(pointtype.hasDeviation())) rbdevi->disable(); + if (!(pointtype.hasType())) rbtype->disable(); + + GLUI_Panel *colorm_ro = glui1->add_rollout_to_panel(color_panel, "Colormap:", false); + colorm_ro->set_alignment(GLUI_ALIGN_LEFT); + + GLUI_RadioGroup *colorm_rog = glui1->add_radiogroup_to_panel(colorm_ro, &listboxColorMapVal, 0, &changeColorMap); + glui1->add_radiobutton_to_group(colorm_rog, "Solid"); + glui1->add_radiobutton_to_group(colorm_rog, "Grey"); + glui1->add_radiobutton_to_group(colorm_rog, "HSV"); + glui1->add_radiobutton_to_group(colorm_rog, "Jet"); + glui1->add_radiobutton_to_group(colorm_rog, "Hot"); + glui1->add_radiobutton_to_group(colorm_rog, "Rand"); + glui1->add_radiobutton_to_group(colorm_rog, "SHSV"); + glui1->add_radiobutton_to_group(colorm_rog, "TEMP"); + + GLUI_Panel *scans_color = glui1->add_rollout_to_panel(color_panel, "Color type:", false); + scans_color->set_alignment(GLUI_ALIGN_LEFT); + GLUI_RadioGroup *scans_colored = glui1->add_radiogroup_to_panel(scans_color, &colorScanVal, 0, &setScansColored); + glui1->add_radiobutton_to_group(scans_colored, "None"); + glui1->add_radiobutton_to_group(scans_colored, "Id Scans by Color"); + GLUI_RadioButton *colorb = glui1->add_radiobutton_to_group( scans_colored, "Color by Points"); + if (!(pointtype.hasColor())) colorb->disable(); + + mincol_spinner = glui1->add_spinner_to_panel(color_panel, "Min Val:", GLUI_SPINNER_FLOAT, &mincolor_value, 0, &minmaxChanged); + mincol_spinner->set_alignment(GLUI_ALIGN_RIGHT); + maxcol_spinner = glui1->add_spinner_to_panel(color_panel, "Max Val:", GLUI_SPINNER_FLOAT, &maxcolor_value, 0, &minmaxChanged); + maxcol_spinner->set_alignment(GLUI_ALIGN_RIGHT); + glui1->add_button_to_panel(color_panel, "Reset Min/Max", 0, &resetMinMax )->set_alignment( GLUI_ALIGN_CENTER ); + + glui1->add_separator(); + + /****** Invert button *****/ + glui1->add_button( "Invert", 0, invertView )->set_alignment( GLUI_ALIGN_CENTER ); + /****** Animate button *****/ + anim_spinner = glui1->add_spinner( "Anim delay:", GLUI_SPINNER_INT, &anim_delay); + anim_spinner->set_int_limits( 0, 100 ); + anim_spinner->set_speed( 1 ); + glui1->add_button( "Animate", 0, startAnimation )->set_alignment( GLUI_ALIGN_CENTER ); + + glui1->add_separator(); + + +/**** Path panel *******/ + + path_panel = glui1->add_rollout("Camera Path :", false ); + path_panel ->set_alignment( GLUI_ALIGN_LEFT ); + path_filename_edit = glui1->add_edittext_to_panel(path_panel,"File: ",GLUI_EDITTEXT_TEXT, path_file_name); + path_filename_edit->set_alignment( GLUI_ALIGN_LEFT ); + glui1->add_button_to_panel(path_panel, "Save Path ", 0, savePath)->set_alignment( GLUI_ALIGN_CENTER); + glui1->add_button_to_panel(path_panel, "Load Path ", 0, loadPath)->set_alignment( GLUI_ALIGN_CENTER); + glui1->add_button_to_panel(path_panel, "Load Robot P.", 0, drawRobotPath )->set_alignment( GLUI_ALIGN_CENTER ); + glui1->add_separator_to_panel(path_panel); + glui1->add_checkbox_to_panel(path_panel, "Save Animation", &save_animation); + interpol_box = glui1->add_checkbox_to_panel(path_panel, "Interpolate by Distance", &inter_by_dist, -1, &callCameraUpdate); + //always_box = glui2->add_checkbox_to_panel(nav_panel, "Always all Points", &dummy4, 0, &changePointMode); + //glui1->add_checkbox_to_panel(path_panel, "Interpolate by Distance", &inter_by_dist); + glui1->add_button_to_panel(path_panel, "Animate Path", 0, pathAnimate)->set_alignment( GLUI_ALIGN_CENTER); + + /**** Position panel *******/ + + pose_panel = glui1->add_rollout("Position :", false ); + pose_panel ->set_alignment( GLUI_ALIGN_LEFT ); + pose_filename_edit = glui1->add_edittext_to_panel(pose_panel,"File: ",GLUI_EDITTEXT_TEXT, pose_file_name); + pose_filename_edit->set_alignment( GLUI_ALIGN_LEFT ); + glui1->add_button_to_panel(pose_panel, "Save Pose ", 0, savePose)->set_alignment( GLUI_ALIGN_CENTER); + glui1->add_button_to_panel(pose_panel, "Load Pose ", 0, loadPose)->set_alignment( GLUI_ALIGN_CENTER); + image_spinner = glui1->add_spinner_to_panel(pose_panel, "Factor : ", +GLUI_SPINNER_INT, &factor); + image_spinner->set_int_limits( 1, 10 ); + image_spinner->set_speed( 1 ); + image_spinner->set_alignment(GLUI_ALIGN_RIGHT); + glui1->add_button_to_panel(pose_panel, "Save Image ", 0, saveImage)->set_alignment( GLUI_ALIGN_CENTER); + + + glui1->add_separator(); + + /**** Selection panel ******/ + selection_panel = glui1->add_rollout("Selection :", false ); + selection_panel ->set_alignment( GLUI_ALIGN_LEFT ); + + selection_filename_edit = glui1->add_edittext_to_panel(selection_panel,"File: ",GLUI_EDITTEXT_TEXT, selection_file_name); + selection_filename_edit->set_alignment( GLUI_ALIGN_LEFT ); + glui1->add_button_to_panel(selection_panel, "Save selected points ", 0, saveSelection)->set_alignment( GLUI_ALIGN_CENTER); + + glui1->add_button_to_panel(selection_panel, "Clear selected points ", 0, clearSelection)->set_alignment( GLUI_ALIGN_CENTER); + glui1->add_checkbox_to_panel(selection_panel, "Select/Unselect", &selectOrunselect); + glui1->add_checkbox_to_panel(selection_panel, "Select Voxels", &select_voxels); + depth_spinner = glui1->add_spinner_to_panel(selection_panel, "Depth : ", + GLUI_SPINNER_INT, &selection_depth); + depth_spinner->set_int_limits( 1, 100 ); + depth_spinner->set_speed( 1 ); + depth_spinner->set_alignment(GLUI_ALIGN_RIGHT); + brushsize_spinner = glui1->add_spinner_to_panel(selection_panel, "Brushsize : ", + GLUI_SPINNER_INT, &brush_size); + brushsize_spinner->set_int_limits( 0, 100 ); + brushsize_spinner->set_speed( 1 ); + brushsize_spinner->set_alignment(GLUI_ALIGN_RIGHT); + + + glui1->add_separator(); + /**** Advanced panel ******/ + if (advanced_controls) { + advanced_panel = glui1->add_rollout("Advanced :", false ); + advanced_panel->set_alignment( GLUI_ALIGN_LEFT ); + + // glui1->add_edittext_to_panel(advanced_panel,"Frame #: ",GLUI_EDITTEXT_TEXT, current_frame)->set_alignment( GLUI_ALIGN_LEFT ); + frame_spinner = glui1->add_spinner_to_panel(advanced_panel, "Frame #: ", + GLUI_SPINNER_INT, ¤t_frame); + frame_spinner->set_int_limits( 0, MetaMatrix[0].size()-1 ); + frame_spinner->set_speed( 10 ); + frame_spinner->set_alignment(GLUI_ALIGN_RIGHT); + + fps_spinner = glui1->add_spinner_to_panel(advanced_panel, "FPS : ", + GLUI_SPINNER_FLOAT, &idealfps); + fps_spinner->set_int_limits( 0, 100 ); + fps_spinner->set_speed( 1 ); + fps_spinner->set_alignment(GLUI_ALIGN_RIGHT); + + farplane_spinner = glui1->add_spinner_to_panel(advanced_panel, "farplane : ", + GLUI_SPINNER_FLOAT, &maxfardistance); + farplane_spinner->set_float_limits( 1, 100000 ); + farplane_spinner->set_speed( 1 ); + farplane_spinner->set_alignment(GLUI_ALIGN_RIGHT); + + nearplane_spinner = glui1->add_spinner_to_panel(advanced_panel, "nearplane : ", + GLUI_SPINNER_FLOAT, &neardistance); + nearplane_spinner->set_int_limits( 1, 100000 ); + nearplane_spinner->set_speed( 1 ); + nearplane_spinner->set_alignment(GLUI_ALIGN_RIGHT); + + glui1->add_button_to_panel(advanced_panel, "Cycle LOD", 0,(GLUI_Update_CB)cycleLOD )->set_alignment( GLUI_ALIGN_CENTER ); + + lod_spinner = glui1->add_spinner_to_panel(advanced_panel, "lod speed : ", + GLUI_SPINNER_FLOAT, &adaption_rate); + lod_spinner->set_float_limits( 0, 3.0 ); + lod_spinner->set_speed( 0.1 ); + lod_spinner->set_alignment(GLUI_ALIGN_RIGHT); + + glui1->add_separator(); + } + + /****** A 'quit' button *****/ + glui1->add_button( "Quit", 0,(GLUI_Update_CB)exit )->set_alignment( GLUI_ALIGN_CENTER ); + + glui1->set_glutMouseFunc(CallBackMouseFuncMoving); + /**** Link windows to GLUI, and register idle callback ******/ + glutSetWindow(window_id); + glui1->set_main_gfx_window( window_id ); // right + glui2->set_main_gfx_window( window_id ); // bottom + glui1->sync_live(); + glui2->sync_live(); + + // cout << "Called : myNewMenu()...."<= 0; iterator--) + selected_points[iterator].clear(); +} + diff --git a/.svn/pristine/32/32bed354a46354672f5ec89a85c9b4140371b3ab.svn-base b/.svn/pristine/32/32bed354a46354672f5ec89a85c9b4140371b3ab.svn-base new file mode 100644 index 0000000..263d15c --- /dev/null +++ b/.svn/pristine/32/32bed354a46354672f5ec89a85c9b4140371b3ab.svn-base @@ -0,0 +1,184 @@ +/* + * scan_io_uso_rgb implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_uos_rgb.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_uos_rgb::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_uos_rgb::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_uos_rgb::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_RGB)); +} + +void ScanIO_uos_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 && rgb != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + + // aquire header informations + /* OPTIONAL: the header isn't always there, would require a sanity check + unsigned int n, m; + char[3] dummy; + data_file >> n; + for(unsigned int i = 0; i < 3; ++i) data_file >> dummy[i]; + data_file >> m; + values.reserve(n*m*3); + */ + // overread the first line + char dummy[255]; + data_file.getline(dummy, 255); + + // read points + double point[3]; + unsigned int color[3]; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; + for(i = 0; i < 3; ++i) data_file >> color[i]; + } catch(std::ios_base::failure& e) { + break; + } + // apply filter and insert point + if(filter.check(point)) { + if(xyz != 0) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + if(rgb != 0) { + for(i = 0; i < 3; ++i) rgb->push_back( + static_cast(color[i])); + } + } + } + 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_uos_rgb; +} + + +/** + * 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/37/3725536eaceee10825f0f4fcaaae5f9a0b8b210d.svn-base b/.svn/pristine/37/3725536eaceee10825f0f4fcaaae5f9a0b8b210d.svn-base new file mode 100644 index 0000000..893841f --- /dev/null +++ b/.svn/pristine/37/3725536eaceee10825f0f4fcaaae5f9a0b8b210d.svn-base @@ -0,0 +1,225 @@ +/* + * point_type implementation + * + * Copyright (C) Jan Elseberg, Dorit Borrmann. + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Representation of a 3D point type + * @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany. + */ + +#include "slam6d/point_type.h" + +#include "slam6d/scan.h" + +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; + +#include +#include + +#include +using std::runtime_error; + + + +PointType::PointType() { + types = USE_NONE; + pointdim = 3; + dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = 1; // choose height per default + dimensionmap[8] = 1; + dimensionmap[0] = 1; // height +} + +PointType::PointType(unsigned int _types) : types(_types) { + dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = dimensionmap[8] = 1; // choose height per default + dimensionmap[0] = 1; // height + + pointdim = 3; + if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++; + if (types & PointType::USE_TEMPERATURE) dimensionmap[2] = pointdim++; + if (types & PointType::USE_AMPLITUDE) dimensionmap[3] = pointdim++; + if (types & PointType::USE_DEVIATION) dimensionmap[4] = pointdim++; + if (types & PointType::USE_TYPE) dimensionmap[5] = pointdim++; + if (types & PointType::USE_COLOR) dimensionmap[6] = pointdim++; + if (types & PointType::USE_TIME) dimensionmap[7] = pointdim++; + if (types & PointType::USE_INDEX) dimensionmap[8] = pointdim++; +} + +bool PointType::hasReflectance() { + return hasType(USE_REFLECTANCE); +} +bool PointType::hasTemperature() { + return hasType(USE_TEMPERATURE); +} +bool PointType::hasAmplitude() { + return hasType(USE_AMPLITUDE); +} +bool PointType::hasDeviation() { + return hasType(USE_DEVIATION); +} +bool PointType::hasType() { + return hasType(USE_TYPE); +} +bool PointType::hasColor() { + return hasType(USE_COLOR); +} +bool PointType::hasTime() { + return hasType(USE_TIME); +} + +bool PointType::hasIndex() { + return hasType(USE_INDEX); +} + +unsigned int PointType::getReflectance() { + return dimensionmap[1]; +} + +unsigned int PointType::getTemperature() { + return dimensionmap[2]; +} + +unsigned int PointType::getAmplitude() { + return dimensionmap[3]; +} + +unsigned int PointType::getDeviation() { + return dimensionmap[4]; +} + +unsigned int PointType::getTime() { + return dimensionmap[7]; +} + +unsigned int PointType::getIndex() { + return dimensionmap[8]; +} + +unsigned int PointType::getType() { + return dimensionmap[5]; +} + +unsigned int PointType::getType(unsigned int type) { + if (type == USE_NONE ) { + return dimensionmap[0]; + } else if (type == USE_HEIGHT) { + return dimensionmap[0]; + } else if (type == USE_REFLECTANCE) { + return dimensionmap[1]; + } else if (type == USE_TEMPERATURE) { + return dimensionmap[2]; + } else if (type == USE_AMPLITUDE) { + return dimensionmap[3]; + } else if (type == USE_DEVIATION) { + return dimensionmap[4]; + } else if (type == USE_TYPE) { + return dimensionmap[5]; + } else if (type == USE_COLOR) { + return dimensionmap[6]; + } else if (type == USE_TIME) { + return dimensionmap[7]; + } else { + return 0; + } +} + + +unsigned int PointType::getPointDim() { return pointdim; } + +PointType PointType::deserialize(std::ifstream &f) { + unsigned int types; + f.read(reinterpret_cast(&types), sizeof(unsigned int)); + return PointType(types); +} + +void PointType::serialize(std::ofstream &f) { + f.write(reinterpret_cast(&types), sizeof(unsigned int)); +} + +unsigned int PointType::toFlags() const { return types; } + +bool PointType::hasType(unsigned int type) { + return types & type; +} + + +const unsigned int PointType::USE_NONE = 0; +const unsigned int PointType::USE_REFLECTANCE = 1; +const unsigned int PointType::USE_TEMPERATURE = 2; +const unsigned int PointType::USE_AMPLITUDE = 4; +const unsigned int PointType::USE_DEVIATION = 8; +const unsigned int PointType::USE_HEIGHT = 16; +const unsigned int PointType::USE_TYPE = 32; +const unsigned int PointType::USE_COLOR = 64; +const unsigned int PointType::USE_TIME = 128; +const unsigned int PointType::USE_INDEX = 256; + + +void PointType::useScan(Scan* scan) +{ + // clear pointers first + m_xyz = 0; m_rgb = 0; m_reflectance = 0; m_temperature = 0; m_amplitude = 0; m_type = 0; m_deviation = 0; + + // collectively load data to avoid unneccessary loading times due to split get("") calls + unsigned int types = DATA_XYZ; + if(hasColor()) types |= DATA_RGB; + if(hasReflectance()) types |= DATA_REFLECTANCE; + if(hasTemperature()) types |= DATA_TEMPERATURE; + if(hasAmplitude()) types |= DATA_AMPLITUDE; + if(hasType()) types |= DATA_TYPE; + if(hasDeviation()) types |= DATA_DEVIATION; + scan->get(types); + + // access data + try { + m_xyz = new DataXYZ(scan->get("xyz")); + if(hasColor()) m_rgb = new DataRGB(scan->get("rgb")); + if(hasReflectance()) m_reflectance = new DataReflectance(scan->get("reflectance")); + if(hasTemperature()) m_temperature = new DataTemperature(scan->get("temperature")); + if(hasAmplitude()) m_amplitude = new DataAmplitude(scan->get("amplitude")); + if(hasType()) m_type = new DataType(scan->get("type")); + if(hasDeviation()) m_deviation = new DataDeviation(scan->get("deviation")); + + // check if data is available, otherwise reset pointer to indicate that the scan doesn't prove this value + if(m_rgb && !m_rgb->valid()) { delete m_rgb; m_rgb = 0; } + if(m_reflectance && !m_reflectance->valid()) { delete m_reflectance; m_reflectance = 0; } + if(m_temperature && !m_temperature->valid()) { delete m_temperature; m_temperature = 0; } + if(m_amplitude && !m_amplitude->valid()) { delete m_amplitude; m_amplitude = 0; } + if(m_type && !m_type->valid()) { delete m_type; m_type = 0; } + if(m_deviation && !m_deviation->valid()) { delete m_deviation; m_deviation = 0; } + } catch(runtime_error& e) { + // unlock everything again + clearScan(); + throw e; + } +} + +void PointType::clearScan() +{ + // unlock data access + if(m_xyz) delete m_xyz; + if(hasColor() && m_rgb) delete m_rgb; + if(hasReflectance() && m_reflectance) delete m_reflectance; + if(hasTemperature() && m_temperature) delete m_temperature; + if(hasAmplitude() && m_amplitude) delete m_amplitude; + if(hasType() && m_type) delete m_type; + if(hasDeviation() && m_deviation) delete m_deviation; + + // TODO: scan->clear() on all of these types +} + + +unsigned int PointType::getScanSize(Scan* scan) +{ + return scan->size("xyz"); +} diff --git a/.svn/pristine/38/38638870846ffabe1b5a9ba2fffe492cc2b456b4.svn-base b/.svn/pristine/38/38638870846ffabe1b5a9ba2fffe492cc2b456b4.svn-base new file mode 100644 index 0000000..2cff39c --- /dev/null +++ b/.svn/pristine/38/38638870846ffabe1b5a9ba2fffe492cc2b456b4.svn-base @@ -0,0 +1,27 @@ +/** + * @file + * @brief IO of a 3D scan in riegl_txt file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_RIEGL_H__ +#define __SCAN_IO_RIEGL_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for Riegl scans + * + * The compiled class is available as shared object file + */ +class ScanIO_riegl_txt : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/39/3904271b6c0880687d06770b926d116b0b928fa4.svn-base b/.svn/pristine/39/3904271b6c0880687d06770b926d116b0b928fa4.svn-base new file mode 100644 index 0000000..6e15f8c --- /dev/null +++ b/.svn/pristine/39/3904271b6c0880687d06770b926d116b0b928fa4.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 uos_rrgbt 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/42/420f66c60f981fa8a46571f136e828917bef1901.svn-base b/.svn/pristine/42/420f66c60f981fa8a46571f136e828917bef1901.svn-base new file mode 100644 index 0000000..70e6bb4 --- /dev/null +++ b/.svn/pristine/42/420f66c60f981fa8a46571f136e828917bef1901.svn-base @@ -0,0 +1,831 @@ +/* + * wxshow implementation + * + * Copyright (C) Jan Elseberg + * + * Released under the GPL version 3. + * + */ + +#include "show_common.cc" +#include "wx/wx.h" +#include "wx/sizer.h" +#include "wx/glcanvas.h" +#include "show/wxshow.h" +#include "show/selectionframe.h" + + +class SelectionImpl : public Selection { + public: + + SelectionImpl( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Selection"), bool advanced_controls = false, const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( -1,-1 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL ) : Selection(parent, id, title, pos, size, style, advanced_controls) { + + if (pointtype.hasReflectance()) m_choice11->Append(wxT("reflectance")); + if (pointtype.hasTemperature()) m_choice11->Append(wxT("temperature")); + if (pointtype.hasAmplitude()) m_choice11->Append(wxT("amplitude")); + if (pointtype.hasDeviation()) m_choice11->Append(wxT("deviation")); + if (pointtype.hasType()) m_choice11->Append(wxT("type")); + + }; + // Virtual event handlers, overide them in your derived class + // + virtual void OnDrawPoints( wxCommandEvent& event ) { + if (event.IsChecked()) { + show_points = true; + } else { + show_points = false; + } + haveToUpdate = 1; + event.Skip(); + } + virtual void OnDrawCameras( wxCommandEvent& event ) + { + if (event.IsChecked()) { + show_cameras = true; + } else { + show_cameras = false; + } + haveToUpdate = 1; + event.Skip(); + } + virtual void OnDrawPaths( wxCommandEvent& event ) + { + if (event.IsChecked()) { + show_path = true; + } else { + show_path = false; + } + haveToUpdate = 1; + event.Skip(); + } + virtual void OnPointSize( wxSpinEvent& event ) + { + pointsize = event.GetPosition(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnFogChoice( wxCommandEvent& event ) + { + show_fog = event.GetSelection(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnFogDensity( wxSpinEvent& event ) { + wxSpinCtrlDbl *spinner = (wxSpinCtrlDbl*)event.GetEventObject(); + fogDensity = spinner->GetValue(); + haveToUpdate = 1; + + event.Skip(); + } + virtual void OnColorValue( wxCommandEvent& event ) + { + // we can't use the glui-show function for changing the color as a listbox is used here instead + switch (event.GetSelection()) { + case 0: + cm->setCurrentType(PointType::USE_HEIGHT); + break; + case 1: + if (pointtype.hasReflectance()) { + cm->setCurrentType(PointType::USE_REFLECTANCE); + break; + } + case 2: + if (pointtype.hasTemperature()) { + cm->setCurrentType(PointType::USE_TEMPERATURE); + break; + } + case 3: + if (pointtype.hasAmplitude()) { + cm->setCurrentType(PointType::USE_AMPLITUDE); + break; + } + case 4: + if (pointtype.hasDeviation()) { + cm->setCurrentType(PointType::USE_DEVIATION); + break; + } + case 5: + if (pointtype.hasType()) { + cm->setCurrentType(PointType::USE_TYPE); + break; + } + default: + break; + }; + haveToUpdate = 1; + resetMinMax(0); + event.Skip(); + } + virtual void OnColorMap( wxCommandEvent& event ) + { + listboxColorMapVal = event.GetSelection(); + changeColorMap(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnColorType( wxCommandEvent& event ) { + colorScanVal = event.GetSelection(); + setScansColored(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnColorMinVal( wxSpinEvent& event ) { + //mincolor_value = event.GetPosition(); + wxSpinCtrlDbl *spinner = (wxSpinCtrlDbl*)event.GetEventObject(); + mincolor_value = spinner->GetValue(); + minmaxChanged(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnColorMaxVal( wxSpinEvent& event ) { + //maxcolor_value = event.GetPosition(); + wxSpinCtrlDbl *spinner = (wxSpinCtrlDbl*)event.GetEventObject(); + maxcolor_value = spinner->GetValue(); + minmaxChanged(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnColorResetMinMax( wxCommandEvent& event ) { + resetMinMax(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnInvert( wxCommandEvent& event ) { + invertView(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnAnimDelay( wxSpinEvent& event ) { + anim_delay = event.GetPosition(); + event.Skip(); + } + virtual void OnAnimate( wxCommandEvent& event ) { + startAnimation(0); + event.Skip(); + } + virtual void OnCameraFile( wxCommandEvent& event ) { + wxString s = event.GetString(); + const wxCharBuffer buffer = wxString(event.GetString()).mb_str(wxConvISO8859_1); + const char* cc = buffer.data(); + strcpy(path_file_name, cc); + event.Skip(); + } + virtual void OnCameraSavePath( wxCommandEvent& event ) { + savePath(0); + event.Skip(); + } + virtual void OnCameraLoadPath( wxCommandEvent& event ) { + loadPath(0); + event.Skip(); + } + virtual void OnCameraLoadRobotPath( wxCommandEvent& event ) { + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + //temp variable + double *temp; + //Now, lets go to the last of each frame file to + //extract the transformation matrix obtained + //after scan matching has been done. + glMultMatrixd(MetaMatrix[i].back()); + + //temp is final transformation matrix + temp = MetaMatrix[i].back(); + + Point campos(temp[12], temp[13] + 100, temp[14]); + + // calculate lookat point + Point lookat(0, 0, 50); + Point up(0, 50, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = temp[i]; + lookat.transform(tmat); + lookat.x = lookat.x ; + lookat.y = lookat.y + 100; + lookat.z = lookat.z ; + + up.transform(tmat); + up.x = up.x ; + up.y = up.y + 100; + up.z = up.z ; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + } + //signal for the update of scene + haveToUpdate = 1; + event.Skip(); + } + virtual void OnSaveAnimation( wxCommandEvent& event ) { + if (event.IsChecked()) { + save_animation = true; + } else { + save_animation = false; + } + haveToUpdate = 1; + event.Skip(); + } + virtual void OnAnimatePath( wxCommandEvent& event ) { + pathAnimate(0); + event.Skip(); + } + virtual void OnPositionFile( wxCommandEvent& event ) { + wxString s = event.GetString(); + const wxCharBuffer buffer = wxString(event.GetString()).mb_str(wxConvISO8859_1); + const char* cc = buffer.data(); + strcpy(pose_file_name, cc); + event.Skip(); + } + virtual void OnPositionSave( wxCommandEvent& event ) { + savePose(0); + event.Skip(); + } + virtual void OnPositionLoad( wxCommandEvent& event ) { + loadPose(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnFactor( wxSpinEvent& event ) { + factor = event.GetPosition(); + event.Skip(); + } + virtual void OnSaveImage( wxCommandEvent& event ) { + saveImage(0); + event.Skip(); + } + virtual void OnSelectionFile( wxCommandEvent& event ) { + wxString s = event.GetString(); + const wxCharBuffer buffer = wxString(event.GetString()).mb_str(wxConvISO8859_1); + const char* cc = buffer.data(); + strcpy(selection_file_name, cc); + event.Skip(); + } + virtual void OnSelectionSave( wxCommandEvent& event ) { + saveSelection(0); + event.Skip(); + } + virtual void OnSelectionClear( wxCommandEvent& event ) { + clearSelection(0); + event.Skip(); + } + virtual void OnSelectionSU( wxCommandEvent& event ) { + if (event.IsChecked()) { + selectOrunselect = true; + } else { + selectOrunselect = false; + } + event.Skip(); + } + virtual void OnSelectionSV( wxCommandEvent& event ) { + if (event.IsChecked()) { + select_voxels = true; + } else { + select_voxels = false; + } + event.Skip(); + } + virtual void OnSelectionDepth( wxSpinEvent& event ) { + selection_depth = event.GetPosition(); + event.Skip(); + } + virtual void OnSelectionBrushsize( wxSpinEvent& event ) { + brush_size = event.GetPosition(); + event.Skip(); + } + virtual void OnFrameSpinner( wxSpinEvent& event ) { + current_frame = event.GetPosition(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnFramerateSpinner( wxSpinEvent& event ) { + idealfps = event.GetPosition(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnFarplaneSpinner( wxSpinEvent& event ) { + maxfardistance = event.GetPosition(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnNearplaneSpinner( wxSpinEvent& event ) { + neardistance = event.GetPosition(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnCycleLOD( wxCommandEvent& event ) { + ::cycleLOD(); + haveToUpdate = 1; + event.Skip(); + } + + virtual void OnLODAdaption( wxSpinEvent& event ) { + adaption_rate = ((wxSpinCtrlDbl*)event.GetEventObject())->GetValue(); + haveToUpdate = 1; + event.Skip(); + } + + public: + void updateControls() { + if (!MetaMatrix.empty()) { + frame_spin->SetRange(0, MetaMatrix[0].size()-1); + frame_spin->SetValue(current_frame); + } + m_spinCtrl61->SetValue(mincolor_value); + m_spinCtrl6->SetValue(maxcolor_value); + } + +}; + +class ControlImpl : public Controls { + + public: + + ControlImpl( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Controls"), const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( -1,-1 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL ) + : Controls( parent, id, title, pos, size, style ) {} + + virtual void OnApexAngle( wxSpinEvent& event ) { + haveToUpdate = 2; + cangle = ((wxSpinCtrlDbl*)event.GetEventObject())->GetValue(); + event.Skip(); + } + virtual void OnParallelZoom( wxSpinEvent& event ) { + haveToUpdate = 2; + pzoom = ((wxSpinCtrlDbl*)event.GetEventObject())->GetValue(); + event.Skip(); + } + virtual void OnTopView( wxCommandEvent& event ) { + topView(); // TODO update controls + haveToUpdate = 2; + event.Skip(); + } + virtual void OnResetPosition( wxCommandEvent& event ) { + resetView(0); + event.Skip(); + } + virtual void OnChooseCamera( wxSpinEvent& event ) { + cam_choice = event.GetPosition(); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnAddCamera( wxCommandEvent& event ) { + callAddCamera(1); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnDeleteCamera( wxCommandEvent& event ) { + callDeleteCamera(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnMouseNav( wxCommandEvent& event ) { + /* + if (event.IsChecked()) { + cameraNavMouseMode = true; + } else { + cameraNavMouseMode = false; + }*/ + // TODO implement secondary navigation procedure + event.Skip(); + } + virtual void OnAlwaysAllPoints( wxCommandEvent& event ) { + changePointMode(0); + haveToUpdate = 1; + event.Skip(); + } + virtual void OnAlwaysReducePoints( wxCommandEvent& event ) { + changePointMode(1); + haveToUpdate = 1; + event.Skip(); + } + + public: + void updateControls() { + apex_spinner->SetValue(cangle); + parallel_spinner->SetValue(pzoom); + + if(showTopView) { + apex_spinner->Disable(); + parallel_spinner->Enable(); + } else { + parallel_spinner->Disable(); + apex_spinner->Enable(); + } + + switch(pointmode) { + case -1: + always_box->SetValue(false); + alwaysred_box->SetValue(true); + break; + case 0: + always_box->SetValue(false); + alwaysred_box->SetValue(false); + break; + case 1: + always_box->SetValue(true); + alwaysred_box->SetValue(false); + break; + } + + + camera_spinner->SetRange(1, cams.size()); + camera_spinner->SetValue(cam_choice); + } +}; + + +class wxShow: public wxApp +{ + virtual bool OnInit(); + + SelectionImpl *selection; + ControlImpl *controls; + + wxFrame *frame; + BasicGLPane * glPane; +public: + void OnClose(wxCloseEvent& event); + void updateControls(){ + controls->updateControls(); + selection->updateControls(); + } + +}; + + +static wxShow *globalGUI = 0; + +void wxShow::OnClose(wxCloseEvent& event) { + exit(0); +} + +IMPLEMENT_APP(wxShow) + + +bool wxShow::OnInit() +{ + + globalGUI = this; + // wxwidgets saves the argv as wxchar, which is 2-byte unicode, so we must convert it back + char **new_argv = new char*[argc]; + for (int i = 0; i < argc; i++) { + const wxCharBuffer buffer = wxString(wxApp::argv[i]).mb_str(wxConvISO8859_1); + const char* cc = buffer.data(); + new_argv[i] = new char[ strlen(cc) +1 ]; + strcpy(new_argv[i], cc); + } + initShow(argc, new_argv); + //glClearColor(0.0, 0.0, 0.0, 0.0); + + wxBoxSizer* sizer = new wxBoxSizer(wxHORIZONTAL); + frame = new wxFrame((wxFrame *)NULL, -1, wxT("Viewer"), wxPoint(START_X, START_Y), wxSize(START_WIDTH, START_HEIGHT)); + + int args[] = {WX_GL_RGBA, WX_GL_DOUBLEBUFFER, WX_GL_DEPTH_SIZE, 16, 0}; + + glPane = new BasicGLPane( (wxFrame*) frame, args); + sizer->Add(glPane, 1, wxEXPAND); + + frame->SetSizer(sizer); + frame->SetAutoLayout(true); + + + selection = new SelectionImpl( (wxWindow*)NULL, wxID_ANY, wxT("Selection"), advanced_controls, wxPoint(START_X + START_WIDTH + 50, START_Y + 30) ); + controls = new ControlImpl( (wxWindow*)NULL, wxID_ANY, wxT("Controls"), wxPoint(START_X, START_Y + START_HEIGHT + 20 ) ); + + selection->SetSize(wxSize(200,393) ); + selection->SetAutoLayout(true); + selection ->Show(); + selection ->Layout(); + + controls->SetAutoLayout(true); + controls ->Show(); + + frame->Show(); + frame->Connect( wxEVT_CLOSE_WINDOW, wxCloseEventHandler( wxShow::OnClose ) ); + + return true; +} + +BEGIN_EVENT_TABLE(BasicGLPane, wxGLCanvas) +EVT_IDLE(BasicGLPane::idle_event) +EVT_MOTION(BasicGLPane::mouseMoved) +EVT_MOUSE_EVENTS(BasicGLPane::mouseEvent) + //EVT_LEFT_DOWN(BasicGLPane::mouseDown) +//EVT_LEFT_UP(BasicGLPane::mouseReleased) +//EVT_RIGHT_DOWN(BasicGLPane::rightClick) +//EVT_LEAVE_WINDOW(BasicGLPane::mouseLeftWindow) +EVT_SIZE(BasicGLPane::resized) +EVT_KEY_DOWN(BasicGLPane::keyPressed) +//EVT_KEY_UP(BasicGLPane::keyReleased) +//EVT_MOUSEWHEEL(BasicGLPane::mouseWheelMoved) +EVT_PAINT(BasicGLPane::render) +END_EVENT_TABLE() + + +// some useful events to use +void BasicGLPane::mouseEvent(wxMouseEvent& event) +{ + SetFocus(); + int x = event.GetX(); + int y = event.GetY(); + + if (event.Dragging()) { + } else if (event.IsButton()) { + int button, state; + if (event.ButtonDown()) { + state = GLUT_DOWN; + } else { + state = GLUT_UP; + } + + if (event.Button(1)) { + button = GLUT_LEFT_BUTTON; + } else if (event.Button(2)) { + button = GLUT_MIDDLE_BUTTON; + } else if (event.Button(3)) { + button = GLUT_RIGHT_BUTTON; + } else { + button = 0; + } + CallBackMouseFunc(button, state, x, y); + } +} + +void BasicGLPane::idle() { + if(glutGetWindow() != window_id) + glutSetWindow(window_id); + + /* + static unsigned long start = GetCurrentTimeInMilliSec(); + // return as nothing has to be updated + if (haveToUpdate == 0) { + if ((GetCurrentTimeInMilliSec()-start) > 100) { + paint(true); + start = GetCurrentTimeInMilliSec(); + } + return; + } + start = GetCurrentTimeInMilliSec(); + */ + if (haveToUpdate == 0) { + if (!fullydisplayed && !mousemoving && !keypressed) { + glDrawBuffer(buffermode); + //Call the display function + DisplayItFunc(GL_RENDER, true); + } + return; + } + + + // case: display is invalid - update it + if (haveToUpdate == 1) { + paint(); + haveToUpdate = 0; + return; + } + // case: display is invalid - update it with all points +/*if (haveToUpdate == 7) { + showall = true; + paint(); + haveToUpdate = 0; + return; + } + */ + + // case: camera angle is changed - instead of repeating code call Reshape, + // since these OpenGL commands are the same + if (haveToUpdate == 2) { + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + paint(); + haveToUpdate = 0; + return; + } + + // case: animation + if(haveToUpdate == 3 ){ + frameNr += 1; + if(!(MetaMatrix.size() > 1 && frameNr < (int) MetaMatrix[1].size())){ + frameNr = 0; + haveToUpdate = 4; + return; + } + paint(); + + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(frameNr,5) + ".ppm"; + cout << "write " << filename << endl; + glDumpWindowPPM(filename.c_str(),0); + + } + + } +#ifdef _MSC_VER + Sleep(300); + Sleep(anim_delay); +#else + usleep(anim_delay * 10000); +#endif + + if (haveToUpdate == 4) { // stop animation + frameNr = 0; // delete these lines if you want a 'continue' functionality. + haveToUpdate = 1; + } + + // case: path animation + if(haveToUpdate == 6){ + + if (path_iterator == 0) { + oldcamNavMode = cameraNavMouseMode; // remember state of old mousenav + cameraNavMouseMode = 0; + } + + + + //check if the user wants to animate both + //scan matching and the path at the same + //time + + //cout << "path_iterator: " << path_iterator << endl; + if(path_iterator < path_vectorX.size()){ // standard animation case + + //call the path animation function + //hide both the cameras and the path + show_cameras = 0; + show_path = 0; + //increase the iteration count + + path_iterator += 1; + //cout << "i am here" << endl; + //repaint the screen + paint(); + + //save the animation + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(path_iterator,5) + ".ppm"; + string jpgname = scan_dir + "animframe" + to_string(path_iterator,5) + ".jpg"; + cout << "written " << filename << " of " << path_vectorX.size() << " files" << endl; + glWriteImagePPM(filename.c_str(), factor, 0); + haveToUpdate = 6; + + } + }else{ // animation has just ended + cameraNavMouseMode = oldcamNavMode; + show_cameras = 1; + show_path = 1; + //cout << "i am here instead" << endl; + haveToUpdate = 0; + } + } +} + +void BasicGLPane::idle_event(wxIdleEvent& event) +{ + idle(); + //event.RequestMore(); +} + +void BasicGLPane::mouseMoved(wxMouseEvent& event) +{ + int x = event.GetX(); + int y = event.GetY(); + + CallBackMouseMotionFunc(x, y); + //idle(); +} +void BasicGLPane::mouseDown(wxMouseEvent& event) {} +void BasicGLPane::mouseWheelMoved(wxMouseEvent& event) {} +void BasicGLPane::mouseReleased(wxMouseEvent& event) {} +void BasicGLPane::rightClick(wxMouseEvent& event) {} +void BasicGLPane::mouseLeftWindow(wxMouseEvent& event) {} + + +void BasicGLPane::keyPressed(wxKeyEvent& event) { + KeyboardFunc(event.GetKeyCode(), event.CmdDown(), event.AltDown(), event.ShiftDown()); +} + + + +void BasicGLPane::keyReleased(wxKeyEvent& event) {} + +// Vertices and faces of a simple cube to demonstrate 3D render +// source: http://www.opengl.org/resources/code/samples/glut_examples/examples/cube.c +GLfloat v[8][3]; +GLint faces[6][4] = { /* Vertex indices for the 6 faces of a cube. */ + {0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4}, + {4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} }; + + + +BasicGLPane::BasicGLPane(wxFrame* parent, int* args) : + wxGLCanvas(parent, wxID_ANY, args, wxDefaultPosition, wxDefaultSize, wxFULL_REPAINT_ON_RESIZE||wxWANTS_CHARS) +{ + + m_context = new wxGLContext(this); + // prepare a simple cube to demonstrate 3D render + // source: http://www.opengl.org/resources/code/samples/glut_examples/examples/cube.c + v[0][0] = v[1][0] = v[2][0] = v[3][0] = -1; + v[4][0] = v[5][0] = v[6][0] = v[7][0] = 1; + v[0][1] = v[1][1] = v[4][1] = v[5][1] = -1; + v[2][1] = v[3][1] = v[6][1] = v[7][1] = 1; + v[0][2] = v[3][2] = v[4][2] = v[7][2] = 1; + v[1][2] = v[2][2] = v[5][2] = v[6][2] = -1; + + // To avoid flashing on MSW + SetBackgroundStyle(wxBG_STYLE_CUSTOM); +} + +BasicGLPane::~BasicGLPane() +{ + delete m_context; +} + +void BasicGLPane::resized(wxSizeEvent& evt) +{ +// wxGLCanvas::OnSize(evt); + wxSize s = evt.GetSize(); + CallBackReshapeFunc(s.GetWidth(), s.GetHeight()); + + Refresh(); +} + +/** Inits the OpenGL viewport for drawing in 3D. */ +void BasicGLPane::prepare3DViewport(int topleft_x, int topleft_y, int bottomrigth_x, int bottomrigth_y) +{ + + glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // Black Background + glClearDepth(1.0f); // Depth Buffer Setup + glEnable(GL_DEPTH_TEST); // Enables Depth Testing + glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do + glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); + + glEnable(GL_COLOR_MATERIAL); + + glViewport(topleft_x, topleft_y, bottomrigth_x-topleft_x, bottomrigth_y-topleft_y); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + + float ratio_w_h = (float)(bottomrigth_x-topleft_x)/(float)(bottomrigth_y-topleft_y); + gluPerspective(45 /*view angle*/, ratio_w_h, 0.1 /*clip close*/, 200 /*clip far*/); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + +} + +int BasicGLPane::getWidth() +{ + return GetSize().x; +} + +int BasicGLPane::getHeight() +{ + return GetSize().y; +} + + +void BasicGLPane::render( wxPaintEvent& evt ) +{ + paint(); +} + + +void BasicGLPane::paint(bool interruptable) +{ + if(!IsShown()) return; + wxGLCanvas::SetCurrent(*m_context); + wxPaintDC(this); // only to be used in paint events. use wxClientDC to paint outside the paint event + + glDrawBuffer(buffermode); + // delete framebuffer and z-buffer + + //Call the display function + DisplayItFunc(GL_RENDER, interruptable); + + // show the rednered scene + if (!interruptable) { // interruptible draw is single buffered + SwapBuffers(); + } +// ProcessPendingEvents(); +} + + +void updateControls() { + globalGUI->updateControls(); +} +void updatePointModeControls() {} +void updateTopViewControls() {} +void resetRotationButton() {} +void updateCamControls() {} + +static bool interrupted; + +void interruptDrawing() { + interrupted = true; +} +void checkForInterrupt() { + interrupted = false; +} +bool isInterrupted() { + globalGUI->Dispatch(); + globalGUI->ProcessPendingEvents(); + return interrupted; +} diff --git a/.svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base b/.svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base new file mode 100644 index 0000000..03e0264 --- /dev/null +++ b/.svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base @@ -0,0 +1,23 @@ +FIND_PACKAGE(OpenCV REQUIRED) + +SET(FBR_IO_SRC scan_cv.cc) +add_library(fbr_cv_io STATIC ${FBR_IO_SRC}) + +SET(FBR_PANORAMA_SRC panorama.cc) +add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC}) + +SET(FBR_FEATURE_SRC feature.cc) +add_library(fbr_feature STATIC ${FBR_FEATURE_SRC}) + +SET(FBR_FEATURE_MATCHER_SRC feature_matcher.cc) +add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC}) + +SET(FBR_REGISTRATION_SRC registration.cc) +add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC}) + +IF(WITH_FBR) +SET(FBR_LIBS scan ANN ${OpenCV_LIBS}) + +add_executable(featurebasedregistration feature_based_registration.cc fbr_global.cc) +target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS}) +ENDIF(WITH_FBR) diff --git a/.svn/pristine/49/49302f71941a2340035fed68892f230e95be8063.svn-base b/.svn/pristine/49/49302f71941a2340035fed68892f230e95be8063.svn-base new file mode 100644 index 0000000..5c0fd82 --- /dev/null +++ b/.svn/pristine/49/49302f71941a2340035fed68892f230e95be8063.svn-base @@ -0,0 +1,256 @@ +/** + * @file + * @brief Representation of a 3D point type + * @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany. + */ + +#ifndef __POINT_TYPE_H__ +#define __POINT_TYPE_H__ + +#include "point.h" +#include "data_types.h" + +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; + +#include +#include +#include + +class Scan; + + + +class PointType { +public: + + static const unsigned int USE_NONE; + static const unsigned int USE_REFLECTANCE; + static const unsigned int USE_TEMPERATURE; + static const unsigned int USE_AMPLITUDE; + static const unsigned int USE_DEVIATION; + static const unsigned int USE_HEIGHT; + static const unsigned int USE_TYPE; + static const unsigned int USE_COLOR; + static const unsigned int USE_TIME; + static const unsigned int USE_INDEX; + + PointType(); + + PointType(unsigned int _types); + + bool hasReflectance(); + bool hasTemperature(); + bool hasAmplitude(); + bool hasDeviation(); + bool hasType(); + bool hasColor(); + bool hasTime(); + bool hasIndex(); + + unsigned int getReflectance(); + unsigned int getTemperature(); + unsigned int getAmplitude(); + unsigned int getDeviation(); + unsigned int getTime(); + unsigned int getIndex(); + unsigned int getType(); + unsigned int getType(unsigned int type); + + + unsigned int getPointDim(); + + static PointType deserialize(std::ifstream &f); + + void serialize(std::ofstream &f); + unsigned int toFlags() const; + + template + T *createPoint(const Point &P, unsigned int index=0); + + template + Point createPoint(T *p); + + //! Aquire DataPointer objects from \a scan, determined by its types + void useScan(Scan* scan); + + //! Release the DataPointer objects + void clearScan(); + + //! Create a point with attributes via the DataPointers from the scan + template + T* createPoint(unsigned int i, unsigned int index = 0); + + //! Create an array with coordinate+attribute array per point with transfer of ownership + template + T** createPointArray(Scan* scan); + +private: + /** + * collection of flags + */ + unsigned int types; + /** + * Derived from types: 3 spatial dimensions + 1 for each flag set + **/ + unsigned int pointdim; + + /** + * Stores the size of each point in bytes + **/ + unsigned int pointbytes; + + /** + * Derived from types, to map type to the array index for each point + **/ + int dimensionmap[9]; + + bool hasType(unsigned int type); + + unsigned int getScanSize(Scan* scan); + + DataXYZ* m_xyz; + DataRGB* m_rgb; + DataReflectance* m_reflectance; + DataTemperature* m_temperature; + DataAmplitude* m_amplitude; + DataType* m_type; + DataDeviation* m_deviation; +}; + + +template +T *PointType::createPoint(const Point &P, unsigned int index ) { + unsigned int counter = 0; + + T *p = new T[pointdim]; + p[counter++] = P.x; + p[counter++] = P.y; + p[counter++] = P.z; + if (types & USE_REFLECTANCE) { + p[counter++] = P.reflectance; + } + if (types & USE_TEMPERATURE) { + p[counter++] = P.temperature; + } + if (types & USE_AMPLITUDE) { + p[counter++] = P.amplitude; + } + if (types & USE_DEVIATION) { + p[counter++] = P.deviation; + } + if (types & USE_TYPE) { + p[counter++] = P.type; + } + if (types & USE_COLOR) { + memcpy(&p[counter], P.rgb, 3); + counter++; + } + if (types & USE_TIME) { +// p[counter++] = P.timestamp; + } + if (types & USE_INDEX) { + p[counter++] = index; + } + + return p; +} + +template +Point PointType::createPoint(T *p) { + Point P; + unsigned int counter = 0; + + P.x = p[counter++]; + P.y = p[counter++]; + P.z = p[counter++]; + if (types & USE_REFLECTANCE) { + P.reflectance = p[counter++]; + } + if (types & USE_TEMPERATURE) { + P.temperature = p[counter++]; + } + if (types & USE_AMPLITUDE) { + P.amplitude = p[counter++]; + } + if (types & USE_DEVIATION) { + P.deviation = p[counter++]; + } + if (types & USE_TYPE) { + P.type = p[counter++]; + } + if (types & USE_COLOR) { + memcpy(P.rgb, &p[counter], 3); + counter++; + } + if (types & USE_TIME) { +// P.timestamp = p[counter++]; + } + + return P; +} + +template +T *PointType::createPoint(unsigned int i, unsigned int index) { + unsigned int counter = 0; + T* p = new T[pointdim]; + + for(unsigned int j = 0; j < 3; ++j) + p[counter++] = (*m_xyz)[i][j]; + + // if a type is requested try to write the value if the scan provided one + if (types & USE_REFLECTANCE) { + p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0); + } + if (types & USE_TEMPERATURE) { + p[counter++] = (m_temperature? (*m_temperature)[i]: 0); + } + if (types & USE_AMPLITUDE) { + p[counter++] = (m_amplitude? (*m_amplitude)[i]: 0); + } + if (types & USE_DEVIATION) { + p[counter++] = (m_deviation? (*m_deviation)[i]: 0); + } + if (types & USE_TYPE) { + p[counter++] = (m_type? (*m_type)[i]: 0); + } + if (types & USE_COLOR) { + if(m_rgb) + memcpy(&p[counter], (*m_rgb)[i], 3); + else + p[counter] = 0; + counter++; + } + if (types & USE_TIME) { + } + if (types & USE_INDEX) { + p[counter++] = index; + } + + return p; +} + +template +T** PointType::createPointArray(Scan* scan) +{ + // access data with prefetching + useScan(scan); + + // create a float array with requested attributes by pointtype via createPoint + unsigned int nrpts = getScanSize(scan); + T** pts = new T*[nrpts]; + for(unsigned int i = 0; i < nrpts; i++) { + pts[i] = createPoint(i); + } + + // unlock access to data, remove unneccessary data fields + clearScan(); + + return pts; +} + +#endif diff --git a/.svn/pristine/58/58db7cca2a4b1aa9365d0494071c6923398d845a.svn-base b/.svn/pristine/58/58db7cca2a4b1aa9365d0494071c6923398d845a.svn-base new file mode 100644 index 0000000..ff4fa84 --- /dev/null +++ b/.svn/pristine/58/58db7cca2a4b1aa9365d0494071c6923398d845a.svn-base @@ -0,0 +1,90 @@ +/** + * @file + * @brief IO of a 3D scan + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_H__ +#define __SCAN_IO_H__ + +#include "slam6d/io_types.h" +#include "slam6d/pointfilter.h" + +#include +#include +#include +#include + + + +/** + * @brief IO of a 3D scan + * + * This class needs to be instantiated by a class loading + * 3D scans from different file formats. + */ +class ScanIO { +public: + /** + * Read a directory and return all possible scans in the [start,end] interval. + * + * @param dir_path The directory from which to read the scans + * @param start Starting index + * @param end Last index + * @return List of IO-specific identifiers of scans, matching the search + */ + virtual std::list readDirectory(const char* dir_path, unsigned int start, unsigned int end) = 0; + + /** + * Reads the pose from a dedicated pose file or from the scan file. + * + * @param dir_path The directory the scan is contained in + * @param scan_identifier IO-specific identifier for the particular scan + * @param pose Pointer to an existing double[6] array where the pose is saved in + */ + virtual void readPose(const char* dir_path, const char* identifier, double* pose) = 0; + + /** + * Given a scan identifier, load the contents of this particular scan. + * + * @param dir_path The directory the scan is contained in + * @param identifier IO-specific identifier for the particular scan + * @param filter Filter object which each point is tested on by its position + */ + virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz = 0, std::vector* rgb = 0, std::vector* reflectance = 0, std::vector* temperature = 0, std::vector* amplitude = 0, std::vector* type = 0, std::vector* deviation = 0) = 0; + + /** + * Returns whether this ScanIO can load the requested data from a scan. + * + * @param type data channel request + * @return whether it's supported or not + */ + virtual bool supports(IODataType type) = 0; + + /** + * @brief Global mapping of io_types to single instances of ScanIOs. + * + * If the ScanIO doesn't exist, it will be created and saved in a map. + * Otherwise, the matching ScanIO will be returned. + * + * @param type Key identifying the ScanIO + * @return The newly created or found ScanIO + */ + static ScanIO* getScanIO(IOType iotype); + + //! Delete all ScanIO instances and (lazy) try to unload the libraries. + static void clearScanIOs(); +private: + static std::map m_scanIOs; +}; + +// Since the shared object files are loaded on the fly, we +// need class factories + +// the types of the class factories +typedef ScanIO* create_sio(); +typedef void destroy_sio(ScanIO*); + +#endif diff --git a/.svn/pristine/5b/5b6818356d295bdacd23f82f454bd7bf0fc53d8c.svn-base b/.svn/pristine/5b/5b6818356d295bdacd23f82f454bd7bf0fc53d8c.svn-base new file mode 100644 index 0000000..6bf4c63 --- /dev/null +++ b/.svn/pristine/5b/5b6818356d295bdacd23f82f454bd7bf0fc53d8c.svn-base @@ -0,0 +1,615 @@ +/* + * scan_io_velodyne implementation + * + * Copyright (C) Mohammad Faisal, Dorit Borrmann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Andreas Nuechter. Jacobs University Bremen gGmbH + * @author Dorit Borrmann. Smart Systems Group, Jacobs University Bremen gGmbH, Germany. + * @author Mohammad Faisal. Smart Systems Group, Jacobs University Bremen gGmbH, Germany. + */ +#include "slam6d/point.h" +#include "veloslam/velodefs.h" +#include "scanio/scan_io_velodyne.h" +#include "slam6d/globals.icc" +#include +#include +#include +using std::ifstream; +using std::ostrstream; +using std::cerr; +using std::endl; +using std::ends; + +#include +using std::swap; + +#include +#include +#include +#include +#include + +using namespace std; + +#ifdef _MSC_VER +#include +#endif +#include +#include +using namespace boost::filesystem; + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".bin" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + +#define BLOCK_OFFSET 42+16 + +#define BLOCK_SIZE 1206 +#define CIRCLELENGTH 360 +#define VELODYNE_NUM_LASERS 64 +#define CircleBufferSize CIRCLELENGTH*32*12 +#define CIRCLEROUND CIRCLELENGTH*6 + +#define RADIANS_PER_LSB 0.0174532925 +#define METERS_PER_LSB 0.002 +#define METERS_PER_CM 0.01 + +#define TWOPI_INV (0.5/M_PI) +#define TWOPI (2*M_PI) + +typedef struct raw_packet + { + unsigned char dat[1200]; + unsigned short revolution; + unsigned char status[4]; + } raw_packet_t; + + +typedef unsigned char BYTE; + +long CountOfLidar = 0; + +//Changes the variable to be filled dynamically. Added a 6th variable to enable or disable a specific sensor +double velodyne_calibrated[VELODYNE_NUM_LASERS][6]; + +/** + * Method to fill the calibration data using the Velodyne64 calibration data from Wuhan. + */ +int backup() +{ + double velodyne_calibrated1[64][6] = + { + { -7.1581192, -4.5, 102, 21.560343, -2.5999999, 1}, + { -6.8178215, -3.4000001, 125, 21.516994, 2.5999999, 1}, + { 0.31782165, 3, 130, 20.617426, -2.5999999, 1}, + { 0.65811908, 4.5999999, 128, 20.574717, 2.5999999, 1}, + { -6.4776502, -0.5, 112, 21.473722, -2.5999999, 1}, + { -6.1375928, 1, 125, 21.430525, 2.5999999, 1}, + { -8.520812, -1.5, 106, 21.734608, -2.5999999, 1}, + { -8.1798887, 0.40000001, 127, 21.690901, 2.5999999, 1}, + { -5.797637, 4, 111, 21.387396, -2.5999999, 1}, + { -5.4577708, 5.5, 126, 21.34433, 2.5999999, 1}, + { -7.8391404, 3.0999999, 113, 21.647291, -2.5999999, 1}, + { -7.4985547, 4.5, 123, 21.603773, 2.5999999, 1}, + { -3.0802133, -4.5, 105, 21.044245, -2.5999999, 1}, + { -2.7406337, -3.2, 133, 21.001518, 2.5999999, 1}, + { -5.1179824, -5.5, 110, 21.301321, -2.5999999, 1}, + { -4.7782598, -4, 129, 21.258366, 2.5999999, 1}, + { -2.4010365, -0.2, 111, 20.958813, -2.5999999, 1}, + { -2.0614092, 1, 130, 20.916126, 2.5999999, 1}, + { -4.4385905, -1.2, 115, 21.215462, -2.5999999, 1}, + { -4.0989642, 0, 133, 21.172602, 2.5999999, 1}, + { -1.7217404, 3.8, 113, 20.873451, -2.5999999, 1}, + { -1.3820176, 5, 130, 20.830786, 2.5999999, 1}, + { -3.7593663, 3, 117, 21.129782, -2.5999999, 1}, + { -3.4197867, 4.5, 129, 21.086998, 2.5999999, 1}, + { 0.998555, -4.5, 107, 20.531982, -2.5999999, 1}, + { 1.339141, -3.2, 131, 20.489222, 2.5999999, 1}, + { -1.0422293, -5.4000001, 128, 20.788124, -2.5999999, 1}, + { -0.70236301, -4, 134, 20.745461, 2.5999999, 1}, + { 1.679889, -0.5, 124, 20.446428, -2.5999999, 1}, + { 2.0208123, 1, 136, 20.403601, 2.5999999, 1}, + { -0.36240739, -1.5, 131, 20.702793, -2.5999999, 1}, + { -0.022349782, 0.2, 136, 20.660116, 2.5999999, 1}, + { -22.737886, -7.8000002, 101, 16.019152, -2.5999999, 1}, + { -22.226072, -5, 88, 15.954137, 2.5999999, 1}, + { -11.513928, 4.5, 121, 14.680806, -2.5999999, 1}, + { -11.002114, 7.4000001, 88, 14.623099, 2.5999999, 1}, + { -21.714685, -1, 94, 15.889649, -2.5999999, 1}, + { -21.203688, 2, 88, 15.82566, 2.5999999, 1}, + { -24.790272, -2.5, 114, 16.284933, -2.5999999, 1}, + { -24.276321, 0.5, 89, 16.217583, 2.5999999, 1}, + { -20.693031, 6, 98, 15.762167, -2.5999999, 1}, + { -20.182682, 9, 92, 15.699132, 2.5999999, 1}, + { -23.762968, 4.5, 107, 16.15085, -2.5999999, 1}, + { -23.250172, 7.5, 80, 16.084715, 2.5999999, 1}, + { -16.615318, -7.5, 121, 15.26925, -2.5999999, 1}, + { -16.105938, -5, 92, 15.209245, 2.5999999, 1}, + { -19.672594, -9, 119, 15.63654, -2.5999999, 1}, + { -19.162729, -6, 89, 15.574372, 2.5999999, 1}, + { -15.596496, -1, 109, 15.14954, -2.5999999, 1}, + { -15.086954, 2, 88, 15.090119, 2.5999999, 1}, + { -18.653046, -2, 117, 15.51261, -2.5999999, 1}, + { -18.143503, 0.69999999, 88, 15.451235, 2.5999999, 1}, + { -14.577271, 5.5, 112, 15.030966, -2.5999999, 1}, + { -14.067405, 8.3999996, 87, 14.972065, 2.5999999, 1}, + { -17.634062, 5, 119, 15.390228, -6.1999998, 1}, + { -17.124681, 7.5, 97, 15.329572, 2.5999999, 1}, + { -10.489829, -7.5, 119, 14.565539, -2.5999999, 1}, + { -9.9770317, -4.6999998, 95, 14.508112, 2.5999999, 1}, + { -13.557318, -8.5, 126, 14.913401, -2.5999999, 1}, + { -13.046968, -6, 92, 14.854958, 2.5999999, 1}, + { -9.4636793, -1, 112, 14.450804, -2.5999999, 1}, + { -8.949728, 1.5, 93, 14.3936, 2.5999999, 1}, + { -12.536313, -2, 121, 14.796721, -2.5999999, 1}, + { -12.025314, 0.40000001, 96, 14.738676, 2.5999999, 1}, + }; + + for ( int i = 0; i < VELODYNE_NUM_LASERS; i++ ) + { + velodyne_calibrated[i][0] = velodyne_calibrated1[i][0]; + velodyne_calibrated[i][1] = velodyne_calibrated1[i][1]; + velodyne_calibrated[i][2] = velodyne_calibrated1[i][2]; + velodyne_calibrated[i][3] = velodyne_calibrated1[i][3]; + velodyne_calibrated[i][4] = velodyne_calibrated1[i][4]; + velodyne_calibrated[i][5] = velodyne_calibrated1[i][5]; + } + + return 0; +} + +double rotCorrection[VELODYNE_NUM_LASERS]; +double vertCorrection[VELODYNE_NUM_LASERS]; +double distCorrection[VELODYNE_NUM_LASERS]; +double vertoffsetCorrection[VELODYNE_NUM_LASERS]; +double horizdffsetCorrection[VELODYNE_NUM_LASERS]; +double enabled[VELODYNE_NUM_LASERS]; //New variable to change enabling and disabling of data. + +int physical2logical[VELODYNE_NUM_LASERS]; +int logical2physical[VELODYNE_NUM_LASERS]; + +double absf ( double a ) +{ + if ( a < 0 ) + return -a; + return a; +} + +int velodyne_physical_to_logical ( int phys ) +{ + return physical2logical[phys]; +} + +int velodyne_logical_to_physical ( int logical ) +{ + return logical2physical[logical]; +} + +int laser_phi_compare(const void *_a, const void *_b) +{ + int a = *((int*) _a); + int b = *((int*) _b); + + if (velodyne_calibrated[a][0] < velodyne_calibrated[b][0]) + return -1; + + return 1; +} + +/** valid only for v > 0 **/ +static inline double mod2pi_positive(double vin) +{ + double q = vin * TWOPI_INV + 0.5; + int qi = (int) q; + + return vin - qi*TWOPI; +} + +/** Map v to [-PI, PI] **/ +static inline double mod2pi(double vin) +{ + if (vin < 0) + return -mod2pi_positive(-vin); + else + return mod2pi_positive(vin); +} + +/** Return vin such that it is within PI degrees of ref **/ +static inline double mod2pi_ref(double ref, double vin) +{ + return ref + mod2pi(vin - ref); +} + +int velodyne_calib_precompute() +{ + int i; + int logical; + for ( i = 0; i < VELODYNE_NUM_LASERS; i++ ) + logical2physical[i] = i; + + qsort ( logical2physical, VELODYNE_NUM_LASERS, sizeof ( int ), laser_phi_compare ); + + for ( logical = 0; logical < VELODYNE_NUM_LASERS; logical++ ) + { + physical2logical[logical2physical[logical]] = logical; + } + + //vertCorrection rotCorrection distCorrection vertOffsetCorrection horizOffsetCorrection enable + for ( i = 0; i < VELODYNE_NUM_LASERS; i++ ) + { + vertCorrection[i] = velodyne_calibrated[i][0] * RADIANS_PER_LSB; + rotCorrection[i] = velodyne_calibrated[i][1] * RADIANS_PER_LSB; + distCorrection[i] = velodyne_calibrated[i][2] * METERS_PER_CM; + vertoffsetCorrection[i] = velodyne_calibrated[i][3] * METERS_PER_CM; + horizdffsetCorrection[i] = velodyne_calibrated[i][4] * METERS_PER_CM; + enabled[i] = velodyne_calibrated[i][5]; + } + + return 0; +} + + +int calibrate(string filename) +{ + int linecount = 0; + string line; + std::ifstream myfile (filename.c_str()); + if (myfile.is_open()) + { + cout << "Using Calibration File"<(string::npos - theDelimiter.size()))?string::npos:end + theDelimiter.size()); + i++; + } + + velodyne_calibrated[j][0] = atof(data[0].c_str()); + velodyne_calibrated[j][1] = atof(data[1].c_str()); + velodyne_calibrated[j][2] = atof(data[2].c_str()); + velodyne_calibrated[j][3] = atof(data[3].c_str()); + velodyne_calibrated[j][4] = atof(data[4].c_str()); + velodyne_calibrated[j][5] = atof(data[5].c_str()); + linecount++; + j++; + } + while(!myfile.eof()); + + myfile.close(); + + if (linecount < 60) + { + for (int i = 32; i<64; i++) + { + velodyne_calibrated[i][0] = 0.0; + velodyne_calibrated[i][1] = 0.0; + velodyne_calibrated[i][2] = 0.0; + velodyne_calibrated[i][3] = 0.0; + velodyne_calibrated[i][4] = 0.0; + velodyne_calibrated[i][5] = 0.0; + } + } + } + else + { + cout << "Unable to open calibration file.\nUsing Wuhan hardcored values."<* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* amplitude, + std::vector* type, + std::vector* deviation) +{ + + int c, i, j; + unsigned char Head; + BYTE buf[BLOCK_SIZE]; + Point point; + BYTE *p; + unsigned short *ps; + unsigned short *pt; + unsigned short *pshort; + + double ctheta; + double theta, phi; + + double sin_ctheta, cos_ctheta; + double sin_theta, cos_theta; + double sin_phi, cos_phi; + + unsigned short physicalNO; + unsigned short logicalNO; + + float rotational; + float distance; + float corredistance; + int intensity; + int physical; + int size; + + unsigned short rot; + double x, y, z; + + int circle_col = 0; + int circle_row = 0; + + for ( c = 0 ; c < CIRCLELENGTH; c++ ) + { +#ifdef _MSC_VER + fseek(fp , BLOCK_OFFSET, SEEK_CUR); +#else + fseeko(fp , BLOCK_OFFSET, SEEK_CUR); +#endif + size=fread ( buf, 1, BLOCK_SIZE, fp ); + + if(size 2.2 ) + { + BYTE *inty = ( BYTE * ) ( p + 4 + j * 3 + 2 ); + intensity = *inty; + + ctheta = 2 * M_PI - rotational * RADIANS_PER_LSB; + if ( ctheta == 2*M_PI ) + ctheta = 0; + + sin_ctheta = sin ( ctheta ); + cos_ctheta = cos ( ctheta ); + + //vertCorrection rotCorrection distCorrection vertOffsetCorrection horizOffsetCorrection + // corredistance = ( distance + distCorrection[physicalNO] ) * ( 1.0 + vertoffsetCorrection[physicalNO] ); + corredistance = ( distance + distCorrection[physicalNO] ) ; + theta = mod2pi_ref ( M_PI, ctheta + rotCorrection[physicalNO] ); /////////????//////// + phi = vertCorrection[physicalNO]; ////////?????///////////////// + + sin_theta = sin ( theta ); + cos_theta = cos ( theta ); + sin_phi = sin ( phi ); + cos_phi = cos ( phi ); + + /////////////////////?a??¡Á?¡À¨º///////////////////// + x = corredistance * cos_theta * cos_phi; + y = corredistance * sin_theta * cos_phi; + z = corredistance * sin_phi +vertoffsetCorrection[physicalNO]*cos_phi; + + x -= horizdffsetCorrection[physicalNO] * cos_ctheta; + y -= horizdffsetCorrection[physicalNO] * sin_ctheta; + + point.rad = sqrt( x*x + y*y ); + point.tan_theta = y/x; + + point.type = POINT_TYPE_GROUND; + point.x=x*100; + point.y=z*100; + point.z=-y*100; + + double p[3]; + p[0] = point.x; + p[1] = point.y; + p[2] = point.z; + + //Enable Check. Could be moved earlier to remove all the unnecessary calculations. + //Here to simplify the implementation + if(filter.check(p) && (enabled[physicalNO]==1)) + { + for(int ii = 0; ii < 3; ++ii) xyz->push_back(p[ii]); + } + } + } + p = p + 100; + ps = ( unsigned short * ) p; + } + + + } + + return 0; +} + +std::list ScanIO_velodyne::readDirectory(const char* dir_path, unsigned int start, unsigned int end) +{ + //Calling the calibration method with the file name "calibration.txt" in the same folder as the scan.bin + string califilename; + string dir(dir_path); + califilename = dir + "calibration" + ".txt"; + calibrate(califilename); + std::list identifiers; +// fileCounter = start; + + velodyne_calib_precompute(); + for(unsigned int i = start; i <= end; ++i) + { + std::string identifier(to_string(i,3)); + + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + DATA_PATH_SUFFIX); + + path pose(dir_path); + pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); + + if(!exists(data)) + break; + identifiers.push_back(identifier); + } + + return identifiers; +} + +void ScanIO_velodyne::readPose(const char* dir_path, const char* identifier, double* pose) +{ + unsigned int i; + // on pose information for veloslam + for(i = 0; i < 6; ++i) pose[i] = 0.0; + for(i = 3; i < 6; ++i) pose[i] = 0.0; + return; +} + +bool ScanIO_velodyne::supports(IODataType type) +{ + return !!(type & (DATA_XYZ)); +} + + +void ScanIO_velodyne::readScan( + const char* dir_path, + const char* identifier, + PointFilter& filter, + std::vector* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* deviation) +{ + unsigned int i; + FILE *scan_in; + + path data_path(dir_path); + data_path /= path(std::string(DATA_PATH_PREFIX) + DATA_PATH_SUFFIX); + if(!exists(data_path)) + throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]"); + + char filename[256]; + sprintf(filename, "%s%s%s",dir_path ,DATA_PATH_PREFIX, DATA_PATH_SUFFIX ); + +#ifdef _MSC_VER + scan_in = fopen(filename,"rb"); +#else + scan_in = fopen64(filename,"rb"); +#endif + + if(scan_in == NULL) + { + cerr<reserve(12*32*CIRCLELENGTH); + + fileCounter= atoi(identifier); + + #ifdef _MSC_VER + fseek(scan_in, 24, SEEK_SET); + fseek(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR); +#else + fseeko(scan_in, 24, SEEK_SET); + fseeko(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR); +#endif + + read_one_packet( + scan_in, + filter, + xyz, + rgb, + reflectance, + amplitude, + type, + deviation); + + cout << " with " << xyz->size() << " Points"; + cout << " done " << fileCounter< Scan::allScans; +bool Scan::scanserver = false; + + +void Scan::openDirectory(bool scanserver, const std::string& path, IOType type, + int start, int end) +{ + Scan::scanserver = scanserver; + if(scanserver) + ManagedScan::openDirectory(path, type, start, end); + else + BasicScan::openDirectory(path, type, start, end); +} + +void Scan::closeDirectory() +{ + if(scanserver) + ManagedScan::closeDirectory(); + else + BasicScan::closeDirectory(); +} + +Scan::Scan() +{ + unsigned int i; + + // pose and transformations + for(i = 0; i < 3; ++i) rPos[i] = 0; + for(i = 0; i < 3; ++i) rPosTheta[i] = 0; + for(i = 0; i < 4; ++i) rQuat[i] = 0; + M4identity(transMat); + M4identity(transMatOrg); + M4identity(dalignxf); + + // trees and reduction methods + cuda_enabled = false; + nns_method = -1; + kd = 0; + ann_kd_tree = 0; + + // reduction on-demand + reduction_voxelSize = 0.0; + reduction_nrpts = 0; + reduction_pointtype = PointType(); + + // flags + m_has_reduced = false; + + // octtree + octtree_reduction_voxelSize = 0.0; + octtree_voxelSize = 0.0; + octtree_pointtype = PointType(); + octtree_loadOct = false; + octtree_saveOct = false; +} + +Scan::~Scan() +{ + if(kd) delete kd; +} + +void Scan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype) +{ + reduction_voxelSize = voxelSize; + reduction_nrpts = nrpts; + reduction_pointtype = pointtype; +} + +void Scan::setSearchTreeParameter(int nns_method, bool cuda_enabled) +{ + searchtree_nnstype = nns_method; + searchtree_cuda_enabled = cuda_enabled; +} + +void Scan::setOcttreeParameter(double reduction_voxelSize, double voxelSize, PointType pointtype, bool loadOct, bool saveOct) +{ + octtree_reduction_voxelSize = reduction_voxelSize; + octtree_voxelSize = voxelSize; + octtree_pointtype = pointtype; + octtree_loadOct = loadOct; + octtree_saveOct = saveOct; +} + +void Scan::clear(unsigned int types) +{ + if(types & DATA_XYZ) clear("xyz"); + if(types & DATA_RGB) clear("rgb"); + if(types & DATA_REFLECTANCE) clear("reflectance"); + if(types & DATA_TEMPERATURE) clear("temperature"); + if(types & DATA_AMPLITUDE) clear("amplitude"); + if(types & DATA_TYPE) clear("type"); + if(types & DATA_DEVIATION) clear("deviation"); +} + +SearchTree* Scan::getSearchTree() +{ + // if the search tree hasn't been created yet, calculate everything + if(kd == 0) { + createSearchTree(); + } + return kd; +} + +void Scan::toGlobal() { + calcReducedPoints(); + transform(transMatOrg, INVALID); +} + +/** + * Computes a search tree depending on the type. + */ +void Scan::createSearchTree() +{ + // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation +// boost::lock_guard lock(m_mutex_create_tree); + if(kd != 0) return; + + // make sure the original points are created before starting the measurement + DataXYZ xyz_orig(get("xyz reduced original")); + +#ifdef WITH_METRICS + Timer tc = ClientMetric::create_tree_time.start(); +#endif //WITH_METRICS + + createSearchTreePrivate(); + +#ifdef WITH_METRICS + ClientMetric::create_tree_time.end(tc); +#endif //WITH_METRICS +} + +void Scan::calcReducedOnDemand() +{ + // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees count as zero) do the reduction +// boost::lock_guard lock(m_mutex_reduction); + if(m_has_reduced) return; + +#ifdef WITH_METRICS + Timer t = ClientMetric::on_demand_reduction_time.start(); +#endif //WITH_METRICS + + calcReducedOnDemandPrivate(); + + m_has_reduced = true; + +#ifdef WITH_METRICS + ClientMetric::on_demand_reduction_time.end(t); +#endif //WITH_METRICS +} + +void Scan::copyReducedToOriginal() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::copy_original_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_r(get("xyz reduced")); + unsigned int size = xyz_r.size(); + DataXYZ xyz_r_orig(create("xyz reduced original", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_r_orig[i][j] = xyz_r[i][j]; + } + } + +#ifdef WITH_METRICS + ClientMetric::copy_original_time.end(t); +#endif //WITH_METRICS +} + +void Scan::copyOriginalToReduced() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::copy_original_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_r_orig(get("xyz reduced original")); + unsigned int size = xyz_r_orig.size(); + DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_r[i][j] = xyz_r_orig[i][j]; + } + } + +#ifdef WITH_METRICS + ClientMetric::copy_original_time.end(t); +#endif //WITH_METRICS +} + + + +/** + * Computes an octtree of the current scan, then getting the + * reduced points as the centers of the octree voxels. + */ +void Scan::calcReducedPoints() +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::scan_load_time.start(); +#endif //WITH_METRICS + + // get xyz to start the scan load, separated here for time measurement + DataXYZ xyz(get("xyz")); + + // if the scan hasn't been loaded we can't calculate anything + if(xyz.size() == 0) + throw runtime_error("Could not calculate reduced points, XYZ data is empty"); + +#ifdef WITH_METRICS + ClientMetric::scan_load_time.end(t); + Timer tl = ClientMetric::calc_reduced_points_time.start(); +#endif //WITH_METRICS + + // no reduction needed + // copy vector of points to array of points to avoid + // further copying + if(reduction_voxelSize <= 0.0) { + // copy the points + DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*xyz.size())); + for(unsigned int i = 0; i < xyz.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_r[i][j] = xyz[i][j]; + } + } + } else { + // start reduction + + // build octree-tree from CurrentScan + // put full data into the octtree + BOctTree *oct = new BOctTree(PointerArray(xyz).get(), + xyz.size(), reduction_voxelSize, reduction_pointtype); + + vector center; + center.clear(); + + if (reduction_nrpts > 0) { + if (reduction_nrpts == 1) { + oct->GetOctTreeRandom(center); + } else { + oct->GetOctTreeRandom(center, reduction_nrpts); + } + } else { + oct->GetOctTreeCenter(center); + } + + // storing it as reduced scan + unsigned int size = center.size(); + DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_r[i][j] = center[i][j]; + } + } + + delete oct; + } + +#ifdef WITH_METRICS + ClientMetric::calc_reduced_points_time.end(tl); +#endif //WITH_METRICS +} + +/** + * Merges the scan's intrinsic coordinates with the robot position. + * @param prevScan The scan that's transformation is extrapolated, + * i.e., odometry extrapolation + * + * For additional information see the following paper (jfr2007.pdf): + * + * Andreas Nüchter, Kai Lingemann, Joachim Hertzberg, and Hartmut Surmann, + * 6D SLAM - 3D Mapping Outdoor Environments Journal of Field Robotics (JFR), + * Special Issue on Quantitative Performance Evaluation of Robotic and Intelligent + * Systems, Wiley & Son, ISSN 1556-4959, Volume 24, Issue 8-9, pages 699 - 722, + * August/September, 2007 + * + */ +void Scan::mergeCoordinatesWithRoboterPosition(Scan* prevScan) +{ + double tempMat[16], deltaMat[16]; + M4inv(prevScan->get_transMatOrg(), tempMat); + MMult(prevScan->get_transMat(), tempMat, deltaMat); + transform(deltaMat, INVALID); //apply delta transformation of the previous scan +} + +/** + * The method transforms all points with the given transformation matrix. + */ +void Scan::transformAll(const double alignxf[16]) +{ + DataXYZ xyz(get("xyz")); + unsigned int i=0 ; +// #pragma omp parallel for + for(; i < xyz.size(); ++i) { + transform3(alignxf, xyz[i]); + } + // TODO: test for ManagedScan compability, may need a touch("xyz") to mark saving the new values +} + +//! Internal function of transform which alters the reduced points +void Scan::transformReduced(const double alignxf[16]) +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::transform_time.start(); +#endif //WITH_METRICS + + DataXYZ xyz_r(get("xyz reduced")); + unsigned int i=0; + // #pragma omp parallel for + for( ; i < xyz_r.size(); ++i) { + transform3(alignxf, xyz_r[i]); + } + +#ifdef WITH_METRICS + ClientMetric::transform_time.end(t); +#endif //WITH_METRICS +} + +//! Internal function of transform which handles the matrices +void Scan::transformMatrix(const double alignxf[16]) +{ + double tempxf[16]; + + // apply alignxf to transMat and update pose vectors + MMult(alignxf, transMat, tempxf); + memcpy(transMat, tempxf, sizeof(transMat)); + Matrix4ToEuler(transMat, rPosTheta, rPos); + Matrix4ToQuat(transMat, rQuat); + +#ifdef DEBUG + cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl; + + cerr << transMat << endl; +#endif + + // apply alignxf to dalignxf + MMult(alignxf, dalignxf, tempxf); + memcpy(dalignxf, tempxf, sizeof(transMat)); +} + +/** + * Transforms the scan by a given transformation and writes a new frame. The idea + * is to write for every transformation in all files, such that the show program + * is able to determine, whcih scans have to be drawn in which color. Hidden scans + * (or later processed scans) are written with INVALID. + * + * @param alignxf Transformation matrix + * @param colour Specifies which colour should the written to the frames file + * @param islum Is the transformtion part of LUM, i.e., all scans are transformed? + * In this case only LUM transformation is stored, otherwise all scans are processed + * -1 no transformation is stored + * 0 ICP transformation + * 1 LUM transformation, all scans except last scan + * 2 LUM transformation, last scan only + */ +void Scan::transform(const double alignxf[16], const AlgoType type, int islum) +{ + MetaScan* meta = dynamic_cast(this); + + if(meta) { + for(unsigned int i = 0; i < meta->size(); ++i) { + meta->getScan(i)->transform(alignxf, type, -1); + } + } + +#ifdef TRANSFORM_ALL_POINTS + transformAll(alignxf); +#endif //TRANSFORM_ALL_POINTS + +#ifdef DEBUG + cerr << alignxf << endl; + cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " + << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> "; +#endif + + // transform points + transformReduced(alignxf); + + // update matrices + transformMatrix(alignxf); + + // store transformation in frames + if(type != INVALID) { +#ifdef WITH_METRICS + Timer t = ClientMetric::add_frames_time.start(); +#endif //WITH_METRICS + bool in_meta; + MetaScan* meta = dynamic_cast(this); + int found = 0; + unsigned int scans_size = allScans.size(); + + switch (islum) { + case -1: + // write no tranformation + break; + case 0: + for(unsigned int i = 0; i < scans_size; ++i) { + Scan* scan = allScans[i]; + in_meta = false; + if(meta) { + for(unsigned int j = 0; j < meta->size(); ++j) { + if(meta->getScan(j) == scan) { + found = i; + in_meta = true; + } + } + } + + if(scan == this || in_meta) { + found = i; + scan->addFrame(type); + } else { + if(found == 0) { + scan->addFrame(ICPINACTIVE); + } else { + scan->addFrame(INVALID); + } + } + } + break; + case 1: + addFrame(type); + break; + case 2: + for(unsigned int i = 0; i < scans_size; ++i) { + Scan* scan = allScans[i]; + if(scan == this) { + found = i; + addFrame(type); + allScans[0]->addFrame(type); + continue; + } + if (found != 0) { + scan->addFrame(INVALID); + } + } + break; + default: + cerr << "invalid point transformation mode" << endl; + } + +#ifdef WITH_METRICS + ClientMetric::add_frames_time.end(t); +#endif //WITH_METRICS + } +} + +/** + * Transforms the scan by a given transformation and writes a new frame. The idea + * is to write for every transformation in all files, such that the show program + * is able to determine, whcih scans have to be drawn in which color. Hidden scans + * (or later processed scans) are written with INVALID. + * + * @param alignQuat Quaternion for the rotation + * @param alignt Translation vector + * @param colour Specifies which colour should the written to the frames file + * @param islum Is the transformtion part of LUM, i.e., all scans are transformed? + * In this case only LUM transformation is stored, otherwise all scans are processed + * -1 no transformation is stored + * 0 ICP transformation + * 1 LUM transformation, all scans except last scan + * 2 LUM transformation, last scan only + */ +void Scan::transform(const double alignQuat[4], const double alignt[3], + const AlgoType type, int islum) +{ + double alignxf[16]; + QuatToMatrix4(alignQuat, alignt, alignxf); + transform(alignxf, type, islum); +} + +/** + * Transforms the scan, so that the given Matrix + * prepresent the next pose. + * + * @param alignxf Transformation matrix to which this scan will be set to + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToMatrix(double alignxf[16], const AlgoType type, int islum) +{ + double tinv[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + transform(alignxf, type, islum); +} + +/** + * Transforms the scan, so that the given Euler angles + * prepresent the next pose. + * + * @param rP Translation to which this scan will be set to + * @param rPT Orientation as Euler angle to which this scan will be set + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToEuler(double rP[3], double rPT[3], const AlgoType type, int islum) +{ +#ifdef WITH_METRICS + // called in openmp context in lum6Deuler.cc:422 + ClientMetric::transform_time.set_threadsafety(true); + ClientMetric::add_frames_time.set_threadsafety(true); +#endif //WITH_METRICS + + double tinv[16]; + double alignxf[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + EulerToMatrix4(rP, rPT, alignxf); + transform(alignxf, type, islum); + +#ifdef WITH_METRICS + ClientMetric::transform_time.set_threadsafety(false); + ClientMetric::add_frames_time.set_threadsafety(false); +#endif //WITH_METRICS +} + +/** + * Transforms the scan, so that the given Euler angles + * prepresent the next pose. + * + * @param rP Translation to which this scan will be set to + * @param rPQ Orientation as Quaternion to which this scan will be set + * @param islum Is the transformation part of LUM? + */ +void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int islum) +{ + double tinv[16]; + double alignxf[16]; + M4inv(transMat, tinv); + transform(tinv, INVALID); + QuatToMatrix4(rPQ, rP, alignxf); + transform(alignxf, type, islum); +} + +/** + * Calculates Source\Target + * Calculates a set of corresponding point pairs and returns them. It + * computes the k-d trees and deletes them after the pairs have been + * found. This slow function should be used only for testing + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + */ + +void Scan::getNoPairsSimple(vector &diff, + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2) +{ + DataXYZ xyz_r(Source->get("xyz reduced")); + KDtree* kd = new KDtree(PointerArray(Target->get("xyz reduced")).get(), Target->size("xyz reduced")); + + cout << "Max: " << max_dist_match2 << endl; + for (unsigned int i = 0; i < xyz_r.size(); i++) { + + double p[3]; + p[0] = xyz_r[i][0]; + p[1] = xyz_r[i][1]; + p[2] = xyz_r[i][2]; + + + double *closest = kd->FindClosest(p, max_dist_match2, thread_num); + if (!closest) { + diff.push_back(xyz_r[i]); + //diff.push_back(closest); + } + } + + delete kd; +} + +/** + * Calculates a set of corresponding point pairs and returns them. It + * computes the k-d trees and deletes them after the pairs have been + * found. This slow function should be used only for testing + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + */ +void Scan::getPtPairsSimple(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, + double *centroid_m, double *centroid_d) +{ + KDtree* kd = new KDtree(PointerArray(Source->get("xyz reduced")).get(), Source->size("xyz reduced")); + DataXYZ xyz_r(Target->get("xyz reduced")); + + for (unsigned int i = 0; i < xyz_r.size(); i++) { + if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only + + double p[3]; + p[0] = xyz_r[i][0]; + p[1] = xyz_r[i][1]; + p[2] = xyz_r[i][2]; + + double *closest = kd->FindClosest(p, max_dist_match2, thread_num); + if (closest) { + centroid_m[0] += closest[0]; + centroid_m[1] += closest[1]; + centroid_m[2] += closest[2]; + centroid_d[0] += p[0]; + centroid_d[1] += p[1]; + centroid_d[2] += p[2]; + PtPair myPair(closest, p); + pairs->push_back(myPair); + } + } + centroid_m[0] /= pairs[thread_num].size(); + centroid_m[1] /= pairs[thread_num].size(); + centroid_m[2] /= pairs[thread_num].size(); + centroid_d[0] /= pairs[thread_num].size(); + centroid_d[1] /= pairs[thread_num].size(); + centroid_d[2] /= pairs[thread_num].size(); + + delete kd; +} + + +/** + * Calculates a set of corresponding point pairs and returns them. + * The function uses the k-d trees stored the the scan class, thus + * the function createTrees and deletTrees have to be called before + * resp. afterwards. + * Here we implement the so called "fast corresponding points"; k-d + * trees are not recomputed, instead the apply the inverse transformation + * to to the given point set. + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num number of the thread (for parallelization) + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + * @return a set of corresponding point pairs + */ +void Scan::getPtPairs(vector *pairs, + Scan* Source, Scan* Target, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d) +{ + // initialize centroids + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[i] = 0; + centroid_d[i] = 0; + } + + // get point pairs + DataXYZ xyz_r(Target->get("xyz reduced")); + Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf, + xyz_r, 0, xyz_r.size(), + thread_num, + rnd, max_dist_match2, sum, centroid_m, centroid_d); + + // normalize centroids + unsigned int size = pairs->size(); + if(size != 0) { + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[i] /= size; + centroid_d[i] /= size; + } + } +} + + +/** + * Calculates a set of corresponding point pairs and returns them. + * The function uses the k-d trees stored the the scan class, thus + * the function createTrees and delteTrees have to be called before + * resp. afterwards. + * + * @param pairs The resulting point pairs (vector will be filled) + * @param Source The scan whose points are matched to Targets' points + * @param Target The scan to whiche the points are matched + * @param thread_num The number of the thread that is computing ptPairs in parallel + * @param step The number of steps for parallelization + * @param rnd randomized point selection + * @param max_dist_match2 maximal allowed distance for matching + * @param sum The sum of distances of the points + * + * These intermediate values are for the parallel ICP algorithm + * introduced in the paper + * "The Parallel Iterative Closest Point Algorithm" + * by Langis / Greenspan / Godin, IEEE 3DIM 2001 + * + */ +void Scan::getPtPairsParallel(vector *pairs, Scan* Source, Scan* Target, + int thread_num, int step, + int rnd, double max_dist_match2, + double *sum, + double centroid_m[OPENMP_NUM_THREADS][3], double centroid_d[OPENMP_NUM_THREADS][3]) +{ + // initialize centroids + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[thread_num][i] = 0; + centroid_d[thread_num][i] = 0; + } + + // get point pairs + SearchTree* search = Source->getSearchTree(); + // differentiate between a meta scan (which has no reduced points) and a normal scan + // if Source is also a meta scan it already has a special meta-kd-tree + MetaScan* meta = dynamic_cast(Target); + if(meta) { + for(unsigned int i = 0; i < meta->size(); ++i) { + // determine step for each scan individually + DataXYZ xyz_r(meta->getScan(i)->get("xyz reduced")); + unsigned int max = xyz_r.size(); + unsigned int step = max / OPENMP_NUM_THREADS; + // call ptpairs for each scan and accumulate ptpairs, centroids and sum + search->getPtPairs(&pairs[thread_num], Source->dalignxf, + xyz_r, step * thread_num, step * thread_num + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num]); + } + } else { + DataXYZ xyz_r(Target->get("xyz reduced")); + search->getPtPairs(&pairs[thread_num], Source->dalignxf, + xyz_r, thread_num * step, thread_num * step + step, + thread_num, + rnd, max_dist_match2, sum[thread_num], + centroid_m[thread_num], centroid_d[thread_num]); + } + + // normalize centroids + unsigned int size = pairs[thread_num].size(); + if(size != 0) { + for(unsigned int i = 0; i < 3; ++i) { + centroid_m[thread_num][i] /= size; + centroid_d[thread_num][i] /= size; + } + } +} + +unsigned int Scan::getMaxCountReduced(ScanVector& scans) +{ + unsigned int max = 0; + for(std::vector::iterator it = scans.begin(); it != scans.end(); ++it) { + unsigned int count = (*it)->size("xyz reduced"); + if(count > max) + max = count; + } + return max; +} diff --git a/.svn/pristine/6b/6b9e1fb3cb0b5d60d3717aca31176336255f00eb.svn-base b/.svn/pristine/6b/6b9e1fb3cb0b5d60d3717aca31176336255f00eb.svn-base new file mode 100644 index 0000000..7469d0c --- /dev/null +++ b/.svn/pristine/6b/6b9e1fb3cb0b5d60d3717aca31176336255f00eb.svn-base @@ -0,0 +1,117 @@ +### TOOLS + +IF(WITH_TOOLS) + FIND_PACKAGE(OpenCV REQUIRED) + ### SCAN_RED + add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc) + + IF(UNIX) + target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS}) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(scan_red scan ANN XGetopt) + ENDIF(WIN32) + + ### SCAN_DIFF + add_executable(scan_diff scan_diff.cc) + # add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc) + + IF(UNIX) + target_link_libraries(scan_diff scan dl ANN) + # target_link_libraries(scan_diff2d scan dl ANN) + ENDIF(UNIX) + + IF (WIN32) + target_link_libraries(scan_diff scan ANN XGetopt) + # target_link_libraries(scan_diff2d scan ANN XGetopt) + ENDIF(WIN32) + + add_executable(frame_to_graph frame_to_graph.cc) + add_executable(convergence convergence.cc) + add_executable(graph_balancer graph_balancer.cc) + add_executable(exportPoints exportPoints.cc) + add_executable(frames2riegl frames2riegl.cc) + add_executable(frames2pose frames2pose.cc) + add_executable(pose2frames pose2frames.cc) + add_executable(riegl2frames riegl2frames.cc) + add_executable(toGlobal toGlobal.cc) + + IF(UNIX) + target_link_libraries(graph_balancer scan ${Boost_GRAPH_LIBRARY} ${Boost_SERIALIZATION_LIBRARY} ${Boost_REGEX_LIBRARY}) + target_link_libraries(exportPoints scan dl ANN) + target_link_libraries(toGlobal scan) + ENDIF(UNIX) + + + IF (WIN32) + target_link_libraries(frame_to_graph XGetopt ${Boost_LIBRARIES}) + target_link_libraries(convergence XGetopt ${Boost_LIBRARIES}) + target_link_libraries(graph_balancer scan XGetopt ${Boost_LIBRARIES}) + target_link_libraries(exportPoints scan ANN XGetopt ${Boost_LIBRARIES}) + target_link_libraries(frames2pose XGetopt ${Boost_LIBRARIES}) + target_link_libraries(pose2frames XGetopt ${Boost_LIBRARIES}) + target_link_libraries(frames2riegl XGetopt ${Boost_LIBRARIES}) + target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES}) + target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES}) + ENDIF(WIN32) +ENDIF(WITH_TOOLS) + +### SCANLIB + +SET(SCANLIB_SRCS + kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc + graph.cc icp6Dapx.cc icp6D.cc icp6Dsvd.cc + icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc + icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc + ghelix6DQ2.cc gapx6D.cc graphToro.cc ann_kd.cc + graphHOG-Man.cc elch6D.cc elch6Dquat.cc elch6DunitQuat.cc + elch6Dslerp.cc elch6Deuler.cc loopToro.cc loopHOG-Man.cc + point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc + allocator.cc + scan.cc basicScan.cc managedScan.cc metaScan.cc + io_types.cc io_utils.cc pointfilter.cc + ) + +if(WITH_METRICS) + set(SCANLIB_SRCS ${SCANLIB_SRCS} metrics.cc) +endif(WITH_METRICS) + +add_library(scan STATIC ${SCANLIB_SRCS}) + +target_link_libraries(scan scanclient scanio) + +IF(UNIX) + target_link_libraries(scan dl) +ENDIF(UNIX) + +### EXPORT SHARED LIBS + +IF(EXPORT_SHARED_LIBS) +add_library(scan_s SHARED ${SCANLIB_SRCS}) +#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat) +ENDIF(EXPORT_SHARED_LIBS) + +### SLAM6D + +IF(WITH_CUDA) + CUDA_COMPILE(CUDA_FILES cuda/CIcpGpuCuda.cu ) + add_executable(slam6D slam6D.cc cuda/icp6Dcuda.cc ${CUDA_FILES}) + target_link_libraries(slam6D ${CUDA_LIBRARIES} ANN cudpp64) + CUDA_ADD_CUBLAS_TO_TARGET(slam6D) + CUDA_ADD_CUTIL_TO_TARGET(slam6D) +ELSE(WITH_CUDA) + add_executable(slam6D slam6D.cc) +ENDIF(WITH_CUDA) + +IF(UNIX) + target_link_libraries(slam6D scan newmat sparse ANN) +ENDIF(UNIX) + +IF(WIN32) + target_link_libraries(slam6D scan newmat sparse ANN XGetopt ${Boost_LIBRARIES}) +ENDIF(WIN32) + +#IF(MSVC) +# INSTALL(TARGETS slam6D RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/windows) +#ENDIF(MSVC) diff --git a/.svn/pristine/6d/6d4722e02f91538af3c17843aa935f7ecd44c693.svn-base b/.svn/pristine/6d/6d4722e02f91538af3c17843aa935f7ecd44c693.svn-base new file mode 100644 index 0000000..3625728 --- /dev/null +++ b/.svn/pristine/6d/6d4722e02f91538af3c17843aa935f7ecd44c693.svn-base @@ -0,0 +1,226 @@ +/** + * @file + * @brief Basic DataPointer class and its derivates SingleArray and TripleArray + * + * This file contains several classes for array-like access. The SingleArray + * and TripleArray classes and their typedefs to DataXYZ/... overload + * the operator[] and have a size function to act as their native arrays. + * Similar to the array classes, SingleObject represents a whole object with + * all its members in that allocated space. + * + * If an array of pointers to the elements of a TripleArray is required it can + * create a temporary class PointerArray which holds creates and deletes a + * native pointer array and follows the RAII-pattern. + */ + +#ifndef DATA_TYPES_H +#define DATA_TYPES_H + + +/** + * Representation of a pointer to a data field with no access methods. + * + * Type-specialized access is gained by deriving from this class and + * implementing access functions like operator[] and size(). + * + * The PrivateImplementation feature enables RAII-type locking mechanisms + * used in the scanserver for holding CacheObject-locks. It is protected so + * that scanserver-unaware code can only construct this class with a pointer + * and size. Initialization of a derived class with these locking mechanisms + * creates this class with the private implementation value, which will be + * deleted in this dtor when it completely falls out of scope. + */ +class DataPointer { +protected: + //! Subclass for storing further members and attaching an overloadable dtor + class PrivateImplementation { + public: + virtual ~PrivateImplementation() {} + }; +public: + // DataPointer& operator=(const DataPointer&) = delete; + // DataPointer(const DataPointer&) = delete; + + /** + * Ctor for the initial creation + * + * @param pointer base pointer to the data + * @param size of the pointed data in bytes + */ + DataPointer(unsigned char* pointer, unsigned int size, + PrivateImplementation* private_impl = 0) : + m_pointer(pointer), m_size(size), m_private_impl(private_impl) { + } + + /** + * Copy-Ctor for passing along via return by value + * + * The type-specialized classes (B) will be called with their + * B(DataPointer&&) temporary ctor and call this constructor, so the private + * imlementation has to be taken away. The temporary inside that constructor + * isn't seen as temporary anymore, so we need a simple reference-ctor. + */ + DataPointer(DataPointer& other) { + m_pointer = other.m_pointer; + m_size = other.m_size; + // take ownership of this value, other is a temporary and will deconstruct + m_private_impl = other.m_private_impl; + other.m_private_impl = 0; + }; + + /** + * Same as DataPointer(DataPointer&), except this is for functions returning + * DataPointer instead of derived classes, so the temporary-ctor is used. + */ + DataPointer(DataPointer&& other) { + m_pointer = other.m_pointer; + m_size = other.m_size; + // take ownership of this value, other is a temporary and will deconstruct + m_private_impl = other.m_private_impl; + other.m_private_impl = 0; + } + + //! Delete the private implementation with its derived dtor + ~DataPointer() { + if(m_private_impl != 0) + delete m_private_impl; + } + + //! Indicator for nullpointer / no data contained if false + inline bool valid() { + return m_size != 0; + } + + inline unsigned char* get_raw_pointer() const { return m_pointer; } + +protected: + unsigned char* m_pointer; + unsigned int m_size; + +private: + PrivateImplementation* m_private_impl; +}; + + + +template +class SingleArray : public DataPointer { +public: + //! Cast return-by-value temporary DataPointer to this type of array + SingleArray(DataPointer&& temp) : + DataPointer(temp) + { + } + + SingleArray(SingleArray&& temp) : + DataPointer(temp) + { + } + + //! Represent the pointer as an array of T + inline T& operator[](unsigned int i) const + { + return *(reinterpret_cast(m_pointer) + i); + } + + //! The number of T instances in this array + unsigned int size() { + return m_size / sizeof(T); + } +}; + + + +template +class TripleArray : public DataPointer { +public: + //! Cast return-by-value temporary DataPointer to this type of array + TripleArray(DataPointer&& temp) : + DataPointer(temp) + { + } + + TripleArray(TripleArray&& temp) : + DataPointer(temp) + { + } + + //! Represent the pointer as an array of T[3] + inline T* operator[](unsigned int i) const + { + return reinterpret_cast(m_pointer) + (i*3); + } + + //! The number of T[3] instances in this array + unsigned int size() const { + return m_size / (3 * sizeof(T)); + } +}; + + + +template +class SingleObject : public DataPointer { +public: + //! Cast return-by-value temporary DataPointer to this type of object + SingleObject(DataPointer&& temp) : + DataPointer(temp) + { + } + + SingleObject(SingleObject&& temp) : + DataPointer(temp) + { + } + + //! Type-cast + inline T& get() const + { + return *reinterpret_cast(m_pointer); + } + + //! There is only one object in here + unsigned int size() const { + return 1; + } +}; + + + +/** + * To simplify T** access patterns for an array of T[3] (points), this RAII- + * type class helps creating and managing this pointer array on the stack. + */ +template +class PointerArray { +public: + //! Create a temporary array and fill it sequentially with pointers to points + PointerArray(const TripleArray& data) { + unsigned int size = data.size(); + m_array = new T*[size]; + for(unsigned int i = 0; i < size; ++i) + m_array[i] = data[i]; + } + + //! Removes the temporary array on destruction (RAII) + ~PointerArray() { + delete[] m_array; + } + + //! Conversion operator to interface the TripleArray to a T** array + inline T** get() const { return m_array; } +private: + T** m_array; +}; + +// TODO: naming, see scan.h +typedef TripleArray DataXYZ; +typedef TripleArray DataXYZFloat; +typedef TripleArray DataRGB; +typedef SingleArray DataReflectance; +typedef SingleArray DataTemperature; +typedef SingleArray DataAmplitude; +typedef SingleArray DataType; +typedef SingleArray DataDeviation; + +#endif //DATA_TYPES_H diff --git a/.svn/pristine/6d/6d7767e09cde7fcee4cc7a84392da17afd831084.svn-base b/.svn/pristine/6d/6d7767e09cde7fcee4cc7a84392da17afd831084.svn-base new file mode 100644 index 0000000..b365a4e --- /dev/null +++ b/.svn/pristine/6d/6d7767e09cde7fcee4cc7a84392da17afd831084.svn-base @@ -0,0 +1,97 @@ +/** + * @file + * @brief + * + * @author Thomas Escher + */ + +#ifndef MULTI_ARRAY_H +#define MULTI_ARRAY_H + +#include +using std::cout; +using std::cerr; +using std::endl; + +#include "scanserver/cache/cacheDataAccess.h" + + + +/** + * @brief Point-Array imitation for xyz[i][0..2] calls + */ +template +class TripleArray : public CacheDataAccess { +public: + //! Take ownership of a cache data + TripleArray(CacheDataAccess&& cd) : CacheDataAccess(cd) {} + + //! Move constructor of this type like CacheData, transfer lock + TripleArray(TripleArray&& other) : CacheDataAccess(other) {} + + //! Represent the CacheData as an array of T[3]'s + inline return_type* operator[](unsigned int i) const + { + return reinterpret_cast(getData()) + (i*3); + } + + //! Count of T[3] objects in this CacheData + inline unsigned int size() const { return CacheDataAccess::getSize() / (3*sizeof(return_type)); } +}; + +/** + * @brief Point-Array imitation for value[i] calls + */ +template +class SingleArray : public CacheDataAccess { +public: + //! Take ownership of a cache data + SingleArray(CacheDataAccess&& cd) : CacheDataAccess(cd) {} + + //! Move constructor of this type like CacheData, transfer lock + SingleArray(SingleArray&& other) : CacheDataAccess(other) {} + + //! Represent the CacheData as an array of T's + inline return_type& operator[](unsigned int i) const + { + return *(reinterpret_cast(getData()) + i); + } + + //! Count of T objects in this CacheData + inline unsigned int size() const { return CacheDataAccess::getSize() / sizeof(return_type); } +}; + +typedef TripleArray DataXYZ; +typedef TripleArray DataRGB; +typedef SingleArray DataReflectance; +typedef SingleArray DataTemperature; +typedef SingleArray DataAmplitude; +typedef SingleArray DataType; +typedef SingleArray DataDeviation; + +/** + * To simplify T** access patterns for an array of T[3] (points), this RAII type class helps creating and managing this pointer array on the stack. + */ +template +class Array { +public: + //! Create a temporary array and fill it sequentially with pointers to each point + Array(const TripleArray& data) { + unsigned int size = data.size(); + m_array = new T*[size]; + for(unsigned int i = 0; i < size; ++i) + m_array[i] = data[i]; + } + + //! Removes the temporary array on destruction (RAII) + ~Array() { + delete[] m_array; + } + + //! Conversion operator to interface the MultiArray to a T** array + inline T* const* get() const { return m_array; } +private: + T** m_array; +}; + +#endif //MULTI_ARRAY_H diff --git a/.svn/pristine/78/78fdca4abe08e977169de18e7ab1794a388fcf80.svn-base b/.svn/pristine/78/78fdca4abe08e977169de18e7ab1794a388fcf80.svn-base new file mode 100644 index 0000000..03e5674 --- /dev/null +++ b/.svn/pristine/78/78fdca4abe08e977169de18e7ab1794a388fcf80.svn-base @@ -0,0 +1,27 @@ +/** + * @file + * @brief IO of a 3D scan in uos file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_KS_H__ +#define __SCAN_IO_KS_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for KS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_ks : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/79/798da527fce49381e8e2e8700cfac5604f0577c0.svn-base b/.svn/pristine/79/798da527fce49381e8e2e8700cfac5604f0577c0.svn-base new file mode 100644 index 0000000..cc52c38 --- /dev/null +++ b/.svn/pristine/79/798da527fce49381e8e2e8700cfac5604f0577c0.svn-base @@ -0,0 +1,208 @@ +/* + * scan_io_rts implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher + */ + +#include "scanio/scan_io_rts.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::vector; +#include +using std::stringstream; + +#ifdef _MSC_VER +#include +#endif + +#include +#include +using namespace boost::filesystem; + +#include "slam6d/globals.icc" + + + +#define DATA_PATH_PREFIX "scan3d_0_" +#define DATA_PATH_SUFFIX ".3d" +#define POSE_PATH_FILE "odometry_0_sync_interpol.dat" + +//! RTS type flag for invalid points +#define TYPE_INVALID 0x10 + + + +std::list ScanIO_rts::readDirectory(const char* dir_path, unsigned int start, unsigned int end) +{ + std::list identifiers; + // only a single pose file, can't do without one + path pose_path(dir_path); + pose_path /= POSE_PATH_FILE; + if(exists(pose_path)) { + for(unsigned int i = start; i <= end; ++i) { + // identifier is a number (0-\infty) + std::string identifier(to_string(i)); + // scan consists of data (.3d) files + path data(dir_path); + data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); + // stop if part of a scan is missing or end by absence is detected + if(!exists(data)) + break; + identifiers.push_back(identifier); + } + } + return identifiers; +} + +void ScanIO_rts::readPose(const char* dir_path, const char* identifier, double* pose) +{ + unsigned int i; + + // if directory doesn't match the cached one, rebuild pose cache + if(cached_dir != dir_path) { + // check for pose file + path pose_path(dir_path); + pose_path /= POSE_PATH_FILE; + if(!exists(pose_path)) + throw std::runtime_error(std::string("There is no pose file in [") + dir_path + "]"); + + // open pose file once and read all poses + ifstream pose_file(pose_path); + pose_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + vector poses; + double p[6], timestamp; + while(pose_file.good()) { + try { + pose_file >> timestamp + >> p[2] >> p[0] >> p[1] // x, y, z + >> p[3] >> p[5] >> p[4]; // theta_x, theta_y, theta_z + } catch(std::ios_base::failure& e) { + break; + } + + // convert + for(i = 0; i < 3; ++i) p[i] *= 0.1; + + // add in poses + for(i = 0; i < 6; ++i) poses.push_back(p[i]); + } + + // after success, set the cache + cached_poses.swap(poses); + cached_dir = dir_path; + } + + // get index from the identifier and pick the pose + stringstream str(identifier); + unsigned int scan_index; + str >> scan_index; + if(cached_poses.size() < scan_index*6 + 6) + throw std::runtime_error(std::string("There is no pose entry for scan [") + identifier + "]"); + + for(i = 0; i < 6; ++i) + pose[i] = cached_poses[scan_index*6 + i]; + return; +} + +bool ScanIO_rts::supports(IODataType type) +{ + return !!(type & (DATA_XYZ)); +} + +void ScanIO_rts::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) +{ + // TODO: Type and other columns? + 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); + + // read points + // z x y type ? ? + double point[3]; + int type, dummy; + while(data_file.good()) { + try { + data_file >> point[2] >> point[0] >> point[1]; + data_file >> type >> dummy >> dummy; + } catch(std::ios_base::failure& e) { + break; + } + + // convert + point[0] *= 0.1; + point[1] *= -0.1; + point[2] *= 0.1; + + // apply filter and insert point + if(!(type & TYPE_INVALID)) { + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + } + } + 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_rts; +} + + +/** + * 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/7d/7dfbf49a9d26347c51d0395601c913fc47033666.svn-base b/.svn/pristine/7d/7dfbf49a9d26347c51d0395601c913fc47033666.svn-base new file mode 100644 index 0000000..f58f936 --- /dev/null +++ b/.svn/pristine/7d/7dfbf49a9d26347c51d0395601c913fc47033666.svn-base @@ -0,0 +1,82 @@ +/** + * @file panorama.h + * @brife create panorama images from 3D scans. + * This class is a panorama image container with different projections. + * It creats panoramic images with specified resolutions. + * @author HamidReza Houshiar. Jacobs University Bremen gGmbH, Germany. + * @date Date: 2012/05/23 2:00 + */ + +#ifndef PANORAMA_H_ +#define PANORAMA_H_ + +#include "fbr_global.h" + +using namespace std; + +namespace fbr{ + /** + * @class panorama : create panorama images with different projection methods from input scan files(Mat from scan_cv class) in opencv Mat format + * @param iReflectance panorama image from reflectance data + * @param iRange panorama image from range data + * @param iMap panorama map of 3D cartesian coordinate of input scan(same points as iRange and iReflectance) + * @param extendedIMap 3D vector as panorama map with all the points + * @param iWidth width of the panorama image (cols in opencv) + * @param iHeight height of panorama image (rows in opencv) + * @param pMethod projection method for panorama creation + * @param nImage number of images per scan specially for Rectilinear, Pannini and Stereographic projections + * @param pParam special d parameter of Pannini projection (Master Thesis for more info) or special R parameter of Stereographic projection (Master Thesis for more info) + */ + class panorama{ + cv::Mat iReflectance; + cv::Mat iMap; + cv::Mat iRange; + vector > > extendedIMap; + unsigned int iWidth; + unsigned int iHeight; + projection_method pMethod; + unsigned int nImages; + double pParam; + panorama_map_method mapMethod; + + void init(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mapMethod); + void map(int x, int y, cv::MatIterator_ it, double range); + public: + /** + * constructor of class panorama + * @param width the width of the panorama image + * @param height the height of the panorama image + * @param method the projection method + * @param images number of subsets to crate panorama image + * @param param special parameter for pannini or stereographic projections + * @param mapMethod mapping method for panorama image and 3D points + */ + panorama (unsigned int width, unsigned int height, projection_method method); + panorama (unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages); + panorama (unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param); + panorama (unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mapMethod); + /** + * @brief creates the panorama reflectance image and map. + */ + void createPanorama(cv::Mat scan); + /** + * @brief recovers the point cloud from the panorama image and range information + * @param image - input range image to be converted to point cloud + * @param file - destination of .3d file containing the point cloud + */ + void recoverPointCloud(const cv::Mat& range_image, const std::string& file); + + unsigned int getImageWidth(); + unsigned int getImageHeight(); + projection_method getProjectionMethod(); + unsigned int getNumberOfImages(); + double getProjectionParam(); + cv::Mat getReflectanceImage(); + cv::Mat getMap(); + cv::Mat getRangeImage(); + vector > > getExtendedMap(); + panorama_map_method getMapMethod(); + void getDescription(); + }; +} +#endif /* PANORAMA_H_ */ diff --git a/.svn/pristine/7e/7e83972a871ef8774dfcdd91945515d308e06b77.svn-base b/.svn/pristine/7e/7e83972a871ef8774dfcdd91945515d308e06b77.svn-base new file mode 100644 index 0000000..9b5bafd --- /dev/null +++ b/.svn/pristine/7e/7e83972a871ef8774dfcdd91945515d308e06b77.svn-base @@ -0,0 +1,2047 @@ +/* + * show_gl implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#include +#include +#include "show/viewcull.h" +#include "show/scancolormanager.h" + +bool fullydisplayed = true; // true if all points have been drawn to the screen +bool mousemoving = false; // true iff a mouse button has been pressed inside a window, + // but hs not been released +bool keypressed = false; // true iff a key button has been pressed inside a window, + // but hs not been released +double ptstodisplay = 100000; +double lastfps = idealfps; // last frame rate +int pointmode = -1; + +bool smallfont = true; +bool label = true; + +/** + * Displays all data (i.e., points) that are to be displayed + * @param mode spezification for drawing to screen or in selection mode + */ +void DrawPoints(GLenum mode, bool interruptable) +{ + long time = GetCurrentTimeInMilliSec(); + double min = 0.000000001; + double max = 1.0; + LevelOfDetail *= 1.0 + adaption_rate*(lastfps - idealfps)/idealfps; + if (LevelOfDetail > max) LevelOfDetail = max; + else if (LevelOfDetail < min) LevelOfDetail = min; + + // In case of animation + if(frameNr != 0) { + cm->setMode(ScanColorManager::MODE_ANIMATION); + +#ifdef USE_GL_POINTS + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { +#else + for(int iterator = (int)Scan::allScans.size()-1; iterator >= 0; iterator--) { +#endif + // ignore scans that don't have any frames associated with them + if((unsigned int)iterator >= MetaMatrix.size()) continue; + // set usable frame + double* frame; + Scan::AlgoType type; + if((unsigned int)frameNr >= MetaMatrix[iterator].size()) { + // use last possible frame + frame = MetaMatrix[iterator].back(); + type = MetaAlgoType[iterator].back(); + } else { + frame = MetaMatrix[iterator][frameNr]; + type = MetaAlgoType[iterator][frameNr]; + } + if(type == Scan::INVALID) continue; + cm->selectColors(type); + glPushMatrix(); + glMultMatrixd(frame); + + + glPointSize(pointsize); +#ifdef USE_GL_POINTS + ExtractFrustum(pointsize); + cm->selectColors(type); + if (pointmode == 1 ) { + octpts[iterator]->display(); + } else { + octpts[iterator]->displayLOD(LevelOfDetail); + } +#else + for (unsigned int jterator = 0; jterator < vvertexArrayList[iterator].size(); jterator++) { + + if ((jterator == 0) && vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { + cm->selectColors(type); + } + + if (vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { + glCallList(vvertexArrayList[iterator][jterator]->name); + } + } +#endif + glPopMatrix(); + } + + setScansColored(0); + } else { + + if(mode == GL_SELECT){ + // select points mode + // ------------------ + GLuint name = 0; + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + glPushMatrix(); + glMultMatrixd(MetaMatrix[iterator].back()); + + glColor4f(1.0, 0.0, 0.0,1.0); + glPointSize(pointsize + 2.0); + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + glLoadName(name++); + glBegin(GL_POINTS); + glVertex3d((*it)[0], (*it)[1], (*it)[2]); + glEnd(); + } + glPointSize(pointsize); + + glFlush(); + glPopMatrix(); + } + + } else { + + // draw point is normal mode + // ------------------------- + + if (interruptable) { + glDrawBuffer (GL_FRONT); + } + glPointSize(pointsize); + + vector sequence; + calcPointSequence(sequence, current_frame); +#ifdef USE_GL_POINTS + //for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + for(unsigned int i = 0; i < sequence.size(); i++) { + int iterator = sequence[i]; +#else + for(int iterator = (int)Scan::allScans.size()-1; iterator >= 0; iterator--) { +#endif + // ignore scans that don't have any frames associated with them + if((unsigned int)iterator >= MetaMatrix.size()) continue; + // set usable frame + double* frame; + Scan::AlgoType type; + if((unsigned int)current_frame >= MetaMatrix[iterator].size()) { + // use last possible frame + frame = MetaMatrix[iterator].back(); + type = MetaAlgoType[iterator].back(); + } else { + frame = MetaMatrix[iterator][current_frame]; + type = MetaAlgoType[iterator][current_frame]; + } + if (type == Scan::INVALID) continue; + glPushMatrix(); + if (invert) // default: white points on black background + glColor4d(1.0, 1.0, 1.0, 0.0); + else // black points on white background + glColor4d(0.0, 0.0, 0.0, 0.0); + + // glMultMatrixd(MetaMatrix[iterator].back()); + if (current_frame != (int)MetaMatrix.back().size() - 1) { + cm->setMode(ScanColorManager::MODE_ANIMATION); + cm->selectColors(type); + } + glMultMatrixd(frame); + +#ifdef USE_GL_POINTS + //cout << endl << endl; calcRay(570, 266, 1.0, 40000.0); + /* // for height mapped color in the vertex shader + GLfloat v[16]; + for (unsigned int l = 0; l < 16; l++) + v[l] = MetaMatrix[iterator].back()[l]; + glUniformMatrix4fvARB(glGetUniformLocationARB(p, "MYMAT"), 1, 0, v); + */ + ExtractFrustum(pointsize); + if (pointmode == 1 ) { + octpts[iterator]->display(); + } else if (interruptable) { + checkForInterrupt(); + glFlush(); + glFinish(); + if (isInterrupted()) { + glPopMatrix(); + return; + } + octpts[iterator]->display(); + } else { + octpts[iterator]->displayLOD(LevelOfDetail); + } + if (!selected_points[iterator].empty()) { + glColor4f(1.0, 0.0, 0.0, 1.0); + glPointSize(pointsize + 2.0); + glBegin(GL_POINTS); + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + glVertex3d((*it)[0], (*it)[1], (*it)[2]); + } + glEnd(); + glPointSize(pointsize); + } + +#else + for (unsigned int jterator = 0; jterator < vvertexArrayList[iterator].size(); jterator++) { + if (vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { + glCallList(vvertexArrayList[iterator][jterator]->name); + } + } +#endif + glPopMatrix(); + } + } + } + + + if (pointmode == 1 ) { + fullydisplayed = true; + } else { + unsigned long td = (GetCurrentTimeInMilliSec() - time); + if (td > 0) + lastfps = 1000.0/td; + else + lastfps = 1000.0; + fullydisplayed = false; + } + if (interruptable) + fullydisplayed = true; +} + + +void DrawObjects(GLenum mode) { + for (unsigned int i = 0; i < displays.size(); i++) + displays[i]->displayAll(); + +} + +/** + * Draw a smooth path passing from all the camera points. + * + */ +void DrawPath() +{ + + glLineWidth(10.0); + // draw path + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < path_vectorX.size(); j++){ + // set the color + glColor4f(0.0, 1.0, 0.0, 1.0); + // set the points + glVertex3f(path_vectorX.at(j).x,path_vectorX.at(j).y,path_vectorZ.at(j).y); + } + glEnd(); + + // draw lookat path + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < lookat_vectorX.size(); j++){ + //set the color + glColor4d(1.0, 1.0, 0.0, 1.0); + //set the points + glVertex3f(lookat_vectorX.at(j).x,lookat_vectorX.at(j).y,lookat_vectorZ.at(j).y); + } + glEnd(); + + // draw up path + /* + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < ups_vectorX.size(); j++){ + //set the color + glColor4d(0.0, 1.0, 0.0, 0.7); + //set the points + glVertex3f(ups_vectorX.at(j).x,ups_vectorX.at(j).y,ups_vectorZ.at(j).y); + } + glEnd(); + */ +} + +/** + * Draw the camera boxes in the viewer + * + */ +void DrawCameras(void) +{ + for (unsigned int i = 0; i < cams.size(); i++) { + glPushMatrix(); + + // TODO improve upon this primitive camera + Point p = cams[i]; + Point l = Point::norm( lookats[i] - p ); // forward vector + Point u = Point::norm( ups[i] - p ); // up vector + Point r = Point::cross(l,u); // right vector + l = 5 * l; + r = 5 * r; + u = 5 * u; + + Point cube[8]; + cube[0] = p + l - r - u; + cube[1] = p + l + r - u; + cube[2] = p - l + r - u; + cube[3] = p - l - r - u; + cube[4] = p + l - r + u; + cube[5] = p + l + r + u; + cube[6] = p - l + r + u; + cube[7] = p - l - r + u; + + int sides[6][4] = {{0,1,2,3}, {4,5,6,7}, {0,1,5,4}, + {3,2,6,7}, {1,2,6,5}, {0,3,7,4}}; + + if (i+1 == cam_choice) { + glColor4f(1, 0, 1, 1); + } else { + glColor4f(0, 1, 0, 1); + } + // camera cube + glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glBegin(GL_QUADS); + for (int j = 0; j < 6; j++) { + if (j == 2) continue; + for (int k = 0; k < 4; k++) { + int index = sides[j][k]; + glVertex3d(cube[index].x, cube[index].y, cube[index].z); + } + } + glEnd(); + + r = 5 * r; + u = 5 * u; + + glColor4f(1, 1, 0, 1); + if (i+1 == cam_choice) { + glPointSize(10); + } else { + glPointSize(5); + } + glBegin(GL_POINTS); + glVertex3d( lookats[i].x, lookats[i].y, lookats[i].z); + glEnd(); + + Point fcube[8]; + fcube[0] = cube[4]; + fcube[1] = cube[5]; + fcube[2] = cube[1]; + fcube[3] = cube[0]; + fcube[4] = lookats[i] - r + u; + fcube[5] = lookats[i] + r + u; + fcube[6] = lookats[i] + r - u; + fcube[7] = lookats[i] - r - u; + + glLineWidth(2.0); + glLineStipple(1, 0x0C0F); + glEnable(GL_LINE_STIPPLE); + glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); + // camera FOV + glBegin(GL_QUADS); + for (int j = 0; j < 6; j++) { + for (int k = 0; k < 4; k++) { + int index = sides[j][k]; + glVertex3d(fcube[index].x, fcube[index].y, fcube[index].z); + } + } + glEnd(); + glDisable(GL_LINE_STIPPLE); + + + /* + if (i+1 == cam_choice) { + glColor3f(1, 1, 0); + glPointSize(20); + } else { + glColor3f(0, 0, 1); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d(p.x, p.y, p.z); + glEnd(); + + if (i+1 == cam_choice) { + glColor3f(0, 1, 1); + glPointSize(20); + } else { + glColor3f(1, 0, 0); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d( l.x, l.y, l.z); + glEnd(); + + if (i+1 == cam_choice) { + glColor3f(1, 0, 1); + glPointSize(20); + } else { + glColor3f(0, 1, 0); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d( u.x, u.y, u.z); + glEnd(); +*/ + + glPopMatrix(); + } +} + +//----------------------------------------------------------------------------------- + + +/** + * Display function + */ +void DisplayItFunc(GLenum mode, bool interruptable) +{ + /** + * Color of the fog + */ + + GLfloat fogColor[4]; + + if(!invert) { + glEnable(GL_COLOR_LOGIC_OP); + glLogicOp(GL_COPY_INVERTED); + } + + // set the clear color buffer in case of + // both invert and non invert mode + if (invert) + glClearColor(0.0, 0.0, 0.0, 0.0); + else + glClearColor(1.0, 1.0, 1.0, 1.0); + + // clear the color and depth buffer bit + if (!interruptable) { // single buffer mode, we need the depth buffer + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + } + + glPushMatrix(); + + // set the matrix mode + glMatrixMode(GL_MODELVIEW); + + // init modelview matrix + glLoadIdentity(); + + // set the polygon mode + glPolygonMode(GL_FRONT/*_AND_BACK*/, GL_LINE); + + // do the model-transformation + if (haveToUpdate == 6 && path_iterator < path_vectorX.size() ) { + gluLookAt(path_vectorX.at(path_iterator).x, + path_vectorX.at(path_iterator).y, + path_vectorZ.at(path_iterator).y, + lookat_vectorX.at(path_iterator).x, + lookat_vectorX.at(path_iterator).y, + lookat_vectorZ.at(path_iterator).y, + ups_vectorX.at(path_iterator).x - path_vectorX.at(path_iterator).x, + ups_vectorX.at(path_iterator).y - path_vectorX.at(path_iterator).y, + ups_vectorZ.at(path_iterator).y - path_vectorZ.at(path_iterator).y); + } else { + if (cameraNavMouseMode == 1) { + glRotated( mouseRotX, 1, 0, 0); + glRotated( mouseRotY, 0, 1, 0); + glRotated( mouseRotZ, 0, 0, 1); + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + update_view_rotate(0); + } else { + double t[3] = {0,0,0}; + double mat[16]; + QuatToMatrix4(quat, t, mat); + glMultMatrixd(mat); + + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + //glRotated(angle, axis[0], axis[1], axis[2]); // rotate the camera + double rPT[3]; + Matrix4ToEuler(mat, rPT); + mouseRotX = deg(rPT[0]); + mouseRotY = deg(rPT[1]); + mouseRotZ = deg(rPT[2]); + + } + updateControls(); + + glTranslated(X, Y, Z); // move camera + } + +// cout << "Position :" << X << " " << Y << " " << Z << endl; +// cout << "Quaternion:" << quat[0] << " " << quat[1] << " " << quat[2] << " " << quat[3] << endl; +// cout << "Axis/Angle:" << axis[0] << " " << axis[1] << " " << axis[2] << " " << angle << endl; +// cout << "Apex angle:" << cangle << endl; +// cout << endl; + + // process fog + if (show_fog > 0) { + if (show_fog > 3) + fogColor[0] = fogColor[1] = fogColor[2] = fogColor[3] = 1.0; + else + fogColor[0] = fogColor[1] = fogColor[2] = fogColor[3] = 0.0; + glEnable(GL_FOG); + { + // ln(1/2^8) = -5.54517744 -> threshold at which the last color bit is gone due to fog + if (show_fog==1) {fogMode = GL_EXP; fardistance = min(5.54517744 / fogDensity, (double)maxfardistance);} + else if (show_fog==2) {fogMode = GL_EXP2; fardistance = min(sqrt(5.54517744) / fogDensity, (double)maxfardistance);} + else if (show_fog==3) {fogMode = GL_LINEAR; fardistance = 32000.0;} + else if (show_fog==4) {fogMode = GL_EXP; fardistance = (double)maxfardistance; } + else if (show_fog==5) {fogMode = GL_EXP2; fardistance = (double)maxfardistance; } + else if (show_fog==6) {fogMode = GL_LINEAR; fardistance = (double)maxfardistance;} + glFogi(GL_FOG_MODE, fogMode); + glFogfv(GL_FOG_COLOR, fogColor); + glFogf(GL_FOG_DENSITY, fogDensity); + glHint(GL_FOG_HINT, GL_FASTEST); + glFogf(GL_FOG_START, neardistance); + glFogf(GL_FOG_END, maxfardistance); + } + } else { + glDisable(GL_FOG); + fardistance = maxfardistance; + } + if (fardistance > maxfardistance) fardistance = maxfardistance; + if ( fabs(oldfardistance - fardistance) > 0.00001 || fabs(oldneardistance - neardistance) > 0.00001 ) { + oldfardistance = fardistance; + oldneardistance = neardistance; + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + } + + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // + // show the objects __after__ the model-transformation + // for all status variables we show the appropiated thing + // using the drawing functions + // + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + if (show_path == 1) { + double *pose; + glColor4d(1.0, 0.0, 0.0, 1.0); + glLineWidth(5); + glBegin(GL_LINE_STRIP); + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + // set usable type + Scan::AlgoType type; + if((unsigned int)frameNr >= MetaMatrix[i].size()) { + type = MetaAlgoType[i].back(); + } else { + type = MetaAlgoType[i][frameNr]; + } + if(frameNr >= 1 && frameNr < (int)MetaMatrix[i].size()) { + if(type == Scan::INVALID) continue; + // avoid incomplete frames in a scan + if((unsigned int)frameNr >= MetaMatrix[i].size()) + pose = MetaMatrix[i].back(); + else + pose = MetaMatrix[i][frameNr]; + } else { + //pose = MetaMatrix[i].back(); + // avoid incomplete frames in a scan + if((unsigned int)current_frame >= MetaMatrix[i].size()) + pose = MetaMatrix[i].back(); + else + pose = MetaMatrix[i][current_frame]; + } + if(showTopView) { + glVertex3f(pose[12], 2000, pose[14]); + } else { + glVertex3f(pose[12], pose[13], pose[14]); + } + } + glEnd(); + } + + // if show camera is true then draw cameras. + if (show_cameras == 1) { + DrawCameras(); + } + + // if show path is true the draw path. + if (show_path == 1) { + DrawPath(); + } + DrawObjects(mode); + + if (label) DrawUrl(); + + // if show points is true the draw points + if (show_points == 1) DrawPoints(mode, interruptable); + + + glPopMatrix(); + + if (!invert) { + glDisable(GL_COLOR_LOGIC_OP); + } + + // force draw the scene + glFlush(); + glFinish(); +} + +void DrawUrl() { + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + glLoadIdentity(); + glMatrixMode(GL_PROJECTION); + + // Save the current projection matrix + glPushMatrix(); + // Make the current matrix the identity matrix + glLoadIdentity(); + + // Set the projection (to 2D orthographic) + glOrtho(0.0,100.0,0.0,100.0,-1.5,1.5); + + glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // TODO + + glColor4d(0.0,0.0,0.0,0.7); + + glBegin(GL_QUADS); + glVertex3f(0,0,1.49); + glVertex3f(0,6,1.49); + if(smallfont) { + glVertex3f(22,6,1.49); + glVertex3f(22,0,1.49); + } else { + glVertex3f(25,6,1.49); + glVertex3f(25,0,1.49); + } + glEnd(); + + glBlendFunc(GL_ONE, GL_ZERO); + glColor3f(1,1,1); + if(smallfont) { + glRasterPos3f(1,3.5,1.5); + _glutBitmapString(GLUT_BITMAP_8_BY_13, "created by 3DTK"); + glRasterPos3f(1,1,1.5); + _glutBitmapString(GLUT_BITMAP_8_BY_13, "http://threedtk.de"); + } else { + glRasterPos3f(1,3.5,1.5); + _glutBitmapString(GLUT_BITMAP_9_BY_15, "created by 3DTK"); + glRasterPos3f(1,1,1.5); + _glutBitmapString(GLUT_BITMAP_9_BY_15, "http://threedtk.de"); + } + + // Restore the original projection matrix + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + glPopMatrix(); + +} +/** + * Function topview. Set the screen for top view. + */ +void topView() +{ + static GLdouble save_qx, save_qy, save_qz, save_qangle, save_X, save_Y, save_Z; + static GLdouble saveMouseRotX, saveMouseRotY, saveMouseRotZ; + + if (!showTopView) // set to top view + { + showTopView = true; + // save current pose + save_X = X; + save_Y = Y; + save_Z = Z; + save_qx = quat[0]; + save_qy = quat[1]; + save_qz = quat[2]; + save_qangle = quat[3]; + saveMouseRotX = mouseRotX; + saveMouseRotY = mouseRotY; + saveMouseRotZ = mouseRotZ; + + Y = Y - 350.0; + Z = Z + 500.0; + quat[0] = quat[1] = sqrt(0.5); + quat[2] = quat[3] = 0.0; + mouseRotX = 90; + mouseRotY = 0; + mouseRotZ = 0; + + haveToUpdate = 2; + } else { + showTopView = false; + + // restore old settings + X = save_X; + Y = save_Y; + Z = save_Z; + quat[0] = save_qx; + quat[1] = save_qy; + quat[2] = save_qz; + quat[3] = save_qangle; + mouseRotX = saveMouseRotX; + mouseRotY = saveMouseRotY; + mouseRotZ = saveMouseRotZ; + + haveToUpdate = 2; + } +} + +//--------------------------------------------------------------------------- +/** + * This function is called when the user wants to + * delete a camera. + */ + +void callDeleteCamera(int dummy){ + + //iterator for the position of camera + //in the camera list + vector::iterator position; + vector::iterator positionL; + vector::iterator positionU; + + //calculate the position of the camera. we are referring + //to the selected camera + position = cams.begin()+ (cam_choice-1); + positionL = lookats.begin()+ (cam_choice-1); + positionU = ups.begin()+ (cam_choice-1); + + //if no camera present then return + if(cam_choice == 0) + return; + + //if the list is not empty then + if(!cams.empty()){ + //delete the camera from the position + cams.erase(position); + lookats.erase(positionL); + ups.erase(positionU); + //reset the cam_choice spinner values + } + + updateCamera(); +} + + +//--------------------------------------------------------------------------- +/** + * Function to reset the viewer window. + */ + +void resetView(int dummy) +{ + cangle = 60.0; + pzoom = defaultZoom; + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + haveToUpdate = 2; + mouseRotX = 0; + mouseRotY = 0; + mouseRotZ = 0; + + resetRotationButton(); +} + +/** + * Function to set the viewer window back to a previously saved state. + */ + +void setView(double pos[3], double new_quat[4], + double newMouseRotX, double newMouseRotY, double newMouseRotZ, + double newCangle, + bool sTV, bool cNMM, double pzoom_new, + bool s_points, bool s_path, bool s_cameras, double ps, int + sf, double fD, bool inv) +{ + X = pos[0]; + Y = pos[1]; + Z = pos[2]; + for(int i = 0; i < 4; i++) { + quat[i] = new_quat[i]; + } + cangle = newCangle; + mouseRotX = newMouseRotX; + mouseRotY = newMouseRotY; + mouseRotZ = newMouseRotZ; + showTopView = sTV, + cameraNavMouseMode = cNMM; + pzoom = pzoom_new; + updateTopViewControls(); + + show_points = s_points; + show_path = s_path; + show_cameras = s_cameras; + pointsize = ps; + show_fog = sf; + fogDensity = fD; + invert = inv; + + haveToUpdate = 2; +} + + +/** + * This function is called when the viewer is created. This + * acts as the display function. + */ +void CallBackDisplayFunc() +{ + if ((cangle_spinner != 0 && (fabs(cangle_old - cangle) > 0.5)) || + (pzoom_spinner != 0 && (fabs(pzoom_old - pzoom) > 0.5))) { + + cangle_old = cangle; + pzoom_old = pzoom; + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); +#ifdef _MSC_VER + Sleep(25); +#else + usleep(250000); +#endif + } + + glDrawBuffer(buffermode); + // delete framebuffer and z-buffer + + //Call the display function + DisplayItFunc(GL_RENDER ); + + // show the rednered scene + glutSwapBuffers(); + +} + +/** + * This function is called when there is nothing to be done + * in the screen. + */ +void CallBackIdleFunc(void) +{ + +#ifdef _MSC_VER + Sleep(1); +#else + usleep(1000); +#endif + + if(glutGetWindow() != window_id) + glutSetWindow(window_id); + + // return as nothing has to be updated + if (haveToUpdate == 0) { + if (!fullydisplayed && !mousemoving && !keypressed && pointmode == 0 + ) { + glDrawBuffer(buffermode); + //Call the display function + DisplayItFunc(GL_RENDER, true); + } + return; + } + + // case: display is invalid - update it + if (haveToUpdate == 1) { + glutPostRedisplay(); + haveToUpdate = 0; + return; + } + // case: display is invalid - update it with all points +/* if (haveToUpdate == 7) { + showall = true; + glutPostRedisplay(); + haveToUpdate = 0; + return; + }*/ + + // case: camera angle is changed - instead of repeating code call Reshape, + // since these OpenGL commands are the same + if (haveToUpdate == 2) { + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + glutPostRedisplay(); + haveToUpdate = 0; + return; + } + + // case: animation + if(haveToUpdate == 3 ){ + frameNr += 1; + if(!(MetaMatrix.size() > 1 && frameNr < (int) MetaMatrix[1].size())){ + frameNr = 0; + haveToUpdate = 4; + return; + } + glutPostRedisplay(); + + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(frameNr,5) + ".ppm"; + cout << "write " << filename << endl; + int tmpUpdate = haveToUpdate; + glWriteImagePPM(filename.c_str(), factor, 0); + haveToUpdate = tmpUpdate; + + string jpgname = scan_dir + "animframe" + to_string(frameNr,5) + ".jpg"; + string systemcall = "convert -quality 100 -type TrueColor " + filename + " " + jpgname; + // cout << systemcall << endl; + system(systemcall.c_str()); + systemcall = "rm " + filename; + system(systemcall.c_str()); + // cout << systemcall << endl; + // for f in *ppm ; do convert -quality 100 -type TrueColor $f `basename $f ppm`jpg; done + } + + } +#ifdef _MSC_VER + Sleep(300); + Sleep(anim_delay); +#else + usleep(anim_delay * 10000); +#endif + + if (haveToUpdate == 4) { // stop animation + frameNr = 0; // delete these lines if you want a 'continue' functionality. + haveToUpdate = 1; + } + + // case: path animation + if(haveToUpdate == 6){ + + if (path_iterator == 0) { + oldcamNavMode = cameraNavMouseMode; // remember state of old mousenav + cameraNavMouseMode = 0; + } + + // check if the user wants to animate both + // scan matching and the path at the same + //time + + // cout << "path_iterator: " << path_iterator << endl; + if(path_iterator < path_vectorX.size()){ // standard animation case + + // call the path animation function + // hide both the cameras and the path + show_cameras = 0; + show_path = 0; + // increase the iteration count + + path_iterator += 1; + // repaint the screen + glutPostRedisplay(); + + // save the animation + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(path_iterator,5) + ".ppm"; + string jpgname = scan_dir + "animframe" + to_string(path_iterator,5) + ".jpg"; + cout << "written " << filename << " of " << path_vectorX.size() << " files" << endl; + glWriteImagePPM(filename.c_str(), factor, 0); + string systemcall = "convert -quality 100 " + filename + " " + jpgname; + system(systemcall.c_str()); + systemcall = "rm " + filename; + system(systemcall.c_str()); + haveToUpdate = 6; + + } + }else{ // animation has just ended + cameraNavMouseMode = oldcamNavMode; + show_cameras = 1; + show_path = 1; + haveToUpdate = 0; + } + } + +} + + +/** + * This function handles the rotation of the view + */ + +void update_view_rotate(int t) +{ + double view_rotate_button_quat[4]; + + // convert the rotate button matrix to quaternion + //Matrix4ToQuaternion(view_rotate_button, view_rotate_button_quat); + double mat[16]; + for (int i = 0; i < 16; i++) + mat[i] = view_rotate_button[i]; + Matrix4ToQuat(mat, view_rotate_button_quat); + + // normalize the quartenion + QuatNormalize(view_rotate_button_quat); + + // copy it to the global quartenion quat + memcpy(quat, view_rotate_button_quat, sizeof(quat)); +} + +/** + * This function handles the translation of view. + */ +void update_view_translation(int t) +{ + double obj_pos_button1[3]; + + for (int i = 0; i < 3; i++) { + if (fabs(obj_pos_button_old[i] - obj_pos_button[i]) > COMPARE_EPSILON) { + obj_pos_button1[i] = obj_pos_button[i] - obj_pos_button_old[i]; + obj_pos_button_old[i] = obj_pos_button[i]; + } else obj_pos_button1[i] = 0.0; + } + + X = X + obj_pos_button1[0] * view_rotate_button[0] + obj_pos_button1[1] * view_rotate_button[1] + obj_pos_button1[2] * view_rotate_button[2]; + Y = Y + obj_pos_button1[0] * view_rotate_button[4] + obj_pos_button1[1] * view_rotate_button[5] + obj_pos_button1[2] * view_rotate_button[6]; + Z = Z + obj_pos_button1[0] * view_rotate_button[8] + obj_pos_button1[1] * view_rotate_button[9] + obj_pos_button1[2] * view_rotate_button[10]; + +} + + +/** + * handles the animation button + * @param dummy not needed necessary for glui + */ +void startAnimation(int dummy) +{ + if (MetaMatrix.size() > 0) { + if (haveToUpdate != 3) { + haveToUpdate = 3; + } + else // stop animation + haveToUpdate = 4; + } +} + +/** + * calls the resetView function + * @param dummy not needed necessary for glui + */ +void callResetView(int dummy) +{ + if (showTopView) callTopView(dummy); + resetView(0); +} + +/** + * calls the resetView function + * @param dummy not needed necessary for glui + */ +void invertView(int dummy) +{ + invert = !invert; +} + +/** + * calls the topView function + * @param dummy not needed necessary for glui + */ +void callTopView(int dummy) +{ + topView(); + if (showTopView) { + rotButton->disable(); + cangle_spinner->disable(); + pzoom_spinner->enable(); + } else { + rotButton->enable(); + cangle_spinner->enable(); + pzoom_spinner->disable(); + } +} + +/** + * calls the cameraView function + * @param dummy not needed necessary for glui + */ +void callAddCamera(int dummy) +{ + Point campos(-X, -Y, -Z); + + // calculate lookat point + Point lookat; + Point up(0, 0, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = view_rotate_button[i]; + + lookat.x = -50*tmat[2] -X; + lookat.y = -50*tmat[6] -Y; + lookat.z = -50*tmat[10] -Z; + + up.x = 50*tmat[1] -X; + up.y = 50*tmat[5] -Y; + up.z = 50*tmat[9] -Z; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + + updateCamera(); + + // signal to repaint screen + haveToUpdate = 1; +} + +void selectPoints(int x, int y) { + + GLuint selectBuf[BUFSIZE]; + GLint hits; + GLint viewport[4]; + if (selectOrunselect) { + // set the matrix mode + glMatrixMode(GL_MODELVIEW); + // init modelview matrix + glLoadIdentity(); + + // do the model-transformation + if (cameraNavMouseMode == 1) { + glRotated( mouseRotX, 1, 0, 0); + glRotated( mouseRotY, 0, 1, 0); + glRotated( mouseRotZ, 0, 0, 1); + } else { + double t[3] = {0,0,0}; + double mat[16]; + QuatToMatrix4(quat, t, mat); + glMultMatrixd(mat); + + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + double rPT[3]; + Matrix4ToEuler(mat, rPT); + mouseRotX = deg(rPT[0]); + mouseRotY = deg(rPT[1]); + mouseRotZ = deg(rPT[2]); + } + updateControls(); + glTranslated(X, Y, Z); // move camera + + static sfloat *sp2 = 0; +/* for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + if (!selected_points[iterator].empty()) sp2 = *selected_points[iterator].begin(); + + // selected_points[iterator].clear(); + }*/ + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + glPushMatrix(); + glMultMatrixd(MetaMatrix[iterator].back()); + calcRay(x, y, 1.0, 40000.0); + if (select_voxels) { + octpts[iterator]->selectRay(selected_points[iterator], selection_depth); + } else if (brush_size == 0) { + sfloat *sp = 0; + octpts[iterator]->selectRay(sp); + if (sp != 0) { + cout << "Selected point: " << sp[0] << " " << sp[1] << " " << sp[2] << endl; + + if (sp2 != 0) { + cout << "Distance to last point: " + << sqrt( sqr(sp2[0] - sp[0]) + sqr(sp2[1] - sp[1]) + sqr(sp2[2] - sp[2]) ) << endl; + } + sp2 = sp; + + selected_points[iterator].insert(sp); + } + } else { // select multiple points with a given brushsize + octpts[iterator]->selectRayBrushSize(selected_points[iterator], brush_size); + } + + glPopMatrix(); + } + + } else { + // unselect points + glGetIntegerv(GL_VIEWPORT, viewport); + + glSelectBuffer(BUFSIZE, selectBuf); + (void) glRenderMode(GL_SELECT); + + glInitNames(); + glPushName(0); + + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + +// gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), 10.0, 10.0, viewport); + gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), brush_size*2, brush_size*2, viewport); + gluPerspective(cangle, aspect, neardistance, fardistance); + glMatrixMode(GL_MODELVIEW); + DisplayItFunc(GL_SELECT); + + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + + hits = glRenderMode(GL_RENDER); // get hits + ProcessHitsFunc(hits, selectBuf); + } + glPopMatrix(); + glutPostRedisplay(); +} + +void CallBackMouseFuncMoving(int button, int state, int x, int y) +{ + + if( state == GLUT_DOWN) { + mousemoving = true; + } else { + mousemoving = false; + } +} + + +/** + * This function is called after a mousebutton has been pressed. + */ +void CallBackMouseFunc(int button, int state, int x, int y) +{ + // Are we selecting points or moving the camera? + if(cameraNavMouseMode != 1) { // selecting points + if (state == GLUT_DOWN && (button == GLUT_LEFT_BUTTON || button == GLUT_RIGHT_BUTTON)) { + selectPoints(x,y); + } + } else { + if( state == GLUT_DOWN) { + + mouseNavX = x; + mouseNavY = y; + mouseNavButton = button; + + mousemoving = true; + } else { + mouseNavButton = -1; + mousemoving = false; + } + } +} + + +void moveCamera(double x, double y, double z, double rotx, double roty, double rotz) { + interruptDrawing(); + double mat[9]; + + double xr = M_PI * mouseRotX / 180; + double yr = M_PI * mouseRotY / 180; + double zr = M_PI * mouseRotZ / 180; + double c1,c2,c3,s1,s2,s3; + s1 = sin(xr); c1 = cos(xr); + s2 = sin(yr); c2 = cos(yr); + s3 = sin(zr); c3 = cos(zr); + mat[0] = c2*c3; + mat[1] = -c2*s3; + mat[2] = s2; + mat[3] = c1*s3+c3*s1*s2; + mat[4] = c1*c3-s1*s2*s3; + mat[5] = -c2*s1; + mat[6] = s1*s3-c1*c3*s2; + mat[7] = c1*s2*s3+c3*s1; + mat[8] = c1*c2; + + double transX, transY, transZ; + transX = transY = transZ = 0.0; + + mouseRotX += rotx; + mouseRotY -= roty; + mouseRotZ -= rotz; + + if (mouseRotX < -90) mouseRotX=-90; + else if (mouseRotX > 90) mouseRotX=90; + if (mouseRotY > 360) mouseRotY-=360; + else if (mouseRotY < 0) mouseRotY+=360; + if (mouseRotZ > 360) mouseRotZ-=360; + else if (mouseRotZ < 0) mouseRotZ+=360; + + transX += x * mat[0] + y * mat[3] + z * mat[6]; + transY += x * mat[1] + y * mat[4] + z * mat[7]; + transZ += x * mat[2] + y * mat[5] + z * mat[8]; + + + X += transX; + Y += transY; + Z += transZ; + haveToUpdate = 1; + +} + +void KeyboardFunc(int key, bool control, bool alt, bool shift) { + double stepsize = movementSpeed; + if (shift) stepsize *= 10.0; + if (control) stepsize *= 0.1; + + double rotsize = 0.2 * stepsize; + + switch (key) { + case 'w': + case 'W': + moveCamera(0,0,stepsize,0,0,0); + break; + case 'a': + case 'A': + moveCamera(stepsize,0,0,0,0,0); + break; + case 's': + case 'S': + moveCamera(0,0,-stepsize,0,0,0); + break; + case 'd': + case 'D': + moveCamera(-stepsize,0,0,0,0,0); + break; + case 'c': + case 'C': + moveCamera(0,stepsize,0,0,0,0); + break; + case 32: // WXK_SPACE + moveCamera(0,-stepsize,0,0,0,0); + break; + case 314: // WXK_LEFT + moveCamera(0,0,0,0,rotsize,0); + break; + case 315: // WXK_UP + moveCamera(0,0,0,rotsize,0,0); + break; + case 316: // WXK_RIGHT + moveCamera(0,0,0,0,-rotsize,0); + break; + case 317: // WXK_DOWN + moveCamera(0,0,0,-rotsize,0,0); + break; + case 'q': + case 'Q': + case 366: // WXK_PAGEUP + moveCamera(0,0,0,0,0,rotsize); + break; + case 'e': + case 'E': + case 367: // WXK_PAGEDOWN + moveCamera(0,0,0,0,0,-rotsize); + break; + case 'f': + if (!fullscreen) { + fullscreen = true; + glutFullScreen(); + } else { + fullscreen = false; + glutReshapeWindow(current_width, current_height); + } + break; + default: + break; + } +} + +void CallBackMouseMotionFunc(int x, int y) { + double deltaMouseX = x - mouseNavX; + double deltaMouseY = mouseNavY - y; + mouseNavX = x; + mouseNavY = y; + + if(cameraNavMouseMode == 1) { + if( mouseNavButton == GLUT_RIGHT_BUTTON){ + if ( showTopView ) { + deltaMouseX *= 5; + deltaMouseY *= 5; + } + deltaMouseX *= movementSpeed/10.0; // moving 10 pixels is equivalent to one key stroke + deltaMouseY *= movementSpeed/10.0; + moveCamera(deltaMouseX, deltaMouseY, 0, 0,0,0); + } else if( mouseNavButton == GLUT_MIDDLE_BUTTON ){ + if ( !showTopView ) { + deltaMouseY *= -5; + } + deltaMouseX *= movementSpeed/10.0; // moving 10 pixels is equivalent to one key stroke + deltaMouseY *= movementSpeed/10.0; + moveCamera(deltaMouseX, 0, deltaMouseY, 0,0,0); + } else if ( mouseNavButton == GLUT_LEFT_BUTTON ){ + moveCamera(0, 0, 0, deltaMouseY,deltaMouseX,0); + } else { + return; + } + } else { + selectPoints(x,y); + } +} + + + +void initScreenWindow() +{ + // init display + glutInitDisplayMode(GLUT_DEPTH | GLUT_RGBA | GLUT_DOUBLE); + + // define the window position and size + glutInitWindowPosition(START_X, START_Y); + glutInitWindowSize( START_WIDTH, START_HEIGHT ); + + // create window and append callback functions + window_id = glutCreateWindow("3D_Viewer"); + + glutDisplayFunc( CallBackDisplayFunc ); + glutReshapeFunc( CallBackReshapeFunc ); + + glutMouseFunc ( CallBackMouseFunc ); + glutKeyboardFunc ( CallBackKeyboardFunc); + glutKeyboardUpFunc ( CallBackKeyboardUpFunc); + glutMotionFunc ( CallBackMouseMotionFunc); + glutSpecialFunc ( CallBackSpecialFunc); + // glutEntryFunc ( CallBackEntryFunc); + GLUI_Master.set_glutReshapeFunc( CallBackReshapeFunc ); + GLUI_Master.set_glutIdleFunc( CallBackIdleFunc ); + + update_view_rotate(0); + glClearColor(0.0, 0.0, 0.0, 0.0); + // glClearColor(1.0, 1.0, 1.0, 1.0); +} + + +/* +++++++++-------------++++++++++++ + * NAME + * glDumpWindowPPM + * DESCRIPTION + * writes an ppm file of the window + * content + * PARAMETERS + * filename + * RESULT + * writes the framebuffer content + * to a ppm file ++++++++++-------------++++++++++++ */ +void glDumpWindowPPM(const char *filename, GLenum mode) +{ + int win_height, win_width; + int i,j,k,l; // Counter variables + GLubyte *buffer; // The GL Frame Buffer + unsigned char *ibuffer; // The PPM Output Buffer + ofstream fp; // The PPM File + + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + win_height = viewport[3]; + win_width = viewport[2]; + + // Allocate memory for the the frame buffer and output buffer + buffer = new GLubyte[win_width * win_height * RGBA]; + ibuffer = new unsigned char[win_width * win_height * RGB]; + + // Read window contents from GL frame buffer with glReadPixels + glFinish(); + glReadBuffer(buffermode); + glReadPixels(0, 0, win_width, win_height, + GL_RGBA, GL_UNSIGNED_BYTE, buffer); + + // Open the output file + fp.open(filename, ios::out); + + // Write a proper P6 PPM header + fp << "P6" << endl << "# CREATOR: 3D_Viewer by Andreas Nuechter, University of Osnabrueck" + << endl << win_width << " " << win_height << " " << UCHAR_MAX << endl; + + // Loop through the frame buffer data, writing to the PPM file. Be careful + // to account for the frame buffer having 4 bytes per pixel while the + // output file has 3 bytes per pixel + l = 0; + for (i = 0; i < win_height; i++) { // For each row + for (j = 0; j < win_width; j++) { // For each column + for (k = 0; k < RGB; k++) { // For each RGB component + //cout << (RGBA*((win_height-1-i)*win_width+j)+k) << endl; + ibuffer[l++] = (unsigned char) + *(buffer + (RGBA*((win_height-1-i)*win_width+j)+k)); + } // end RGB + } // end column + } // end row + + // to make a video do: + // for f in *ppm ; do convert -quality 100 $f `basename $f ppm`jpg; done + // mencoder "mf://*.jpg" -mf fps=10 -o test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800 + // Write output buffer to the file */ + fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * win_width * win_height)); + fp.close(); + fp.clear(); + delete [] buffer; + delete [] ibuffer; +} + +/* +++++++++-------------++++++++++++ + * NAME + * glDumpWindowPPM + * DESCRIPTION + * writes an ppm file of the window + * content + * size is scale times the window size + * PARAMETERS + * filename + * RESULT + * writes the framebuffer content + * to a ppm file ++++++++++-------------++++++++++++ */ +void glWriteImagePPM(const char *filename, int scale, GLenum mode) +{ + //if(!showTopView) { + int m,o,k; // Counter variables + // Get viewport parameters + double left, right, top, bottom; + double tmp = 1.0/tan(rad(cangle)/2.0); + + // Save camera parameters + GLdouble savedMatrix[16]; + glGetDoublev(GL_PROJECTION_MATRIX,savedMatrix); + GLdouble savedModel[16]; + glGetDoublev(GL_MODELVIEW_MATRIX,savedModel); + //glMatrixMode(GL_PROJECTION); + //glPushMatrix(); + //glMatrixMode(GL_MODELVIEW); + //glPushMatrix(); + top = 1.0/tmp; + bottom = -top; + right = (aspect)/tmp; + left = -right; + + double part_height, part_width; + if(!showTopView) { + part_width = (right - left)/(double)scale; + part_height = (top - bottom)/(double)scale; + } else { + part_height = (2*pzoom)/scale; + part_width = (2*pzoom*aspect)/scale; + cout << part_width << " " << part_height << endl; + } + // Calculate part parameters + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + int win_width = viewport[2]; + int win_height = viewport[3]; + int image_width = scale * win_width; + int image_height = scale * win_height; + + // Allocate memory for the the frame buffer and output buffer + GLubyte *buffer; // The GL Frame Buffer + unsigned char *ibuffer; // The PPM Output Buffer + buffer = new GLubyte[win_width * win_height * RGBA]; + ibuffer = new unsigned char[image_width * image_height * RGB]; + + smallfont = (scale==1); + double height; + if(!showTopView) { + height = bottom; + } else { + height = -pzoom; + } + for(int i = 0; i < scale; i++) { + double width; + if(!showTopView) { + width = left; + } else { + width = -pzoom*aspect; + } + for(int j = 0; j < scale; j++) { + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + label = false; + if(!showTopView) { + glFrustum(neardistance*width, neardistance*(width + part_width), + neardistance*(height), + neardistance*(height + part_height), + neardistance, fardistance); + glMatrixMode(GL_MODELVIEW); + if(i==0 && j==0) { + label = true; + } + DisplayItFunc(mode); + } else { + glOrtho( width, width + part_width, + height, height + part_height, + 1.0, 32000.0 ); + glMatrixMode(GL_MODELVIEW); + if(i==0 && j==0) { + label = true; + } + DisplayItFunc(mode); + } + + // Read window contents from GL frame buffer with glReadPixels + glFinish(); + glReadBuffer(buffermode); + glReadPixels(0, 0, win_width, win_height, GL_RGBA, GL_UNSIGNED_BYTE, buffer); + + // Loop through the frame buffer data, writing to the PPM file. Be careful + // to account for the frame buffer having 4 bytes per pixel while the + // output file has 3 bytes per pixel + // end row + for (m = 0; m < win_height; m++) { // For each row + for (o = 0; o < win_width; o++) { // For each column + for (k = 0; k < RGB; k++) { // For each RGB component + int l = (k+RGB*(image_width*((scale - 1 - i)*win_height + m) + j*win_width + o)); + ibuffer[l] = (unsigned char) *(buffer + (RGBA*((win_height-1-m)*win_width+o)+k)); + } // end RGB + } // end column + } + width += part_width; + } + height += part_height; + } + + // show the starting scene + + // Restore the original projection matrix + glMatrixMode(GL_PROJECTION); + glLoadMatrixd(savedMatrix); + glMatrixMode(GL_MODELVIEW); + glLoadMatrixd(savedModel); + // show the rednered scene + label = true; + smallfont = true; + haveToUpdate=2; + DisplayItFunc(mode); + + ofstream fp; // The PPM File + + // Open the output file + fp.open(filename, ios::out); + + // Write a proper P6 PPM header + fp << "P6" << endl << "# CREATOR: 3D_Viewer by Dorit Borrmann, Jacobs University Bremen gGmbH" + << endl << image_width << " " << image_height << " " << UCHAR_MAX << endl; + + // Write output buffer to the file + fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * image_width * image_height)); + fp.close(); + fp.clear(); + delete [] buffer; + delete [] ibuffer; +} + +/** Reshape Function + * TODO: have to describe it. + * + */ +void CallBackReshapeFunc(int width, int height) +{ + if (!fullscreen) { + current_height = height; + current_width = width; + } + aspect = (double)width/(double)height; + if (!showTopView) { + // usage of the vsize of a structiewport + glViewport(0, 0, (GLint)width, (GLint)height); + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + + // angle, aspect, near clip, far clip + // get matrix + gluPerspective(cangle, aspect, neardistance, fardistance); + + // now use modelview-matrix as current matrix + glMatrixMode(GL_MODELVIEW); + + haveToUpdate = 1; + + } else { + + // usage of the viewport + glViewport ( 0, 0, width, height); + + glMatrixMode ( GL_PROJECTION ); + glLoadIdentity (); + + // get matrix + glOrtho ( -aspect * pzoom, aspect * pzoom, + -1 * pzoom, pzoom, + 1.0, 32000.0 ); + + // now use modelview-matrix as current matrix + glMatrixMode(GL_MODELVIEW); + + haveToUpdate = 1; + + } + // glDepthMask(false); + glEnable(GL_BLEND); // TODO + glBlendFunc(GL_ONE, GL_ZERO); // TODO + // glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // TODO + // glBlendFunc(GL_ONE, GL_ONE); // TODO + // glBlendFunc(GL_SRC_COLOR, GL_DST_COLOR); + glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); + // TODO glDepthFunc(GL_LEQUAL); + glDepthFunc(GL_LESS); //TODO + glEnable(GL_DEPTH_TEST); + glEnable (GL_POINT_SMOOTH); +} + +/** + * Prints out which points were clicked on + */ +void ProcessHitsFunc(GLint hits, GLuint buffer[]) +{ + //cout << "SIZE " << selected_points[0].size() << endl; + //cout << "processing " << endl; + set names; + set unsel_points; + + GLuint *ptr, nr_names; + + ptr = (GLuint *)buffer; + + for(int i = 0 ; i < hits ; i++) { + nr_names = *ptr; + ptr+=3; // skip 2 z values + for(unsigned int j = 0;j < nr_names ; j++){ // iterate over all names + names.insert(*ptr); + + ptr++; + } + } + //cout << "number of names " << names.size() << endl; + if (names.empty()) return; + + int index = 0; + set::iterator nit = names.begin(); + // find the respective name + + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + // iterate over the selected points as in DrawPoints + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + if (index == *nit) { // if the current index is the current name + unsel_points.insert(*it); + nit++; + } + if (nit == names.end()) goto Done; // break out of the loop + index++; + } + } + + Done: + + cout << "Erasing " << endl; + for (set::iterator it = unsel_points.begin(); + it != unsel_points.end(); it++) { // iterate to the index as indicated by the name *ptr + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { // erase for all scans + selected_points[iterator].erase(*it); + } + } + + + cout << "processing done" << endl; + +} + + +//------------------------------------------------------------------ +/** + * This function deals with all our keyboard activities + */ + +void InterfaceFunc(unsigned char key){ + + strncpy(path_file_name, path_filename_edit->get_text(), 1024); + strncpy(pose_file_name, pose_filename_edit->get_text(), 1024); + return; +} + + +void CallBackSpecialFunc(int key , int x, int y) { + //KeyboardFunc(key + 214, false, false, false); + // return; +} + +/** + * Function drawRobotPath + * \brief This functions draws the path where the + * robot has travelled along while taking the scans + */ +void drawRobotPath(int dummy){ + // clear the camera list as we are going to add the cameras + // in the path where the robot travelled. + + // lets loop through the entire frame files to extract the + // total number of places where the robot has taken the scans from + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + //temp variable + double *temp; + // Now, lets go to the last of each frame file to + // extract the transformation matrix obtained + // after scan matching has been done. + glMultMatrixd(MetaMatrix[i].back()); + + // temp is final transformation matrix + temp = MetaMatrix[i].back(); + + Point campos(temp[12], temp[13] + 100, temp[14]); + + // calculate lookat point + Point lookat(0, 0, 50); + Point up(0, 50, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = temp[i]; + lookat.transform(tmat); + lookat.x = lookat.x ; + lookat.y = lookat.y + 100; + lookat.z = lookat.z ; + + up.transform(tmat); + up.x = up.x ; + up.y = up.y + 100; + up.z = up.z ; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + } + updateCamera(); + + // signal for the update of scene + haveToUpdate = 1; +} + +/** + * Calculates the positions of the interpolated camera path positions on the + * Nurbs path. There will be an equal number of intermediate positions between + * neighboring cameras. + */ +void calcInterpolatedCameras(vector vec1, vector vec2) { + NurbsPath::camRatio.clear(); + double distance = 0.0; + double dx, dy, dz; + for(unsigned int i=0;i vec1, vector vec2) +{ + double distance = 0.0; + double dx, dy, dz; + for(unsigned int i=0;i= 'A' && key <= 'Z') { + keymap[key+ ('a'-'A')] = false; + } + if (key >= 'a' && key <= 'z') { + keymap[key+ ('A'-'a')] = false; + } + + for (unsigned int i = 0; i < 256; i++) { + if (keymap[i]) { + keypressed = true; + return; + } + } + keypressed = false; +} + +void CallBackKeyboardFunc(unsigned char key, int x, int y) { + keymap[key] = true; + keypressed = true; + bool cmd,alt,shift; + cmd = glutGetModifiers() & GLUT_ACTIVE_CTRL; + alt = glutGetModifiers() & GLUT_ACTIVE_ALT; + shift = glutGetModifiers() & GLUT_ACTIVE_SHIFT; + if (cmd) { + key += 96; + } + KeyboardFunc(key, cmd, alt, shift); +} + +void mapColorToValue(int dummy) { + switch (listboxColorVal) { + case 0: + cm->setCurrentType(PointType::USE_HEIGHT); + break; + case 1: + cm->setCurrentType(PointType::USE_REFLECTANCE); + break; + case 2: + cm->setCurrentType(PointType::USE_TEMPERATURE); + break; + case 3: + cm->setCurrentType(PointType::USE_AMPLITUDE); + break; + case 4: + cm->setCurrentType(PointType::USE_DEVIATION); + break; + case 5: + cm->setCurrentType(PointType::USE_TYPE); + break; + case 6: + cm->setCurrentType(PointType::USE_COLOR); + break; + default: + break; + }; + resetMinMax(0); +} + +void changeColorMap(int dummy) { + ColorMap c; + GreyMap gm; + HSVMap hsv; + SHSVMap shsv; + JetMap jm; + HotMap hot; + DiffMap diff; + TempMap temp; + + switch (listboxColorMapVal) { + case 0: + // TODO implement no color map + cm->setColorMap(c); + break; + case 1: + cm->setColorMap(gm); + break; + case 2: + cm->setColorMap(hsv); + break; + case 3: + cm->setColorMap(jm); + break; + case 4: + cm->setColorMap(hot); + break; + case 5: + cm->setColorMap(diff); + break; + case 6: + cm->setColorMap(shsv); + break; + case 7: + cm->setColorMap(temp); + break; + default: + break; + } +} + +void minmaxChanged(int dummy) { + cm->setMinMax(mincolor_value, maxcolor_value); +} + +void resetMinMax(int dummy) { + mincolor_value = cm->getMin(); + maxcolor_value = cm->getMax(); + minmaxChanged(0); +} + +void setScansColored(int dummy) { + switch(colorScanVal) { + case 0: + cm->setMode(ScanColorManager::MODE_STATIC); + break; + case 1: + cm->setMode(ScanColorManager::MODE_COLOR_SCAN); + break; + case 2: + cm->setMode(ScanColorManager::MODE_POINT_COLOR); + break; + default: + break; + } +} + + +void changePointMode(int dummy) { + if (dummy == 0) { // always display + if (pointmode != 1) { // standard mode + pointmode = 1; + //never_box->set_int_val(0); + } else { + pointmode = 0; + } + } else if (dummy == 1) { // never display + if (pointmode != -1) { // standard mode + pointmode = -1; + //always_box->set_int_val(0); + } else { + pointmode = 0; + } + } + updatePointModeControls(); +} + + +void callCameraUpdate(int dummy) { + updateCamera(); +} + +void calcPointSequence(vector &sequence, int frameNr) { + sequence.clear(); + vector > dists; + double x,y,z; + + for (unsigned int i = 0; i < octpts.size(); i++) { + // stop at scans that don't have any frames associated with them + if(i >= MetaMatrix.size()) break; + // set usable frame + double* frame; + if((unsigned int)frameNr >= MetaMatrix[i].size()) { + // use last possible frame + frame = MetaMatrix[i].back(); + } else { + frame = MetaMatrix[i][frameNr]; + } + x = frame[12]; + y = frame[13]; + z = frame[14]; + dists.push_back( pair(sqr(X + x) + sqr(Y + y) + sqr(Z + z), i) ); + } + + sort( dists.begin(), dists.end()); + + for (unsigned int i = 0; i < dists.size(); i++) { + sequence.push_back( dists[i].second); + } +} diff --git a/.svn/pristine/7f/7f316f2c49e978d7de57d0e7dec3d98009b24e38.svn-base b/.svn/pristine/7f/7f316f2c49e978d7de57d0e7dec3d98009b24e38.svn-base new file mode 100644 index 0000000..56179aa --- /dev/null +++ b/.svn/pristine/7f/7f316f2c49e978d7de57d0e7dec3d98009b24e38.svn-base @@ -0,0 +1,405 @@ +/* + * basicScan implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/basicScan.h" + +#include "scanio/scan_io.h" +#include "slam6d/kd.h" +#include "slam6d/Boctree.h" +#include "slam6d/ann_kd.h" + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif //WITH_METRICS + +#include +#include +#include +using std::ifstream; +using std::ofstream; +using std::flush; +using std::string; +using std::map; +using std::pair; +using std::vector; + +#include +using namespace boost::filesystem; + + + +void BasicScan::openDirectory(const std::string& path, IOType type, int start, int end) +{ +#ifdef WITH_METRICS + Timer t = ClientMetric::read_scan_time.start(); +#endif //WITH_METRICS + + // create an instance of ScanIO + ScanIO* sio = ScanIO::getScanIO(type); + + // query available scans in the directory from the ScanIO + std::list identifiers(sio->readDirectory(path.c_str(), start, end)); + + Scan::allScans.reserve(identifiers.size()); + + // for each identifier, create a scan + for(std::list::iterator it = identifiers.begin(); it != identifiers.end(); ++it) { + Scan::allScans.push_back(new BasicScan(path, *it, type)); + } + +#ifdef WITH_METRICS + ClientMetric::read_scan_time.end(t); +#endif //WITH_METRICS +} + +void BasicScan::closeDirectory() +{ + // clean up the scan vector + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) + delete *it; + Scan::allScans.clear(); +} + +BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOType type) : + m_path(path), m_identifier(identifier), m_type(type) +{ + init(); + + // request pose from file + double euler[6]; + ScanIO* sio = ScanIO::getScanIO(m_type); + sio->readPose(m_path.c_str(), m_identifier.c_str(), euler); + rPos[0] = euler[0]; + rPos[1] = euler[1]; + rPos[2] = euler[2]; + rPosTheta[0] = euler[3]; + rPosTheta[1] = euler[4]; + rPosTheta[2] = euler[5]; + + // write original pose matrix + EulerToMatrix4(euler, &euler[3], transMatOrg); + + // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead + transformMatrix(transMatOrg); + + // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one + M4identity(dalignxf); +} + +BasicScan::~BasicScan() +{ + // TODO: clean m_data up +} + +void BasicScan::init() +{ + m_filter_max = 0.0; + m_filter_min = 0.0; + m_filter_top = 0.0; + m_filter_bottom = 0.0; + m_range_mutation = 0.0; + m_filter_range_set = false; + m_filter_height_set = false; + m_range_mutation_set = false; +} + + +void BasicScan::setRangeFilter(double max, double min) +{ + m_filter_max = max; + m_filter_min = min; + m_filter_range_set = true; +} + +void BasicScan::setHeightFilter(double top, double bottom) +{ + m_filter_top = top; + m_filter_bottom = bottom; + m_filter_height_set = true; +} + +void BasicScan::setRangeMutation(double range) +{ + m_range_mutation_set = true; + m_range_mutation = range; +} + +void BasicScan::get(unsigned int types) +{ + ScanIO* sio = ScanIO::getScanIO(m_type); + + vector xyz; + vector rgb; + vector reflectance; + vector temperature; + vector amplitude; + vector type; + vector deviation; + + PointFilter filter; + if(m_filter_range_set) + filter.setRange(m_filter_max, m_filter_min); + if(m_filter_height_set) + filter.setHeight(m_filter_top, m_filter_bottom); + if(m_range_mutation_set) + filter.setRangeMutator(m_range_mutation); + + sio->readScan(m_path.c_str(), + m_identifier.c_str(), + filter, + &xyz, + &rgb, + &reflectance, + &temperature, + &litude, + &type, + &deviation); + + // for each requested and filled data vector, allocate and write contents to their new data fields + if(types & DATA_XYZ && !xyz.empty()) { + double* data = reinterpret_cast(create("xyz", sizeof(double) * xyz.size()).get_raw_pointer()); + for(unsigned int i = 0; i < xyz.size(); ++i) data[i] = xyz[i]; + } + if(types & DATA_RGB && !rgb.empty()) { + unsigned char* data = reinterpret_cast(create("rgb", sizeof(unsigned char) * rgb.size()).get_raw_pointer()); + for(unsigned int i = 0; i < rgb.size(); ++i) data[i] = rgb[i]; + } + if(types & DATA_REFLECTANCE && !reflectance.empty()) { + float* data = reinterpret_cast(create("reflectance", sizeof(float) * reflectance.size()).get_raw_pointer()); + for(unsigned int i = 0; i < reflectance.size(); ++i) data[i] = reflectance[i]; + } + if(types & DATA_TEMPERATURE && !temperature.empty()) { + float* data = reinterpret_cast(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer()); + for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i]; + } + if(types & DATA_AMPLITUDE && !amplitude.empty()) { + int* data = reinterpret_cast(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer()); + for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i]; + } + if(types & DATA_TYPE && !type.empty()) { + float* data = reinterpret_cast(create("type", sizeof(double) * type.size()).get_raw_pointer()); + for(unsigned int i = 0; i < type.size(); ++i) data[i] = type[i]; + } + if(types & DATA_DEVIATION && !deviation.empty()) { + float* data = reinterpret_cast(create("deviation", sizeof(float) * deviation.size()).get_raw_pointer()); + for(unsigned int i = 0; i < deviation.size(); ++i) data[i] = deviation[i]; + } +} + +DataPointer BasicScan::get(const std::string& identifier) +{ + // try to get data + map>::iterator it = m_data.find(identifier); + + // create data fields + if(it == m_data.end()) { + // load from file + if(identifier == "xyz") get(DATA_XYZ); else + if(identifier == "rgb") get(DATA_RGB); else + if(identifier == "reflectance") get(DATA_REFLECTANCE); else + if(identifier == "temperature") get(DATA_TEMPERATURE); else + if(identifier == "amplitude") get(DATA_AMPLITUDE); else + if(identifier == "type") get(DATA_TYPE); else + if(identifier == "deviation") get(DATA_DEVIATION); else + // reduce on demand + if(identifier == "xyz reduced") calcReducedOnDemand(); else + if(identifier == "xyz reduced original") calcReducedOnDemand(); else + // show requests reduced points, manipulate in showing the same entry + if(identifier == "xyz reduced show") { + calcReducedOnDemand(); + m_data["xyz reduced show"] = m_data["xyz reduced"]; + } else + if(identifier == "octtree") { + createOcttree(); + } + + it = m_data.find(identifier); + } + + // if nothing can be loaded, return an empty pointer + if(it == m_data.end()) + return DataPointer(0, 0); + else + return DataPointer(it->second.first, it->second.second); +} + +DataPointer BasicScan::create(const std::string& identifier, unsigned int size) +{ + map>::iterator it = m_data.find(identifier); + if(it != m_data.end()) { + // try to reuse, otherwise reallocate + if(it->second.second != size) { + delete it->second.first; + it->second.first = new unsigned char[size]; + it->second.second = size; + } + } else { + // create a new block of data + it = m_data.insert( + std::make_pair( + identifier, + std::make_pair( + new unsigned char[size], + size + ) + ) + ).first; + } + return DataPointer(it->second.first, it->second.second); +} + +void BasicScan::clear(const std::string& identifier) +{ + map>::iterator it = m_data.find(identifier); + if(it != m_data.end()) { + delete it->second.first; + m_data.erase(it); + } +} + + +void BasicScan::createSearchTreePrivate() +{ + DataXYZ xyz_orig(get("xyz reduced original")); + PointerArray ar(xyz_orig); + switch(searchtree_nnstype) + { + case simpleKD: + kd = new KDtree(ar.get(), xyz_orig.size()); + break; + case ANNTree: + kd = new ANNtree(ar.get(), xyz_orig.size()); + break; + case BOCTree: + kd = new BOctTree(ar.get(), xyz_orig.size(), 10.0, PointType(), true); + break; + case -1: + throw runtime_error("Cannot create a SearchTree without setting a type."); + default: + throw runtime_error("SearchTree type not implemented"); + } + + // TODO: make the switch cases above work with CUDA + if (searchtree_cuda_enabled) createANNTree(); +} + +void BasicScan::calcReducedOnDemandPrivate() +{ + // create reduced points and transform to initial position, save a copy of this for SearchTree + calcReducedPoints(); + transformReduced(transMatOrg); + copyReducedToOriginal(); +} + +void BasicScan::createANNTree() +{ + // TODO: metrics +#ifdef WITH_CUDA + if(!ann_kd_tree) { + DataXYZ xyz_orig(get("xyz reduced original")); + ann_kd_tree = new ANNkd_tree(PointArray(xyz_orig).get(), xyz_orig.size(), 3, 1, ANN_KD_STD); + cout << "Cuda tree was generated with " << xyz_orig.size() << " points" << endl; + } else { + cout << "Cuda tree exists. No need for another creation" << endl; + } +#endif +} + +void BasicScan::createOcttree() +{ + string scanFileName = m_path + "scan" + m_identifier + ".oct"; + BOctTree* btree = 0; + + // try to load from file, if successful return + if(octtree_loadOct && exists(scanFileName)) { + btree = new BOctTree(scanFileName); + m_data.insert( + std::make_pair( + "octtree", + std::make_pair( + reinterpret_cast(btree), + 0 // or memorySize()? + ) + ) + ); + return; + } + + // create octtree from scan + if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points + DataXYZ xyz_r(get("xyz reduced show")); + btree = new BOctTree(PointerArray(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true); + } else { // without reduction, xyz + attribute points + float** pts = octtree_pointtype.createPointArray(this); + unsigned int nrpts = size("xyz"); + btree = new BOctTree(pts, nrpts, octtree_voxelSize, octtree_pointtype, true); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + } + + // save created octtree + if(octtree_saveOct) { + cout << "Saving octree " << scanFileName << endl; + btree->serialize(scanFileName); + } + + m_data.insert( + std::make_pair( + "octtree", + std::make_pair( + reinterpret_cast(btree), + 0 // or memorySize()? + ) + ) + ); +} + +unsigned int BasicScan::readFrames() +{ + string filename = m_path + "scan" + m_identifier + ".frames"; + ifstream file(filename.c_str()); + file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + try { + double transformation[16]; + unsigned int type; + do { + file >> transformation >> type; + m_frames.push_back(Frame(transformation, type)); + } while(file.good()); + } catch(...) {} + + return m_frames.size(); +} + +void BasicScan::saveFrames() +{ + string filename = m_path + "scan" + m_identifier + ".frames"; + ofstream file(filename.c_str()); + for(vector::iterator it = m_frames.begin(); it != m_frames.end(); ++it) { + file << it->transformation << it->type << '\n'; + } + file << flush; + file.close(); +} + +unsigned int BasicScan::getFrameCount() +{ + return m_frames.size(); +} + +void BasicScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) +{ + const Frame& frame(m_frames.at(i)); + pose_matrix = frame.transformation; + type = static_cast(frame.type); +} + +void BasicScan::addFrame(AlgoType type) +{ + m_frames.push_back(Frame(transMat, type)); +} diff --git a/.svn/pristine/80/804aef16c807604ac0a4c1ec56fbeb52cc2f8d10.svn-base b/.svn/pristine/80/804aef16c807604ac0a4c1ec56fbeb52cc2f8d10.svn-base new file mode 100644 index 0000000..8978441 --- /dev/null +++ b/.svn/pristine/80/804aef16c807604ac0a4c1ec56fbeb52cc2f8d10.svn-base @@ -0,0 +1,178 @@ +/* + * scan_io_uos implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_uos.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_uos::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_uos::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_uos::supports(IODataType type) +{ + return !!(type & (DATA_XYZ)); +} + +void ScanIO_uos::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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); + + + // aquire header informations + /* OPTIONAL: the header isn't always there, would require a sanity check + unsigned int n, m; + char[3] dummy; + data_file >> n; + for(unsigned int i = 0; i < 3; ++i) data_file >> dummy[i]; + data_file >> m; + values.reserve(n*m*3); + */ + // overread the first line + char dummy[255]; + data_file.getline(dummy, 255); + + // read points + double point[3]; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter and insert point + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + } + 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_uos; +} + + +/** + * 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/81/819fd950bf1502a5f4dd46137719b6f40802aa18.svn-base b/.svn/pristine/81/819fd950bf1502a5f4dd46137719b6f40802aa18.svn-base new file mode 100644 index 0000000..8f98beb --- /dev/null +++ b/.svn/pristine/81/819fd950bf1502a5f4dd46137719b6f40802aa18.svn-base @@ -0,0 +1,210 @@ +/* + * scan_io_ks_rgb implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_ks_rgb.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 "Color_ScanPos" +#define DATA_PATH_SUFFIX " - Scan001.txt" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + + + +std::list ScanIO_ks_rgb::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 (001-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.txt) 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_ks_rgb::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(); +/* +Don't use this part of io_ks because the original io_ks_rgb didn't +have it either, reasoning was "we never got ks_rgb scans with pose", so +the pose files created were already corrected in terms of offset and +scaling + // CAD map -> pose correction [ks x/y/z -> slam -z/y/x] + double tmp; + tmp = pose[0]; + pose[0] = - pose[2]; + pose[2] = tmp; + + // convert coordinate to cm + for(i = 0; i < 3; ++i) pose[i] *= 100.0; +*/ + // 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_ks_rgb::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_RGB | DATA_REFLECTANCE | DATA_AMPLITUDE)); +} + +void ScanIO_ks_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 + "]"); + + + // TODO: support for amplitude and reflectance + if(xyz != 0 || rgb != 0 || reflectance != 0 || amplitude != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // overread the first line + char dummy[255]; + data_file.getline(dummy, 255); + + // read points + double point[8]; + double tmp; + while(data_file.good()) { + try { + for(i = 0; i < 8; ++i) data_file >> point[i]; + } catch(std::ios_base::failure& e) { + break; + } + + // still convert the points, needed for range check + + // the enemy's x/y/z is mapped to slam's x/z/y, shuffle time! + tmp = point[1]; + point[1] = point[2]; + point[2] = tmp; + + // TODO: offset is application specific, handle with care + // correct constant offset (in slam coordinates) + point[0] -= 70000.0; // x + point[2] -= 20000.0; // z + + // convert coordinate to cm + for(i = 0; i < 3; ++i) point[i] *= 100.0; + + // apply filter and insert point + if(filter.check(point)) { + if(xyz != 0) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + if(rgb != 0) { + for(i = 3; i < 6; ++i) rgb->push_back( + static_cast(point[i] * 255.0)); + } + if(reflectance != 0) { + reflectance->push_back(point[7]); + } + if(amplitude != 0) { + amplitude->push_back(point[6]); + } + } + } + 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_ks_rgb; +} + + +/** + * 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/84/84cb681f916fed5a1d04b8bcad34e3e4b9942627.svn-base b/.svn/pristine/84/84cb681f916fed5a1d04b8bcad34e3e4b9942627.svn-base new file mode 100644 index 0000000..1fa3917 --- /dev/null +++ b/.svn/pristine/84/84cb681f916fed5a1d04b8bcad34e3e4b9942627.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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/85/856df877a4996d8a6edc6f8d285145db631721ae.svn-base b/.svn/pristine/85/856df877a4996d8a6edc6f8d285145db631721ae.svn-base new file mode 100644 index 0000000..8223e43 --- /dev/null +++ b/.svn/pristine/85/856df877a4996d8a6edc6f8d285145db631721ae.svn-base @@ -0,0 +1,27 @@ +/** + * @file + * @brief IO of a 3D scan in uos file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_KS_RGB_H__ +#define __SCAN_IO_KS_RGB_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for KS RGB scans + * + * The compiled class is available as shared object file + */ +class ScanIO_ks_rgb : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base b/.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base new file mode 100644 index 0000000..6dbf896 --- /dev/null +++ b/.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base @@ -0,0 +1,754 @@ +/* + * panorama implementation + * + * Copyright (C) HamidReza Houshiar + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/fbr/panorama.h" + +using namespace std; + +namespace fbr{ + + void panorama::init(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){ + iWidth = width; + iHeight = height; + pMethod = method; + nImages = numberOfImages; + pParam = param; + if(mMethod == FARTHEST){ + iMap.create(iHeight, iWidth, CV_32FC(3)); + iMap = cv::Scalar::all(0); + } + else if(mMethod == EXTENDED){ + extendedIMap.resize(iHeight); + for (unsigned int i = 0; i < iHeight; i++) + extendedIMap[i].resize(iWidth); + } + iReflectance.create(iHeight, iWidth, CV_8U); + iReflectance = cv::Scalar::all(0); + iRange.create(iHeight, iWidth, CV_32FC(1)); + iRange = cv::Scalar::all(0); + mapMethod = mMethod; + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){ + init(width, height, method, numberOfImages, param, mMethod); + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param){ + init(width, height, method, numberOfImages, param, FARTHEST); + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages){ + double param = 0; + if(method == PANNINI) + param = 1; + else if (method == STEREOGRAPHIC) + param = 2; + + init(width, height, method, numberOfImages, param, FARTHEST); + } + + panorama::panorama(unsigned int width, unsigned int height, projection_method method){ + double param = 0; + unsigned int numberOfImages = 1; + if(method == RECTILINEAR) + numberOfImages = 3; + else if(method == PANNINI){ + numberOfImages = 3; + param = 1; + } else if (method == STEREOGRAPHIC){ + numberOfImages = 3; + param = 2; + } + + init(width, height, method, numberOfImages, param, FARTHEST); + } + + void panorama::map(int x, int y, cv::MatIterator_ it, double range){ + iReflectance.at(y,x) = (*it)[3]*255;//reflectance + iRange.at(y,x) = range;//range + if(mapMethod == FARTHEST){ + //adding the point with max distance + if( iRange.at(y,x) < range ){ + iMap.at(y,x)[0] = (*it)[0];//x + iMap.at(y,x)[1] = (*it)[1];//y + iMap.at(y,x)[2] = (*it)[2];//z + } + }else if(mapMethod == EXTENDED){ + //adding all the points + cv::Vec3f point; + point[0] = (*it)[0];//x + point[1] = (*it)[1];//y + point[2] = (*it)[2];//z + extendedIMap[y][x].push_back(point); + } + } + + void panorama::createPanorama(cv::Mat scan){ + + //EQUIRECTANGULAR projection + if(pMethod == EQUIRECTANGULAR){ + //adding the longitude to x axis and latitude to y axis + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + double yFactor = (double) iHeight / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI); + //shift all the valuse to positive points on image + double heightLow =(0 - MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + int y = (int) ( yFactor * (theta + heightLow) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //CONIC projection + if(pMethod == CONIC){ + // set up maximum latitude and longitude angles of the robot + double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0, + MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI; + // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html + double Lat0 = 0., Long0 = 0.; + double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0; + double n = (sin(Phi1) + sin(Phi2)) / 2.; + double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1); + double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n; + // set up max values for x and y and add the longitude to x axis and latitude to y axis + double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0)); + double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0)); + double xFactor = (double) iWidth / ( xmax - xmin ); + int widthMax = iWidth - 1; + double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 )); + double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 )); + double yFactor = (double) iHeight / ( ymax - ymin ); + //shift all the values to positive points on image + int heightMax = iHeight - 1; + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //phi == longitude == horizantal angle of view of [0:360] + phi = 180.0 - phi; + phi *= M_PI / 180.0; + //theta == latitude == vertical angle of view of [-40:60] + theta -= 90; + theta *= -1; + theta *= M_PI / 180.0; + + // add minimum x position as an offset + int x = (int) ( xFactor * (sqrt(C - 2 * n * sin( theta) ) / n * sin(n * (phi - Long0)) + fabs(xmin) ) ); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + + // add minimum y position as an offset + int y = (int) ( yFactor * (Rho0 - (1/n * sqrt(C - 2 * n * sin( theta) ) ) * cos(n * (phi - Long0)) + fabs( ymin ) ) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //CYLINDRICAL projection + if(pMethod == CYLINDRICAL){ + //adding the longitude to x and tan(latitude) to y + //find the x and y range + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + double yFactor = (double) iHeight / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI)); + double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + int y = (int) ((double) yFactor * (tan(theta) - tan(heightLow))); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //Mercator Projection + if( pMethod == MERCATOR){ + //find the x and y range + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + double yFactor = (double) iHeight / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) ); + double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI))); + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + int y = (int) ( yFactor * (log(tan(theta) + (1/cos(theta))) - heightLow) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + + //RECTILINEAR projection + if(pMethod == RECTILINEAR){ + //default value for nImages + if(nImages == 0) nImages = 3; + cout<<"Number of images per scan is: "< it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + for(unsigned int j = 0 ; j < nImages ; j++){ + iMinx = j * interval; + iMaxx = (j + 1) * interval; + //check for point in interval + if(phi < iMaxx && phi > iMinx){ + double max, min, coscRectilinear; + //the longitude of projection center + l0 = iMinx + interval / 2; + //finding the min and max of the x direction + coscRectilinear = sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0); + max = (cos(iMaxy) * sin(iMaxx - l0) / coscRectilinear); + coscRectilinear = sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0); + min = (cos(iMiny) * sin(iMinx - l0) / coscRectilinear); + double xFactor = (double) (iWidth / nImages) / (max - min); + double xlow = min; + int widthMax = (iWidth / nImages) - 1; + //finding the min and max of y direction + coscRectilinear = sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0); + max = ( (cos(p1) * sin(iMaxy) - sin(p1) * cos(iMaxy) * cos(iMaxx - l0) )/ coscRectilinear); + coscRectilinear = sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0); + min = ( (cos(p1) * sin(iMiny) - sin(p1) * cos(iMiny) * cos(iMinx - l0) )/ coscRectilinear); + double yFactor = (double) iHeight / (max - min); + double heightLow = min; + int heightMax = iHeight - 1; + //project the points and add them to image + coscRectilinear = sin(p1) * sin(theta) + cos(p1) * cos(theta) * cos(phi - l0); + int x = (int)(xFactor) * ((cos(theta) * sin(phi - l0) / coscRectilinear) - xlow); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + x = x + (j * iWidth / nImages); + int y = (int) (yFactor) * (( (cos(p1) * sin(theta) - sin(p1) * cos(theta) * cos(phi - l0)) / coscRectilinear) - heightLow); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + } + + //PANNINI projection + if(pMethod == PANNINI){ + //default values for nImages and dPannini==pParam + if(pParam == 0) pParam = 1; + if(nImages == 0) nImages = 3; + cout << "Parameter d is:" << pParam <<", Number of images per scan is:" << nImages << endl; + double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval; + interval = 2 * M_PI / nImages; + iMiny = -M_PI/9; + iMaxy = 2*M_PI/9; + //latitude of projection center + p1 = 0; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + for(unsigned int j = 0 ; j < nImages ; j++){ + iMinx = j * interval; + iMaxx = (j + 1) * interval; + //check for point in interval + if(phi < (iMaxx) && phi > (iMinx)){ + double max, min, sPannini; + //the longitude of projection center + l0 = iMinx + interval / 2; + //use the S variable of pannini projection mentioned in the thesis + //finding the min and max of the x direction + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMaxy) + cos(p1) * cos(iMaxx - l0)); + max = sPannini * (sin(iMaxx - l0)); + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMiny) + cos(p1) * cos(iMinx - l0)); + min = sPannini * (sin(iMinx - l0)); + double xFactor = (double) (iWidth / nImages) / (max - min); + double xlow = min; + int widthMax = (iWidth / nImages) - 1; + //finding the min and max of y direction + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMaxy) + cos(p1) * cos(iMaxx - l0)); + max = sPannini * (tan(iMaxy) * (cos(p1) - sin(p1) * 1/tan(iMaxy) * cos(iMaxx - l0))); + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(iMiny) + cos(p1) * cos(iMinx - l0)); + min = sPannini * (tan(iMiny) * (cos(p1) - sin(p1) * 1/tan(iMiny) * cos(iMinx - l0))); + double yFactor = (double) iHeight / (max - min); + double heightLow = min; + int heightMax = iHeight - 1; + //project the points and add them to image + sPannini = (pParam + 1) / (pParam + sin(p1) * tan(theta) + cos(p1) * cos(phi - l0)); + int x = (int)(xFactor) * (sPannini * sin(phi - l0) - xlow); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + x = x + (j * iWidth / nImages); + int y = (int) (yFactor) * ( (sPannini * tan(theta) * (cos(p1) - sin(p1) * (1/tan(theta)) * cos(phi - l0) ) ) - heightLow ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + } + + //STEREOGRAPHIC projection + if(pMethod == STEREOGRAPHIC){ + //default values for nImages and rStereographic==pParam + if(pParam == 0) pParam = 2; + if(nImages == 0) nImages = 3; + cout << "Paremeter R is:" << pParam << ", Number of images per scan is:" << nImages << endl; + // l0 and p1 are the center of projection iminx, imaxx, iminy, imaxy are the bounderis of intervals + double l0, p1, iMinx, iMaxx, iMiny, iMaxy, interval; + interval = 2 * M_PI / nImages; + iMiny = -M_PI/9; + iMaxy = 2*M_PI/9; + //latitude of projection center + p1 = 0; + + //go through all points + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + for (unsigned int j = 0 ; j < nImages ; j++){ + iMinx = j * interval; + iMaxx = (j + 1) * interval; + //check for point in intervals + if(phi < (iMaxx) && phi > (iMinx)){ + double max, min, k; + //longitude of projection center + l0 = iMinx + interval / 2; + //use the R variable of stereographic projection mentioned in the thesis + //finding the min and max of x direction + k = (2 * pParam) / (1 + sin(p1) * sin(p1) + cos(p1) * cos(p1) * cos(iMaxx - l0)); + max = k * cos(p1) * sin (iMaxx - l0); + k = (2 * pParam) / (1 + sin (p1) * sin(p1) + cos(p1) * cos(p1) * cos(iMinx -l0)); + min = k * cos(p1) * sin (iMinx -l0); + double xFactor = (double) (iWidth / nImages) / (max - min); + double xlow = min; + int widthMax = (iWidth / nImages) - 1; + //finding the min and max of y direction + k = (2 * pParam) / (1 + sin(p1) * sin(iMaxy) + cos(p1) * cos(iMaxy) * cos(iMaxx - l0)); + max = k * (cos(p1) * sin(iMaxy) - sin(p1) * cos(iMaxy) * cos(iMaxx - l0)); + k = (2 * pParam) / (1 + sin(p1) * sin(iMiny) + cos(p1) * cos(iMiny) * cos(iMinx - l0)); + min = k * (cos(p1) * sin(iMiny) - sin(p1) * cos(iMiny) * cos(iMinx - l0)); + double yFactor = (double) iHeight / (max - min); + double heightLow = min; + int heightMax = iHeight - 1; + //project the points and add them to image + k = (2 * pParam) / (1 + sin(p1) * sin(theta) + cos(p1) * cos(theta) * cos(phi - l0)); + int x = (int) (xFactor) * (k * cos(theta) * sin(phi - l0) - xlow); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + x = x + (j * iWidth / nImages); + int y = (int) (yFactor) * (k * ( cos(p1) * sin(theta) - sin(p1) * cos(theta) * cos(phi - l0) ) - heightLow); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + } + + //ZAXIS projection + if(pMethod == ZAXIS){ + double zmin = -200; + double zmax = 4000; + //adding the longitude to x axis and latitude to y axis + double xFactor = (double) iWidth / 2 / M_PI; + int widthMax = iWidth - 1; + cout << "ZMAX= " << zmax << " ZMIN= "<< zmin << endl; + double yFactor = (double) iHeight / (zmax - zmin); + //shift all the valuse to positive points on image + double heightLow = zmin; + int heightMax = iHeight - 1; + + cv::MatIterator_ it, end; + + for( it = scan.begin(), end = scan.end(); it != end; ++it){ + double kart[3], polar[3], phi, theta, range; + kart[0] = (*it)[2]/100; + kart[1] = (*it)[0]/-100; + kart[2] = (*it)[1]/100; + toPolar(kart, polar); + //theta == polar[0] == scan [4] + //phi == polar[1] == scan [5] + //range == polar[2] == scan [3] + theta = polar[0] * 180 / M_PI; + phi = polar[1] * 180 / M_PI; + range = polar[2]; + //horizantal angle of view of [0:360] and vertical of [-40:60] + phi = 360.0 - phi; + phi = phi * 2.0 * M_PI / 360.0; + theta -= 90; + theta *= -1; + theta *= 2.0 * M_PI / 360.0; + int x = (int) ( xFactor * phi); + if (x < 0) x = 0; + if (x > widthMax) x = widthMax; + ///////////////////check this + int y = (int) ( yFactor * ((*it)[1] - heightLow) ); + y = heightMax - y; + if (y < 0) y = 0; + if (y > heightMax) y = heightMax; + + //create the iReflectance iRange and map + map(x, y, it, range); + } + } + } + + void panorama::recoverPointCloud(const cv::Mat& range_image, const string& file ) { + std::ofstream scan_file (file.c_str()); + //recover from EQUIRECTANGULAR projection + if(pMethod == EQUIRECTANGULAR) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + //int widthMax = range_image.size().width - 1; + double yFactor = (double) range_image.size().height / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI); + double heightLow = (0 - MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float theta = (heightMax - row) / yFactor - heightLow; + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + //recover from CYLINDRICAL projection + if(pMethod == CYLINDRICAL) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + //int widthMax = range_image.size().width - 1; + double yFactor = (double) range_image.size().height / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI)); + double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI; + //int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float theta = atan2(row + yFactor * tan(heightLow), yFactor); + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + //recover from MERCATOR projection + if(pMethod == MERCATOR) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + double yFactor = (double) range_image.size().height / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) ); + double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI))); + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float theta = 2 * atan2(exp((heightMax - row) / yFactor + heightLow), 1.) - M_PI_2; + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 180.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + //recover from CONIC projection + if(pMethod == CONIC) { + // set up maximum latitude and longitude angles of the robot + double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0, + MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI; + // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html + double Lat0 = 0., Long0 = 0.; + double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0; + double n = (sin(Phi1) + sin(Phi2)) / 2.; + double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1); + double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n; + // set up max values for x and y and add the longitude to x axis and latitude to y axis + double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0)); + double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0)); + double xFactor = (double) range_image.size().width / ( xmax - xmin ); + double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 )); + double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 )); + double yFactor = (double) range_image.size().height / ( ymax - ymin ); + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float x = col * 1. / xFactor - fabs(xmin); + float y = (heightMax - row) * 1. / yFactor - fabs(ymin); + float theta = asin((C - (x*x + (Rho0 - y) * (Rho0 - y)) * n * n) / (2 * n)); + float phi = Long0 + (1./n) * atan2(x, Rho0 - y); + + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + //if ( std::isnan(cartesian[0]) || std::isnan(cartesian[1]) || std::isnan(cartesian[2]) ) continue; + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + scan_file.close(); + } + + unsigned int panorama::getImageWidth(){ + return iWidth; + } + + unsigned int panorama::getImageHeight(){ + return iHeight; + } + + projection_method panorama::getProjectionMethod(){ + return pMethod; + } + + unsigned int panorama::getNumberOfImages(){ + return nImages; + } + + double panorama::getProjectionParam(){ + return pParam; + } + + cv::Mat panorama::getReflectanceImage(){ + return iReflectance; + } + + cv::Mat panorama::getMap(){ + return iMap; + } + + cv::Mat panorama::getRangeImage(){ + return iRange; + } + + vector > > panorama::getExtendedMap(){ + return extendedIMap; + } + + panorama_map_method panorama::getMapMethod(){ + return mapMethod; + } + + void panorama::getDescription(){ + cout << "panorama created with width: " << iWidth << ", and height: " + << iHeight << ", and projection method: " << projectionMethodToString(pMethod) + << ", number of images: " << nImages << ", projection param: " << pParam << "." + << endl; + cout << endl; + } +} + diff --git a/.svn/pristine/88/884ca405924cd5b55bbc514b37378486cb476353.svn-base b/.svn/pristine/88/884ca405924cd5b55bbc514b37378486cb476353.svn-base new file mode 100644 index 0000000..7a4decc --- /dev/null +++ b/.svn/pristine/88/884ca405924cd5b55bbc514b37378486cb476353.svn-base @@ -0,0 +1,154 @@ +/* + * io_tpyes implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +#include "slam6d/io_types.h" + +#include "slam6d/globals.icc" + +#ifdef _MSC_VER +//#include // TODO: TEST +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +#include +#include +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; + else if (strcasecmp(string, "uos_rgb") == 0) return UOS_RGB; + else if (strcasecmp(string, "uos_rrgbt") == 0) return UOS_RRGBT; + else if (strcasecmp(string, "uosr") == 0) return UOSR; + else if (strcasecmp(string, "old") == 0) return OLD; + else if (strcasecmp(string, "rts") == 0) return RTS; + else if (strcasecmp(string, "rts_map") == 0) return RTS_MAP; + else if (strcasecmp(string, "ifp") == 0) return IFP; + else if (strcasecmp(string, "riegl_txt") == 0) return RIEGL_TXT; + else if (strcasecmp(string, "riegl_project") == 0) return RIEGL_PROJECT; + else if (strcasecmp(string, "riegl_rgb") == 0) return RIEGL_RGB; + else if (strcasecmp(string, "riegl_bin") == 0) return RIEGL_BIN; + else if (strcasecmp(string, "zahn") == 0) return ZAHN; + else if (strcasecmp(string, "ply") == 0) return PLY; + else if (strcasecmp(string, "wrl") == 0) return WRL; + else if (strcasecmp(string, "xyz") == 0) return XYZ; + else if (strcasecmp(string, "zuf") == 0) return ZUF; + else if (strcasecmp(string, "asc") == 0) return ASC; + else if (strcasecmp(string, "iais") == 0) return IAIS; + else if (strcasecmp(string, "front") == 0) return FRONT; + else if (strcasecmp(string, "x3d") == 0) return X3D; + else if (strcasecmp(string, "rxp") == 0) return RXP; + else if (strcasecmp(string, "ais") == 0) return AIS; + else if (strcasecmp(string, "oct") == 0) return OCT; + else if (strcasecmp(string, "txyzr") == 0) return TXYZR; + else if (strcasecmp(string, "xyzr") == 0) return XYZR; + else if (strcasecmp(string, "xyz_rgb") == 0) return XYZ_RGB; + else if (strcasecmp(string, "ks") == 0) return KS; + else if (strcasecmp(string, "ks_rgb") == 0) return KS_RGB; + else if (strcasecmp(string, "stl") == 0) return STL; + else if (strcasecmp(string, "leica") == 0) return LEICA; + else if (strcasecmp(string, "pcl") == 0) return PCL; + else if (strcasecmp(string, "pci") == 0) return PCI; + else if (strcasecmp(string, "cad") == 0) return UOS_CAD; + else if (strcasecmp(string, "velodyne") == 0) return VELODYNE; + else if (strcasecmp(string, "velodyne_frames") == 0) return VELODYNE_FRAMES; + else throw std::runtime_error(std::string("Io type ") + string + std::string(" is unknown")); +} + +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: + return "scan_io_uos_frames"; + case UOS_MAP_FRAMES: + return "scan_io_uos_map_frames"; + case UOS_RGB: + return "scan_io_uos_rgb"; + case UOS_RRGBT: + return "scan_io_uos_rrgbt"; + case OLD: + return "scan_io_old"; + case RTS: + return "scan_io_rts"; + case RTS_MAP: + return "scan_io_rts_map"; + case IFP: + return "scan_io_ifp"; + case RIEGL_TXT: + return "scan_io_riegl_txt"; + case RIEGL_PROJECT: + return "scan_io_riegl_project"; + case RIEGL_RGB: + return "scan_io_riegl_rgb"; + case RIEGL_BIN: + return "scan_io_riegl_bin"; + case ZAHN: + return "scan_io_zahn"; + case PLY: + return "scan_io_ply"; + case WRL: + return "scan_io_wrl"; + case XYZ: + return "scan_io_xyz"; + case ZUF: + return "scan_io_zuf"; + case ASC: + return "scan_io_asc"; + case IAIS: + return "scan_io_iais"; + case FRONT: + return "scan_io_front"; + case X3D: + return "scan_io_x3d"; + case RXP: + return "scan_io_rxp"; + case AIS: + return "scan_io_ais"; + case OCT: + return "scan_io_oct"; + case TXYZR: + return "scan_io_txyzr"; + case XYZR: + return "scan_io_xyzr"; + case XYZ_RGB: + return "scan_io_xyz_rgb"; + case KS: + return "scan_io_ks"; + case KS_RGB: + return "scan_io_ks_rgb"; + case STL: + return "stl"; + case LEICA: + return "leica_txt"; + case PCL: + return "pcl"; + case PCI: + return "pci"; + case UOS_CAD: + return "cad"; + case VELODYNE: + return "scan_io_velodyne"; + case 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/89/89d59fde8504d5c4792e36cc8576ff3cded2ea7a.svn-base b/.svn/pristine/89/89d59fde8504d5c4792e36cc8576ff3cded2ea7a.svn-base new file mode 100644 index 0000000..3b45148 --- /dev/null +++ b/.svn/pristine/89/89d59fde8504d5c4792e36cc8576ff3cded2ea7a.svn-base @@ -0,0 +1,1165 @@ +/* + * 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; + +/** + * Center of Mass coordinates + */ +GLdouble CoM[3] = { 0.0, 0.0, 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 << " -D, --temperature, --degree" << normal << endl + << " use temperature 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 << " -b" << normal << " NR, " << bold << "--sphere=" << normal << "NR" << endl + << " map all measurements on a sphere (of radius NRcm)" << 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, + double& sphereMode) +{ + 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, 'C' }, + { "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' }, + { "temperature", no_argument, 0, 'D' }, + { "degree", no_argument, 0, 'D' }, + { "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' }, + { "sphere", required_argument, 0, 'b' }, + { 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:SwtRDadhTcb", 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 'D': + types |= PointType::USE_TEMPERATURE; + 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; + case 'b': + sphereMode = atof(optarg); + 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) { + // set origin to the pose of the first scan + 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) { + // set origin to the center of the first octree + double center[3], center_transformed[3]; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[0])->getCenter(center); +#else + ((Show_BOctTree*)octpts[0])->getCenter(center); +#endif + VTrans(MetaMatrix[0].back(), center, center_transformed); + RVX = -center_transformed[0]; + RVY = -center_transformed[1]; + RVZ = -center_transformed[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } else if (origin == 3) { + // set origin to the center of mass of all scans + for (size_t i = 0; i < octpts.size(); ++i) { + vector points; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[i])->AllPoints( points ); +#else + BOctTree* cur_tree = ((Show_BOctTree*)octpts[i])->getTree(); + cur_tree->AllPoints( points ); +#endif + + cout << "Scan " << i << " size: " << points.size() << endl; + double centroid[3] = {0., 0., 0.}; + double centroid_transformed[3];; + for (size_t j = 0; j < points.size(); ++j) { + for (unsigned int k = 0; k < 3; ++k) + centroid[k] += points[j][k]; + } + for (unsigned int k = 0; k < 3; ++k) { + centroid[k] /= (double)points.size(); + } + VTrans(MetaMatrix[i].back(), centroid, centroid_transformed); + for (unsigned int k = 0; k < 3; ++k) { + CoM[k] += centroid_transformed[k]; + } + } + for (unsigned int k = 0; k < 3; ++k) + CoM[k] /= octpts.size() * 1.; + + cout << "Center of Mass at: " << CoM[0] << ", " << CoM[1] << ", " << CoM[2] << endl; + + RVX = -CoM[0]; + RVY = -CoM[1]; + RVZ = -CoM[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; + double sphereMode = 0.0; + + 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, sphereMode); + + // 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 (sphereMode > 0.0) scan->setRangeMutation(sphereMode); + 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/8e/8e9f9f6ff76474503ac8cca8bcc2d666997014f8.svn-base b/.svn/pristine/8e/8e9f9f6ff76474503ac8cca8bcc2d666997014f8.svn-base new file mode 100644 index 0000000..d815fb1 --- /dev/null +++ b/.svn/pristine/8e/8e9f9f6ff76474503ac8cca8bcc2d666997014f8.svn-base @@ -0,0 +1,83 @@ +/** + * @file + * @brief IO of a 3D scan in rxp file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_RXP_H__ +#define __SCAN_IO_RXP_H__ + +#include "scan_io.h" +#include "slam6d/point.h" + +#include "riegl/scanlib.hpp" +using namespace scanlib; +using namespace std; +using namespace std::tr1; + +class importer; + + + +/** + * @brief 3D scan loader for RXP scans + * + * The compiled class is available as shared object file + */ +class ScanIO_rxp : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); + + ScanIO_rxp() : dec(0), imp(0) {} +private: + std::tr1::shared_ptr rc; + decoder_rxpmarker *dec; + importer *imp; + std::string old_path; +}; + +/** + * The importer class is the interface to riegl's pointcloud class, and will convert their point struct to slam6d's point class. + * + * Code adapted from rivlib/example/pointcloudcpp.cpp available from http://www.riegl.com . + */ +class importer : public scanlib::pointcloud +{ +public: + importer(PointFilter& filter, int start, std::vector* xyz, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation) : + pointcloud(false), // set this to true if you need gps aligned timing + filter(&filter), xyz(xyz), reflectance(reflectance), amplitude(amplitude), type(type), deviation(deviation), + start(start), currentscan(0) + {} + inline int getCurrentScan() { return currentscan; } + inline void set(PointFilter& filter, std::vector* xyz, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation) + { + this->filter = &filter; + this->xyz = xyz; + this->reflectance = reflectance; + this->amplitude = amplitude; + this->type = type; + this->deviation = deviation; + } +protected: + PointFilter* filter; + std::vector *xyz; + std::vector *reflectance; + std::vector *amplitude; + std::vector *type; + std::vector *deviation; + int start; + int currentscan; + // overridden from pointcloud class + void on_echo_transformed(echo_type echo); + + void on_frame_stop(const scanlib::frame_stop& arg) { + scanlib::basic_packets::on_frame_stop(arg); + currentscan++; + } +}; + +#endif diff --git a/.svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base b/.svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base new file mode 100644 index 0000000..93ede82 --- /dev/null +++ b/.svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base @@ -0,0 +1,282 @@ +/* + * sharedScan implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann + * + * Released under the GPL version 3. + * + */ + +#include "scanserver/sharedScan.h" + +#include +using std::cout; +using std::cerr; +using std::endl; +#include + +#include "scanserver/clientInterface.h" + + + +SharedScan::SharedScan(const ip::allocator & allocator, + const SharedStringSharedPtr& dir_path_ptr, const char *io_identifier, + IOType iotype) : + m_dir_path_ptr(dir_path_ptr), + m_io_identifier(io_identifier, allocator), + m_iotype(iotype), + m_prefetch(0), + m_max_dist(0.0), m_min_dist(0.0), + m_height_top(0.0), m_height_bottom(0.0), + m_range_mutator_param(0.0), + m_range_mutator_param_set(false), + m_range_param_set(false), m_height_param_set(false), + m_reduction_parameters(allocator), + m_show_parameters(allocator), + m_octtree_parameters(allocator), + m_load_frames_file(true), + m_frames(allocator) +{ + // until boost-1.47 ipc-strings can do garbage with g++-4.4 -O2 and higher (optimizations and versions) + std::string bugfix(io_identifier); + m_io_identifier[bugfix.length()] = '\0'; + + // COs are allocated in the ServerScan-ctor +} + +SharedScan::~SharedScan() +{ + // TODO + // get segment manager and deconstruct all available data fields + // TODO: Delete COs if notzero and notify CM? +} + +bool SharedScan::operator==(const SharedScan& r) const +{ + return (m_io_identifier == r.m_io_identifier) && (*m_dir_path_ptr == *r.m_dir_path_ptr) && m_iotype == r.m_iotype; +} + +void SharedScan::setRangeParameters(double max_dist, double min_dist) +{ + // if a non-first set differs from the previous ones, invalidate all COs + if(m_range_param_set) { + if(max_dist != m_max_dist || min_dist != m_min_dist) { + invalidateFull(); + invalidateReduced(); + invalidateShow(); + } + } + m_max_dist = max_dist; + m_min_dist = min_dist; + m_range_param_set = true; +} + +void SharedScan::setHeightParameters(double top, double bottom) +{ + // if a non-first set differs from the previous ones, invalidate all COs + if(m_height_param_set) { + if(top != m_height_top || bottom != m_height_bottom) { + invalidateFull(); + invalidateReduced(); + invalidateShow(); + } + } + m_height_top = top; + m_height_bottom = bottom; + m_height_param_set = true; +} + +void SharedScan::setRangeMutationParameters(double range) +{ + // if a non-first set differs from the previous ones, invalidate all COs + if(m_range_mutator_param_set) { + if(range != m_range_mutator_param) { + invalidateFull(); + invalidateReduced(); + invalidateShow(); + } + } + m_range_mutator_param = range; + m_range_mutator_param_set = true; +} + +void SharedScan::setReductionParameters(const char* params) +{ + // if a non-first set differs from the previous ones, invalidate reduced COs + if(!m_reduction_parameters.empty() && m_reduction_parameters != params) { + invalidateReduced(); + } + m_reduction_parameters = params; +} + +void SharedScan::setShowReductionParameters(const char* params) +{ + // if a non-first set differs from the previous ones, invalidate reduced COs + if(!m_show_parameters.empty() && m_show_parameters != params) { + invalidateShow(); + } + m_show_parameters = params; +} + +void SharedScan::setOcttreeParameters(const char* params) +{ + // if a non-first set differs from the previous ones, invalidate reduced COs + if(!m_octtree_parameters.empty() && m_octtree_parameters != params) { + m_octtree->invalidate(); + } + m_octtree_parameters = params; +} + +PointFilter SharedScan::getPointFilter() const +{ + PointFilter r; + if(m_range_param_set) + r.setRange(m_max_dist, m_min_dist); + if(m_height_param_set) + r.setHeight(m_height_top, m_height_bottom); + if(m_range_mutator_param_set) + r.setRangeMutator(m_range_mutator_param); + + return r; +} + +void SharedScan::invalidateFull() +{ + m_xyz->invalidate(); + m_rgb->invalidate(); +} + +void SharedScan::invalidateReduced() +{ + m_xyz_reduced->invalidate(); + m_xyz_reduced_original->invalidate(); +} + +void SharedScan::invalidateShow() +{ + m_show_reduced->invalidate(); + m_octtree->invalidate(); +} + +void SharedScan::clearFrames() +{ + ClientInterface* client = ClientInterface::getInstance(); + client->clearFrames(this); + // don't try to load again from the still existing files + m_load_frames_file = false; +} + +void SharedScan::addFrame(double* transformation, unsigned int type) +{ + ClientInterface* client = ClientInterface::getInstance(); + client->addFrame(this, transformation, type); +} + +const FrameVector& SharedScan::getFrames() +{ + // on a restart with existing frame files try to load these + if(m_frames.empty() && m_load_frames_file == true) { + ClientInterface* client = ClientInterface::getInstance(); + client->loadFramesFile(this); + // don't try to load again if frames are still empty + m_load_frames_file = false; + } + return m_frames; +} + +void SharedScan::saveFrames() +{ + ClientInterface* client = ClientInterface::getInstance(); + client->saveFramesFile(this); + // we just saved the file, no need to read it + m_load_frames_file = false; +} + +double* SharedScan::getPose() +{ + if(m_pose == 0) { + ClientInterface* client = ClientInterface::getInstance(); + client->getPose(this); + } + return m_pose.get(); +} + +DataXYZ SharedScan::getXYZ() { + return m_xyz.get()->getCacheData(); +} + +DataRGB SharedScan::getRGB() { + return m_rgb->getCacheData(); +} + +DataReflectance SharedScan::getReflectance() { + return m_reflectance->getCacheData(); +} + +DataTemperature SharedScan::getTemperature() { + return m_temperature->getCacheData(); +} + +DataAmplitude SharedScan::getAmplitude() { + return m_amplitude->getCacheData(); +} + +DataType SharedScan::getType() { + return m_type->getCacheData(); +} + +DataDeviation SharedScan::getDeviation() { + return m_deviation->getCacheData(); +} + +DataXYZ SharedScan::getXYZReduced() { + return m_xyz_reduced->getCacheData(); +} + +DataXYZ SharedScan::createXYZReduced(unsigned int size) { + // size is in units of double[3], scale to bytes + return m_xyz_reduced->createCacheData(size*3*sizeof(double)); +} + +DataXYZ SharedScan::getXYZReducedOriginal() { + return m_xyz_reduced_original->getCacheData(); +} + +DataXYZ SharedScan::createXYZReducedOriginal(unsigned int size) { + // size is in units of double[3], scale to bytes + return m_xyz_reduced_original->createCacheData(size*3*sizeof(double)); +} + +TripleArray SharedScan::getXYZReducedShow() { + return m_show_reduced->getCacheData(); +} + +TripleArray SharedScan::createXYZReducedShow(unsigned int size) { + return m_show_reduced->createCacheData(size*3*sizeof(float)); +} + +DataPointer SharedScan::getOcttree() { + return m_octtree->getCacheData(); +} + +DataPointer SharedScan::createOcttree(unsigned int size) { + return m_octtree->createCacheData(size); +} + +void SharedScan::onCacheMiss(CacheObject* obj) +{ + ClientInterface* client = ClientInterface::getInstance(); + client->loadCacheObject(obj); +} + +void SharedScan::onAllocation(CacheObject* obj, unsigned int size) +{ + ClientInterface* client = ClientInterface::getInstance(); + client->allocateCacheObject(obj, size); +} + +void SharedScan::onInvalidation(CacheObject* obj) +{ + ClientInterface* client = ClientInterface::getInstance(); + client->invalidateCacheObject(obj); +} diff --git a/.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base b/.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base new file mode 100644 index 0000000..35aed9d --- /dev/null +++ b/.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base @@ -0,0 +1,410 @@ +cmake_minimum_required (VERSION 2.6.1) +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH}) +project (Slam6D) + +#include_directories(OPENGL_INCLUDE_DIR) +IF(WIN32) + set(Boost_USE_STATIC_LIBS TRUE) +ELSE(WIN32) + set(Boost_USE_STATIC_LIBS FALSE) +ENDIF(WIN32) + +SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49") +IF(WIN32) + # for some unknown reason no one variant works on all windows platforms + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED) +ELSE(WIN32) + find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED) +ENDIF(WIN32) + +if(Boost_FOUND) + link_directories(${BOOST_LIBRARY_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS}) +endif() + +################################################# +# Declare Options and modify build accordingly ## +################################################# + +## FreeGLUT +OPTION(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON) + +IF(WITH_FREEGLUT) + MESSAGE(STATUS "With freeglut") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT") +ELSE(WITH_FREEGLUT) + MESSAGE(STATUS "Without freeglut") +ENDIF(WITH_FREEGLUT) + +## Show +OPTION(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON) + +IF(WITH_SHOW) + FIND_PACKAGE(OpenGL REQUIRED) + FIND_PACKAGE(GLUT REQUIRED) + MESSAGE(STATUS "With show") +ELSE(WITH_SHOW) + # SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE) + MESSAGE(STATUS "Without show") +ENDIF(WITH_SHOW) + +## WXShow +OPTION(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF) + +IF(WITH_WXSHOW) + FIND_PACKAGE(OpenGL REQUIRED) + FIND_PACKAGE(GLUT REQUIRED) + find_package(wxWidgets COMPONENTS core base gl REQUIRED) + # set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration + SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE) + # wxWidgets include (this will do all the magic to configure everything) + include( ${wxWidgets_USE_FILE}) + MESSAGE(STATUS "With wxshow") +ELSE(WITH_XWSHOW) + MESSAGE(STATUS "Without wxshow") +ENDIF(WITH_WXSHOW) + +## Shapes +OPTION(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF) +IF(WITH_SHAPE_DETECTION) + MESSAGE(STATUS "With shape detection") +ELSE(WITH_SHAPE_DETECTION) + MESSAGE(STATUS "Without shape detection") +ENDIF(WITH_SHAPE_DETECTION) + +## Interior reconstruction +option(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF) + +if(WITH_MODEL) + message(STATUS "With interior reconstruction") +else(WITH_MODEL) + message(STATUS "Without interior reconstruction") +endif(WITH_MODEL) + +## Thermo +OPTION(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF) +IF(WITH_THERMO) + FIND_PACKAGE(OpenCV REQUIRED) + add_subdirectory(3rdparty/cvblob) + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) + MESSAGE(STATUS "With thermo") +ELSE(WITH_THERMO) + MESSAGE(STATUS "Without thermo") +ENDIF(WITH_THERMO) + +## Octree +OPTION(WITH_OCTREE_DISPLAY "Whether to use octree display for efficiently culling scans ON/OFF" ON) + +IF(WITH_OCTREE_DISPLAY) + MESSAGE(STATUS "Using octree display") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_GL_POINTS") +ELSE(WITH_OCTREE_DISPLAY) + MESSAGE(STATUS "Using displaylists: Warning potentially much slower") +ENDIF(WITH_OCTREE_DISPLAY) +#SET (WITH_OCTREE_DISPLAY ${WITH_OCTREE_DISPLAY} CACHE BOOL +#"Whether to use octree display for efficiently culling scans ON/OFF" FORCE) + + +## Octree +OPTION(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF) + +IF(WITH_COMPACT_OCTREE) + MESSAGE(STATUS "Using compact octrees") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE") +ELSE(WITH_COMPACT_OCTREE) + MESSAGE(STATUS "Not using compact octreees: Warning uses more memory") +ENDIF(WITH_COMPACT_OCTREE) + +## Glee? +OPTION(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF) + +IF(WITH_GLEE) + MESSAGE(STATUS "Using opengl extensions") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE") +ELSE(WITH_GLEE) + MESSAGE(STATUS "Not using opengl extensions") +ENDIF(WITH_GLEE) + +## Gridder +OPTION(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF) + +IF(WITH_GRIDDER) + MESSAGE(STATUS "With 2DGridder") +ELSE(WITH_GRIDDER) + MESSAGE(STATUS "Without 2DGridder") +ENDIF(WITH_GRIDDER) + +## Dynamic VELOSLAM +OPTION(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF) + +IF(WITH_VELOSLAM) + MESSAGE(STATUS "With VELOSLAM") +ELSE(WITH_VELOSLAM) + MESSAGE(STATUS "Without VELOSLAM") +ENDIF(WITH_VELOSLAM) + +## Home-made Laserscanner +OPTION(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF) + +IF(WITH_DAVID_3D_SCANNER) + MESSAGE(STATUS "With David scanner") +ELSE(WITH_DAVID_3D_SCANNER) + MESSAGE(STATUS "Without David scanner") +ENDIF(WITH_DAVID_3D_SCANNER) + +## Tools + +OPTION(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF) + +IF(WITH_TOOLS) + MESSAGE(STATUS "With Tools") +ELSE(WITH_TOOLS) + MESSAGE(STATUS "Without Tools") +ENDIF(WITH_TOOLS) + + + +## CAD matching +OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF) + +IF (WITH_CAD) + MESSAGE (STATUS "With CAD import") + find_package (Boost COMPONENTS program_options filesystem REQUIRED) +ELSE (WITH_CAD) + MESSAGE (STATUS "Without CAD import") +ENDIF (WITH_CAD) + +## RivLib +OPTION(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF) + +IF(WITH_RIVLIB) + MESSAGE(STATUS "Compiling a scan IO for RXP files") + include_directories(${CMAKE_SOURCE_DIR}/3rdparty) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty) + SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/) + IF(WIN32) + SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib) + ELSE(WIN32) + SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread) + ENDIF(WIN32) + FIND_PACKAGE(LibXml2 ) + +ELSE(WITH_RIVLIB) + MESSAGE(STATUS "Do NOT compile a scan IO for RXP") +ENDIF(WITH_RIVLIB) + +## CUDA support, TODO depend on CUDA_FIND +OPTION(WITH_CUDA "Compile with CUDA support" OFF) +IF(WITH_CUDA) + MESSAGE(STATUS "Compiling WITH CUDA support") + FIND_PACKAGE(CUDA) + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA") +ELSE(WITH_CUDA) + MESSAGE(STATUS "Compiling WITHOUT CUDA support") +ENDIF(WITH_CUDA) + +## PMD +OPTION(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF) + +IF(WITH_PMD) + FIND_PACKAGE(OpenGL REQUIRED) + MESSAGE(STATUS "With Tools") +ELSE(WITH_PMD) + MESSAGE(STATUS "Without Tools") +ENDIF(WITH_PMD) + +## FBR +OPTION(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF) + +IF(WITH_FBR) + MESSAGE(STATUS "With FBR ") +ELSE(WITH_FBR) + MESSAGE(STATUS "Without FBR") +ENDIF(WITH_FBR) + +## Special treatment for system specifics +IF(APPLE) +add_definitions(-Dfopen64=fopen) +ENDIF(APPLE) + +## Multiple Cores +IF(APPLE) + SET(PROCESSOR_COUNT 2) +ELSE(APPLE) + INCLUDE(CountProcessors) + SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" ) +ENDIF(APPLE) + +# OPEN +FIND_PACKAGE(OpenMP) +IF(OPENMP_FOUND) + OPTION(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON) +ENDIF(OPENMP_FOUND) + +IF(OPENMP_FOUND AND WITH_OPENMP) + MESSAGE(STATUS "With OpenMP ") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${PROCESSOR_COUNT} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP") +ELSE(OPENMP_FOUND AND WITH_OPENMP) + MESSAGE(STATUS "Without OpenMP") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1") +ENDIF(OPENMP_FOUND AND WITH_OPENMP) + +## TORO +OPTION(WITH_TORO "Whether to use TORO. ON/OFF" OFF) + +IF(WITH_TORO) + IF(WIN32) + SET(Subversion_SVN_EXECUTABLE "svn.exe") + ENDIF(WIN32) + cmake_minimum_required (VERSION 2.8) + include(ExternalProject) + ExternalProject_Add(toro3d + SVN_REPOSITORY https://www.openslam.org/data/svn/toro/trunk + SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/toro" + CONFIGURE_COMMAND "" + BUILD_COMMAND make + BUILD_IN_SOURCE 1 + INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/toro/toro3d ${CMAKE_SOURCE_DIR}/bin/ + ) + MESSAGE(STATUS "With TORO ") +ELSE(WITH_TORO) + MESSAGE(STATUS "Without TORO") +ENDIF(WITH_TORO) + + +## HOGMAN +OPTION(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF) + +IF(WITH_HOGMAN) + # dependant on libqt4-devi + find_package( Qt4 REQUIRED) + # CMake of earlier versions do not have external project capabilities + cmake_minimum_required (VERSION 2.8) + include(ExternalProject) + ExternalProject_Add(hogman + SVN_REPOSITORY https://svn.openslam.org/data/svn/hog-man/trunk + SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/hogman" + CONFIGURE_COMMAND /configure --prefix= + BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make + BUILD_IN_SOURCE 1 + INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ && + cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/ + ) + MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH") +ELSE(WITH_HOGMAN) + MESSAGE(STATUS "Without HOGMAN") +ENDIF(WITH_HOGMAN) + +OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF) + +IF(EXPORT_SHARED_LIBS) + MESSAGE(STATUS "exporting additional libraries") +ELSE(EXPORT_SHARED_LIBS) + MESSAGE(STATUS "not exporting libraries") +ENDIF(EXPORT_SHARED_LIBS) + +OPTION(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF) + +IF(WITH_METRICS) + MESSAGE(STATUS "With metrics in slam6d.") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS") +ELSE(WITH_METRICS) + MESSAGE(STATUS "Without metrics in slam6d.") +ENDIF(WITH_METRICS) + +IF(WIN32) + SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" ) + include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows) + link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64) + add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp) + SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib") +ELSE(WIN32) + SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" ) + # Add include path for OpenGL without GL/-prefix + # to avoid the include incompatibility between MACOS + # and linux + FIND_PATH(OPENGL_INC gl.h /usr/include/GL) + include_directories(${OPENGL_INC}) +ENDIF(WIN32) + +# Add OpenGL includes for MACOS if needed +# The OSX OpenGL frameworks natively supports freeglut extensions +IF(APPLE) + include_directories(/System/Library/Frameworks/GLUT.framework/Headers) + include_directories(/System/Library/Frameworks/OpenGL.framework/Headers) +ENDIF(APPLE) + +SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}") + +# Hide CMake variables +SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE) +SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE) + + +# Set output directories for libraries and executables +SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib ) +SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj ) +SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin ) + +# Set include and link dirs ... +include_directories(${CMAKE_SOURCE_DIR}/include) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include) +include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src) +link_directories(${CMAKE_SOURCE_DIR}/obj) +link_directories(${CMAKE_SOURCE_DIR}/lib) + +add_subdirectory(3rdparty) +add_subdirectory(src/slam6d) +add_subdirectory(src/scanio) +add_subdirectory(src/scanserver) +add_subdirectory(src/veloslam) +add_subdirectory(src/show) +add_subdirectory(src/grid) +add_subdirectory(src/pmd) +add_subdirectory(src/shapes) +add_subdirectory(src/thermo) +IF(WITH_FBR) + add_subdirectory(src/slam6d/fbr) +ENDIF(WITH_FBR) +add_subdirectory(src/scanner) +add_subdirectory(src/model) + +IF(EXPORT_SHARED_LIBS) +## Compiling a shared library containing all of the project +add_library(slam SHARED src/slam6d/icp6D.cc) +target_link_libraries(slam scanlib_s ANN_s sparse_s newmat_s) +ENDIF(EXPORT_SHARED_LIBS) + +MESSAGE (STATUS "Build environment is set up!") + + +# hack to "circumvent" Debug and Release folders that are created under visual studio +# this is why the INSTALL target has to be used in visual studio +IF(MSVC) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) + + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe") + ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) +ENDIF(MSVC) diff --git a/.svn/pristine/99/991ffadeaeba8b7efaaf67cabfbbe66e943705cf.svn-base b/.svn/pristine/99/991ffadeaeba8b7efaaf67cabfbbe66e943705cf.svn-base new file mode 100644 index 0000000..82418ea --- /dev/null +++ b/.svn/pristine/99/991ffadeaeba8b7efaaf67cabfbbe66e943705cf.svn-base @@ -0,0 +1,391 @@ +/* + * scan_diff implementation + * + * Copyright (C) Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Main program calculating difference of 3D scans. + * + * Program for calculating the difference between scans after matching with slam6d + * Usage: bin/scan_diff -s -e -d 'dir', + * Use -s for the first scan, -e for the second scan + * 'dir' the directory of a set of scans + * The result is a scan in the UOS format (.3d) that contains all points from + * the first scan that do NOT have a corresponding point in the second scan + * within a distance of less than NR units. + * Difference scans will be written to 'dir/diff' + * ATTENTION: All scans between START and END will be loaded! + * @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany. + */ +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#define WANT_STREAM ///< define the WANT stream :) +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::ofstream; +using std::ifstream; +#include +#include + +#include "slam6d/globals.icc" +#include "slam6d/io_utils.h" +#include "slam6d/scan.h" + +#include "scanserver/clientInterface.h" + +#ifdef _OPENMP +#include +#endif + +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#ifdef _MSC_VER + #define strcasecmp _stricmp + #define strncasecmp _strnicmp + #include + #include +#else + #include + #include + #include + #include +#endif + + +/** + * Explains the usage of this program's command line parameters + */ +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 << "-s -e [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << 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 << " -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, rxp,zahn, ply})" << 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 << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR" << endl + << " write all points that have no corresponding point closer than NR 'units'" << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl << endl; + + cout << bold << "EXAMPLES " << normal << endl + << " " << prog << " -m 500 -d 5 dat" << endl + << " " << prog << " --max=5000 -d 10.2 dat" << 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 the directory + * @param start first scan number 'start' + * @param end last scan number 'end' + * @param maxDist - maximal distance of points being loaded + * @param minDist - minimal distance of points being loaded + * @param dist the maximal distance for a point pair + * @param type the scan format + * @param desc true if start is greater than end + * @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 &dist, + IOType &type, bool &desc, bool scanserver) +{ + int c; + // from unistd.h: + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + /* options descriptor */ + // 0: no arguments, 1: required argument, 2: optional argument + static struct option longopts[] = { + { "format", required_argument, 0, 'f' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "dist", required_argument, 0, 'd' }, + { "scanserver", no_argument, 0, 'S' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + cout << endl; + while ((c = getopt_long(argc, argv, "f:d:s:e:m:M:", longopts, NULL)) != -1) + switch (c) + { + case 'd': + dist = atof(optarg); + break; + 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); } + break; + case 'f': + try { + type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case '?': + usage(argv[0]); + return 1; + case 'S': + scanserver = true; + break; + default: + abort (); + } + + if(start < 0 || end < 0 || start == end) { + cerr << "\n*** You need two different scans for difference calculations ***" << endl; + usage(argv[0]); + } + 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 + if(start > end) { + double tmp = start; + start = end; + end = tmp; + desc = true; + } + + parseFormatFile(dir, w_type, w_start, w_end); + + return 0; +} + + +/** + * Main program for calculating the difference of two scans. + * Usage: bin/scan_diff -d -s -e 'dir', + * Use -s and -e for the two scans, + * -d + * and 'dir' the directory of a set of scans + * Difference scans will be written to 'dir/diff/scan[00]s.3d' + * + */ +int main(int argc, char **argv) +{ + + cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl; + + if (argc <= 1) { + usage(argv[0]); + } + + // parsing the command line parameters + // init, default values if not specified + string dir; + double dist = 0; + int start = -1, end = -1; + int maxDist = -1; + int minDist = -1; + IOType type = RIEGL_TXT; + bool desc = false; + bool scanserver = false; + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc, scanserver); + + if (scanserver) { + try { + ClientInterface::create(); + } catch(std::runtime_error& e) { + cerr << "ClientInterface could not be created: " << e.what() << endl; + cerr << "Start the scanserver first." << endl; + exit(-1); + } + } + + string diffdir = dir + "diff"; + +#ifdef _MSC_VER + int success = mkdir(diffdir.c_str()); +#else + int success = mkdir(diffdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << diffdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << diffdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << diffdir << " failed" << endl; + exit(1); + } + string scanFileName; + string framesFileName; + ifstream frames_in; + double inMatrix0[17]; + double inMatrix1[17]; + + cout << "Reading Scan No. " << start << endl; + framesFileName = dir + "scan" + to_string(start,3) + ".frames"; + frames_in.open(framesFileName.c_str()); + + if(!frames_in.good()) { + cerr << "Couldn't read frames " << end << endl; + exit(1); + } + while(frames_in.good()) { + for (unsigned int i = 0; i < 17; frames_in >> inMatrix0[i++]); + } + frames_in.close(); + frames_in.clear(); + for(int i = 0; i < 16; i++) { + cout << inMatrix0[i] << " "; + } + cout << endl; + + cout << "Reading Scan No. " << end << endl; + framesFileName = dir + "scan" + to_string(end,3) + ".frames"; + frames_in.open(framesFileName.c_str()); + + if(!frames_in.good()) { + cerr << "Couldn't read frames " << end << endl; + exit(1); + } + while(frames_in.good()) { + for (unsigned int i = 0; i < 17; frames_in >> inMatrix1[i++]); + } + frames_in.close(); + frames_in.clear(); + for(int i = 0; i < 16; i++) { + cout << inMatrix1[i] << " "; + } + cout << endl; + + 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); + } + + Scan* scan_first = *(Scan::allScans.begin()); + Scan* scan_second = *(Scan::allScans.end()-1); + + scan_first->transform(inMatrix0, Scan::INVALID); + scan_second->transform(inMatrix1, Scan::INVALID); + + DataXYZ xyz_first(scan_first->get("xyz")); + DataXYZ xyz_second(scan_second->get("xyz")); + + cout << "Scan " << scan_first->getIdentifier() << " with " << xyz_first.size() << " points" << endl; + cout << "Scan " << scan_second->getIdentifier() << " with " << xyz_second.size() << " points" << endl; + + int thread_num = 0; + std::vector diff; + double transMat[16]; + + if(desc) { + Scan::getNoPairsSimple(diff, scan_second, scan_first, thread_num, dist); + M4inv(inMatrix1, transMat); + scanFileName = dir + "diff/scan" + to_string(end,3) + ".3d"; + } else { + Scan::getNoPairsSimple(diff, scan_first, scan_second, thread_num, dist); + M4inv(inMatrix0, transMat); + scanFileName = dir + "diff/scan" + to_string(start,3) + ".3d"; + } + + cout << endl; + for(int i = 0; i < 16; i++) { + cout << transMat[i] << " "; + } + cout << endl; + for(int i = 0; i < 16; i++) { + cout << inMatrix0[i] << " "; + } + cout << endl; + + ofstream ptsout(scanFileName.c_str()); + + for(unsigned int i = 0; i < diff.size(); i++) { + Point p(diff[i]); + p.transform(transMat); + ptsout << p.x << " " << p.y << " " << p.z << " " << endl; + } + + ptsout.close(); + ptsout.clear(); + + if (scanserver) { + scan_first->clear("xyz"); + scan_second->clear("xyz"); + } + + cout << endl << endl; + cout << "Normal program end." << endl << endl; + + if (scanserver) { + Scan::closeDirectory(); + } + + return 0; +} diff --git a/.svn/pristine/9a/9a2b77870ce86d532bd74b7def02202e729c5eb1.svn-base b/.svn/pristine/9a/9a2b77870ce86d532bd74b7def02202e729c5eb1.svn-base new file mode 100644 index 0000000..5942bac --- /dev/null +++ b/.svn/pristine/9a/9a2b77870ce86d532bd74b7def02202e729c5eb1.svn-base @@ -0,0 +1,31 @@ +/** + * @file + * @brief Scan types and mapping functions. + * + * @author Thomas Escher, Billy Okal, Dorit Borrmann + */ + +#ifndef IO_TYPES_H +#define IO_TYPES_H + +//! IO types for file formats, distinguishing the use of ScanIOs +enum IOType { + UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, UOS_RRGBT, 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 +enum IODataType { + DATA_XYZ = 1<<0, + DATA_RGB = 1<<1, + DATA_REFLECTANCE = 1<<2, + DATA_TEMPERATURE = 1<<3, + DATA_AMPLITUDE = 1<<4, + DATA_TYPE = 1<<5, + DATA_DEVIATION = 1<<6 +}; + +IOType formatname_to_io_type(const char * string); + +const char * io_type_to_libname(IOType type); + +#endif //IO_TYPES_H diff --git a/.svn/pristine/9e/9ee28ee2356366bb5a56c6e3edf0f26c0d3d9c71.svn-base b/.svn/pristine/9e/9ee28ee2356366bb5a56c6e3edf0f26c0d3d9c71.svn-base new file mode 100644 index 0000000..383b9af --- /dev/null +++ b/.svn/pristine/9e/9ee28ee2356366bb5a56c6e3edf0f26c0d3d9c71.svn-base @@ -0,0 +1,27 @@ +/** + * @file + * @brief IO of a 3D scan in uos file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_UOS_RGB_H__ +#define __SCAN_IO_UOS_RGB_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for UOS scans with color information + * + * The compiled class is available as shared object file + */ +class ScanIO_uos_rgb : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/a2/a2c23051daf890ddd824c51a4fabcbc94be18b76.svn-base b/.svn/pristine/a2/a2c23051daf890ddd824c51a4fabcbc94be18b76.svn-base new file mode 100644 index 0000000..e8f07ad --- /dev/null +++ b/.svn/pristine/a2/a2c23051daf890ddd824c51a4fabcbc94be18b76.svn-base @@ -0,0 +1,287 @@ +/* + * scan_io_rxp implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher + */ + +#include "scanio/scan_io_rxp.h" +#include "riegl/scanlib.hpp" + +#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" + +using namespace scanlib; +using namespace std; +using namespace std::tr1; + + + +#define DATA_PATH_PREFIX "scan" +#define DATA_PATH_SUFFIX ".rxp" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + +/* +TODO: this file is still work in progress for change to the new scanserver workflow +this ScanIO has to distinguish a multi scan file and a directory of single scan files and is currently very messy handling these with the importer class +*/ + + + +std::list ScanIO_rxp::readDirectory(const char* dir_path, unsigned int start, unsigned int end) +{ + std::list identifiers; + + path pose(dir_path); + if(is_regular_file(pose)) { + // TODO: create identifiers for this case + // a) from start to end, requires sanity checks and goodwill of user + // b) iterate through the file and get last index + // c) check last index by other means through the riegl api? + // TEMP: implementing a) without sanity checks :) + for(unsigned int i = start; i < end; ++i) { + identifiers.push_back(to_string(i,3)); + } + } else { + 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_rxp::readPose(const char* dir_path, const char* identifier, double* pose) +{ + unsigned int i; + + path pose_path(dir_path); + + // if the directory actually marks a (multi scan) file, return zero pose + // TODO: test if pose_path gets constructed correctly, see removal of trailing / in the old code + if(is_regular_file(pose_path.string())) { + for(i = 0; i < 6; ++i) pose[i] = 0.0; + return; + } + + 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 + boost::filesystem::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_rxp::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_AMPLITUDE | DATA_DEVIATION | DATA_TYPE)); +} + +void ScanIO_rxp::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) +{ + + path data_path(dir_path); + + // distinguish file and directory + if(is_regular_file(data_path)) { + stringstream str(identifier); + int scan_index; + str >> scan_index; + // first clean up if they "directory" has changed + // second case of resetting: rewinding index in case of non-sequential index access + if(old_path != dir_path || (imp != 0 && imp->getCurrentScan() > scan_index)) { + // TODO: I'm assuming here that I can simply do this + if(rc != 0) { rc->close(); } + if(dec != 0) { delete dec; dec = 0; } + if(imp != 0) { delete imp; imp = 0; } + // TODO ^ + } + // create directory-persistent decoder + if (!dec) { + // remember path + old_path = dir_path; + + // create new connection + rc = basic_rconnection::create(data_path.string()); + rc->open(); + + // decoder splits the binary file into readable chunks + dec = new decoder_rxpmarker(rc); + // importer interprets the chunks + // TODO: this won't really work because filter is gone and xyz too and everything breaks + // probably set new filter and xyz in the importer + imp = new importer(filter, scan_index, xyz, reflectance, amplitude, type, deviation); + } + + // set new filter and vectors + imp->set(filter, xyz, reflectance, amplitude, type, deviation); + + buffer buf; + + // skip the first scans + if(imp->getCurrentScan() < scan_index) { + for(dec->get(buf); !dec->eoi(); dec->get(buf)) { + imp->dispatch(buf.begin(), buf.end()); + if(imp->getCurrentScan() >= scan_index) break; + } + } + if(dec->eoi()) return; + int cscan = imp->getCurrentScan(); + // iterate over chunks, until the next scan is reached + for(dec->get(buf); !dec->eoi(); dec->get(buf)) { + imp->dispatch(buf.begin(), buf.end()); + if(imp->getCurrentScan() != cscan) break; + } + + return; + } + + // error handling + 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) { + string data_path_str; + data_path_str = "file://" + data_path.string(); + rc = basic_rconnection::create(data_path_str); + rc->open(); + + // decoder splits the binary file into readable chunks + dec = new decoder_rxpmarker(rc); + // importer interprets the chunks + imp = new importer(filter, 0, xyz, reflectance, amplitude, type, deviation); + + // iterate over chunks + buffer buf; + for(dec->get(buf); !dec->eoi(); dec->get(buf)) { + imp->dispatch(buf.begin(), buf.end()); + } + + //done + rc->close(); + // TODO: clean up all these pointers, aren't they dangling? + } +} + +void importer::on_echo_transformed(echo_type echo) +{ + // for multi scan files, ignore those before start + if (currentscan < start) return; + + // targets is a member std::vector that contains all + // echoes seen so far, i.e. the current echo is always + // indexed by target_count-1. + target& t(targets[target_count - 1]); + + double point[3]; + point[0] = t.vertex[1]*-100.0; + point[1] = t.vertex[2]*100.0; + point[2] = t.vertex[0]*100.0; + + if(filter->check(point)) { + if(xyz) { + for(unsigned int i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + if(reflectance) reflectance->push_back(t.reflectance); + if(amplitude) amplitude->push_back(t.amplitude); + if(deviation) deviation->push_back(t.deviation); + if(type) { + if(pointcloud::first == echo) type->push_back(0); + else if(pointcloud::interior == echo) type->push_back(1); + else if(pointcloud::last == echo) type->push_back(10); + else if(pointcloud::single == echo) type->push_back(9); + } + } + + /* + // target.reflectance + // target.amplitude + // target.deviation + // target.time + // target.vertex point coordinates + */ +} + + + +/** + * 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_rxp; +} + + +/** + * 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/a5/a5f14802187a6a91366cf4341e365d2606036cf4.svn-base b/.svn/pristine/a5/a5f14802187a6a91366cf4341e365d2606036cf4.svn-base new file mode 100644 index 0000000..24e4772 --- /dev/null +++ b/.svn/pristine/a5/a5f14802187a6a91366cf4341e365d2606036cf4.svn-base @@ -0,0 +1,137 @@ +/** + * @file + * @brief Representation of a 3D point + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#ifndef __POINT_H__ +#define __POINT_H__ + +#include +#include +using std::ostream; +using std::istream; + +#include +using std::runtime_error; + +/** + * @brief Representation of a point in 3D space + */ +class Point { + +public: + /** + * Default constructor + */ + inline Point() { x = y = z = 0.0; point_id = 0; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; + /** + * Copy constructor + */ + inline Point(const Point& p) { x = p.x; y = p.y; z = p.z; type = p.type; point_id = p.point_id; + reflectance = p.reflectance; temperature = p.temperature; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];}; + /** + * Constructor with an array, i.e., vecctor of coordinates + */ + inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; + inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];}; + + /** + * Constructor with three double values + */ + inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; + inline Point(const double _x, const double _y, const double _z, const char _r, + const char _g, const char _b) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;}; + + static inline Point cross(const Point &X, const Point &Y) { + Point res; + res.x = X.y * Y.z - X.z * Y.y; + res.y = X.z * Y.x - X.x * Y.z; + res.z = X.x * Y.y - X.y * Y.x; + return res; + }; + + static inline Point norm(const Point &p) { + double l = sqrt(p.x*p.x + p.y*p.y + p.z*p.z); + Point res(p.x/l, p.y/l, p.z/l); + return res; + }; + + inline Point operator+(const Point &p) const { + Point res; + res.x = x + p.x; + res.y = y + p.y; + res.z = z + p.z; + return res; + }; + + inline Point operator-(const Point &p) const { + Point res; + res.x = x - p.x; + res.y = y - p.y; + res.z = z - p.z; + return res; + }; + + inline Point& operator-=(const Point &p) { + x -= p.x; + y -= p.y; + z -= p.z; + return *this; + }; + inline Point& operator+=(const Point &p) { + x += p.x; + y += p.y; + z += p.z; + return *this; + }; + + + + inline void transform(const double alignxf[16]); + inline double distance(const Point& p); + inline friend ostream& operator<<(ostream& os, const Point& p); + inline friend istream& operator>>(istream& is, Point& p); + + // also public; set/get functions not necessary here + /// x coordinate in 3D space + double x; + /// y coordinate in 3D space + double y; + /// z coordinate in 3D space + double z; + /// additional information about the point, e.g., semantic + /// also used in veloscan for distiuguish moving or static + int type; + + /////////////////////////for veloslam///////////////////////////// + double rad; + /// tang in cylindrical coordinates for veloscan + double tan_theta; + // point id in points for veloscan , you can use it find point. + long point_id; + /////////////////////////for veloslam///////////////////////////// + + // color information of the point between 0 and 255 + // rgb + unsigned char rgb[3]; + + float reflectance; + float temperature; + float amplitude; + float deviation; +}; + + +inline Point operator*(const double &v, const Point &p) { + Point res; + res.x = v * p.x; + res.y = v * p.y; + res.z = v * p.z; + return res; +} + +#include "point.icc" + +#endif diff --git a/.svn/pristine/b1/b1864c308cf24f4ea259589fcff8e3c66aaafc0f.svn-base b/.svn/pristine/b1/b1864c308cf24f4ea259589fcff8e3c66aaafc0f.svn-base new file mode 100644 index 0000000..7864a1f --- /dev/null +++ b/.svn/pristine/b1/b1864c308cf24f4ea259589fcff8e3c66aaafc0f.svn-base @@ -0,0 +1,192 @@ +/* + * scan_io_ks implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_ks.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 "ScanPos" +#define DATA_PATH_SUFFIX " - Scan001.txt" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".pose" + + + +std::list ScanIO_ks::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 (001-999) + std::string identifier(to_string(i,3)); + // scan consists of data (.txt) 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_ks::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(); + + // CAD map -> pose correction [ks x/y/z -> slam -z/y/x] + double tmp; + tmp = pose[0]; + pose[0] = - pose[2]; + pose[2] = tmp; + + // convert coordinate to cm + for(i = 0; i < 3; ++i) pose[i] *= 100.0; + + // 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_ks::supports(IODataType type) +{ + return !!(type & (DATA_XYZ)); +} + +void ScanIO_ks::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 + char dummy[255]; + data_file.getline(dummy, 255); + + // read points + double point[3]; + double tmp; + while(data_file.good()) { + try { + for(i = 0; i < 3; ++i) data_file >> point[i]; + } catch(std::ios_base::failure& e) { + break; + } + + // the enemy's x/y/z is mapped to slam's x/z/y, shuffle time! + tmp = point[1]; + point[1] = point[2]; + point[2] = tmp; + + // TODO: offset is application specific, handle with care + // correct constant offset (in slam coordinates) + point[0] -= 70000.0; // x + point[2] -= 20000.0; // z + + // convert coordinate to cm + for(i = 0; i < 3; ++i) point[i] *= 100.0; + + // apply filter and insert point + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + } + 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_ks; +} + + +/** + * 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/b5/b5117f12dc472b13429f166b092d2a532347f9ac.svn-base b/.svn/pristine/b5/b5117f12dc472b13429f166b092d2a532347f9ac.svn-base new file mode 100644 index 0000000..fbe510f --- /dev/null +++ b/.svn/pristine/b5/b5117f12dc472b13429f166b092d2a532347f9ac.svn-base @@ -0,0 +1,46 @@ +/* + * serverScan implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann + * + * Released under the GPL version 3. + * + */ + +#include "scanserver/serverScan.h" + +#include "scanserver/cache/cacheManager.h" +#include "scanserver/scanHandler.h" +#include "scanserver/temporaryHandler.h" + +ServerScan::ServerScan(const ip::allocator & allocator, + const SharedStringSharedPtr& dir_path_ptr, const char* io_identifier, + IOType iotype, CacheManager* cm) : + SharedScan(allocator, dir_path_ptr, io_identifier, iotype) +{ + // let the manager allocate cache objects in shared memory and assign them process local handlers, called through the interface + m_xyz = cm->createCacheObject(); + m_xyz->setCacheHandler(new ScanHandler(m_xyz.get(), cm, this, DATA_XYZ)); + m_rgb = cm->createCacheObject(); + m_rgb->setCacheHandler(new ScanHandler(m_rgb.get(), cm, this, DATA_RGB)); + m_reflectance = cm->createCacheObject(); + m_reflectance->setCacheHandler(new ScanHandler(m_reflectance.get(), cm, this, DATA_REFLECTANCE)); + m_temperature = cm->createCacheObject(); + m_temperature->setCacheHandler(new ScanHandler(m_temperature.get(), cm, this, DATA_TEMPERATURE)); + m_amplitude = cm->createCacheObject(); + m_amplitude->setCacheHandler(new ScanHandler(m_amplitude.get(), cm, this, DATA_AMPLITUDE)); + m_type = cm->createCacheObject(); + m_type->setCacheHandler(new ScanHandler(m_type.get(), cm, this, DATA_TYPE)); + m_deviation = cm->createCacheObject(); + m_deviation->setCacheHandler(new ScanHandler(m_deviation.get(), cm, this, DATA_DEVIATION)); + + m_xyz_reduced = cm->createCacheObject(); + m_xyz_reduced->setCacheHandler(new TemporaryHandler(m_xyz_reduced.get(), cm, this)); + m_xyz_reduced_original = cm->createCacheObject(); + m_xyz_reduced_original->setCacheHandler(new TemporaryHandler(m_xyz_reduced_original.get(), cm, this, true)); + + m_show_reduced = cm->createCacheObject(); + m_show_reduced->setCacheHandler(new TemporaryHandler(m_show_reduced.get(), cm, this, true)); + m_octtree = cm->createCacheObject(); + m_octtree->setCacheHandler(new TemporaryHandler(m_octtree.get(), cm, this, true)); +} diff --git a/.svn/pristine/b7/b7523d794a1f3568cfd172daf46910f686009fc8.svn-base b/.svn/pristine/b7/b7523d794a1f3568cfd172daf46910f686009fc8.svn-base new file mode 100644 index 0000000..cb6e511 --- /dev/null +++ b/.svn/pristine/b7/b7523d794a1f3568cfd172daf46910f686009fc8.svn-base @@ -0,0 +1,28 @@ +/** + * @file + * @brief IO of a 3D scan in uos file format with reflectance, color and + * temperature information + * @author Dorit Borrmann + */ + +#ifndef __SCAN_IO_UOS_RRGBT_H__ +#define __SCAN_IO_UOS_RRGBT_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for UOS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_uos_rrgbt : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/.svn/pristine/b8/b8bcad940414d7024206bef39c6334a9b786d9ac.svn-base b/.svn/pristine/b8/b8bcad940414d7024206bef39c6334a9b786d9ac.svn-base new file mode 100644 index 0000000..bc8b6e2 --- /dev/null +++ b/.svn/pristine/b8/b8bcad940414d7024206bef39c6334a9b786d9ac.svn-base @@ -0,0 +1,58 @@ +/** + * @file + * @brief + * + * @author Thomas Escher + * @author Dorit Borrmann + */ + +#ifndef SCAN_HANDLER_H +#define SCAN_HANDLER_H + +#include "scanserver/temporaryHandler.h" +#include +#include + + +/** + * @brief CacheHandler for scan files. + * + * This class handles scan files. On a cache miss it reads from the original scan file by calling ScanIO to load the proper library for input. + * If binary scan caching is enabled the TemporaryHandler functionality is invoked in saves and, if binary scan caching is enabled, also on loads. In the latter case the binary cache file has priority over parsing the scan anew. Invalidation is taken into consideration, reloading the scan with new range parameters. + */ +class ScanHandler : public TemporaryHandler +{ +public: + ScanHandler(CacheObject* obj, CacheManager* cm, SharedScan* scan, IODataType data); + + /** + * Reads specific scan data from a file, located by SharedScan's identifiers via ScanIO. + * If binary caching is enabled it will try to read from this resource first to speed up the process. + * @return true, will throw otherwise because we are desperate for scans + * @throw if both the binary cache and the scan resource were unavailable + */ + virtual bool load(); + + /** + * Does nothing unless binary caching is enabled, which will save the contents via CacheIO. + */ + virtual void save(unsigned char* data, unsigned int size); + + //! Enable binary caching of scan data + static void setBinaryCaching(); +private: + IODataType m_data; + + static bool binary_caching; + + //! Cached vectors for prefetching + static std::map* > m_prefetch_xyz; + static std::map* > m_prefetch_rgb; + static std::map* > m_prefetch_reflectance; + static std::map* > m_prefetch_temperature; + static std::map* > m_prefetch_amplitude; + static std::map* > m_prefetch_type; + static std::map* > m_prefetch_deviation; +}; + +#endif //SCAN_HANDLER_H diff --git a/.svn/pristine/cd/cd0b815ad1356c0cbdbffaba48ab1707a8a63fc6.svn-base b/.svn/pristine/cd/cd0b815ad1356c0cbdbffaba48ab1707a8a63fc6.svn-base new file mode 100644 index 0000000..f94f46a --- /dev/null +++ b/.svn/pristine/cd/cd0b815ad1356c0cbdbffaba48ab1707a8a63fc6.svn-base @@ -0,0 +1,32 @@ +/** + * @file + * @brief IO of a 3D scan in velodyne raw file format + * @author Liwei. Computer School of Wuhan University, China. + */ + +#ifndef __SCAN_IO_VELODYNE_H__ +#define __SCAN_IO_VELODYNE_H__ + +#include +using std::string; +#include +using std::vector; + +#include "scan_io.h" + +/** + * @brief 3D scan loader for VELODYNE scans + * + * The compiled class is available as shared object file + */ +class ScanIO_velodyne : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); + + int fileCounter; +}; + +#endif diff --git a/.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base b/.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base new file mode 100644 index 0000000..9b24d38 --- /dev/null +++ b/.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base @@ -0,0 +1,197 @@ +/** + * @file + * @brief Scan containing all interprocess shared data and management values. + * + * @author Thomas Escher + */ + +#ifndef SHARED_SCAN_H +#define SHARED_SCAN_H + +#include "scanserver/frame.h" +#include "slam6d/io_types.h" +#include "slam6d/data_types.h" +#include "slam6d/pointfilter.h" + +#include +#include +#include +#include + +// hide the boost namespace and shorten others +namespace +{ + namespace ip = boost::interprocess; + // the segment manager providing the allocations + typedef ip::managed_shared_memory::segment_manager SegmentManager; +} + +// allocator and type for a scan vector +class SharedScan; +typedef ip::allocator, SegmentManager> SharedScanAllocator; +typedef ip::vector, SharedScanAllocator> SharedScanVector; + +// allocator and type for a shared string +typedef ip::allocator CharAllocator; +typedef ip::basic_string, CharAllocator> SharedString; + +// shared pointer for shared strings +typedef ip::managed_shared_ptr::type SharedStringSharedPtr; + +class CacheObject; + + + +/** + * @brief Central class representing a single scan file in shared memory. + * + * This class identifies a scan file by its path, identificator and io type, which are given to the ScanIO to read a scan. + * It holds all neccessary information like pose, frames and cached data from the scan. The cached data is contained in CacheObjects, one for each data channel and two reduced (one transformed, one untransformed). The access to the CacheObjects' data is given via a derivation from CacheData via MultiArray-types, which imitates a double**-array for easy use. + * All calls to the server are relayed to the ClientInterface and handled there. Access to the CacheObjects causing a cache miss also invoke a server call to the CacheManager. + */ +class SharedScan +{ + // private static class access for onChacheMiss/onAllocation + friend class CacheObject; +public: + //! Constructor by identifiers + SharedScan(const ip::allocator & allocator, + const SharedStringSharedPtr& dir_path_ptr, const char* io_identifier, + IOType iotype); + + //! Deconstructor + ~SharedScan(); + + //! Equality operator based on identifier, directory and type + bool operator==(const SharedScan& r) const; + + //! Filter parameters for range checks when loading from file, invalidate cache for scan CacheObject if it differs + void setRangeParameters(double max_dist, double min_dist); + + //! Filter parameters for height checks when loading from file, invalidate cache for scan CacheObject if it differs + void setHeightParameters(double top, double bottom); + + //! Filter parameters for the range mutator for showing the spheres, invalidate cache for scan CacheObject if it differs + void setRangeMutationParameters(double range); + + //! Set parameters and invalidate cache for reduced CacheObjects if it differs + void setReductionParameters(const char* params); + + //! Set parameters and invalidate cache for reduced CacheObjects if it differs + void setShowReductionParameters(const char* params); + + //! Set parameters and invalidate cache for reduced CacheObjects if it differs + void setOcttreeParameters(const char* params); + + //! Add a new frame with the current transformation and given type + void addFrame(double* transformation, unsigned int type); + + //! Save frames into a file for later use + void saveFrames(); + + //! Clear existing frames + void clearFrames(); + + //! Get contained frames + const FrameVector& getFrames(); + + //! Get pose from pose file + double* getPose(); + + //! Get specific cached data + DataXYZ getXYZ(); + DataRGB getRGB(); + DataReflectance getReflectance(); + DataTemperature getTemperature(); + DataAmplitude getAmplitude(); + DataType getType(); + DataDeviation getDeviation(); + + //! Reduced transformed points + DataXYZ getXYZReduced(); + + //! Create a new set of reduced points + DataXYZ createXYZReduced(unsigned int size); + + //! Reduced untransformed points + DataXYZ getXYZReducedOriginal(); + + //! Create a new set of reduced points originals + DataXYZ createXYZReducedOriginal(unsigned int size); + + //! Individual reduced points to use in show if requested + TripleArray getXYZReducedShow(); + + //! Create a new set of reduced points for use in show + TripleArray createXYZReducedShow(unsigned int size); + + //! Cached tree structure for show + DataPointer getOcttree(); + + //! Create a cached tree structure for show + DataPointer createOcttree(unsigned int size); + + //! ScanHandler related prefetching values to combine loading of separate cache objects + void prefetch(unsigned int type) { m_prefetch |= type; } + + //! Return prefetch values + unsigned int getPrefetch() const { return m_prefetch; } + + //! Clear prefetch values + void clearPrefetch() { m_prefetch = 0; } + + // IO-specific getters + inline const char* getDirPath() const { return m_dir_path_ptr->c_str(); } + inline const char* getIdentifier() const { return m_io_identifier.c_str(); } + inline IOType getIOType() const { return m_iotype; } + inline float getMaxDist() const { return m_max_dist; } + inline float getMinDist() const { return m_min_dist; } + inline double getRangeMutator() const { return m_range_mutator_param; } + inline double getHeightTop() const { return m_height_top; } + inline double getHeightBottom() const { return m_height_bottom; } + + //! Assembles an PointFilter with range/height parameters (if set) to use process-locally + PointFilter getPointFilter() const; + +private: + SharedStringSharedPtr m_dir_path_ptr; + SharedString m_io_identifier; + IOType m_iotype; + unsigned int m_prefetch; + double m_max_dist, m_min_dist; + double m_height_top, m_height_bottom; + double m_range_mutator_param; + bool m_range_mutator_param_set, m_range_param_set, m_height_param_set; + SharedString m_reduction_parameters; + SharedString m_show_parameters; + SharedString m_octtree_parameters; + bool m_load_frames_file; + +protected: + ip::offset_ptr m_pose; + ip::offset_ptr m_xyz, m_rgb, m_reflectance, m_temperature, m_amplitude, m_type, m_deviation, + m_xyz_reduced, m_xyz_reduced_original, + m_show_reduced, m_octtree; + FrameVector m_frames; + + //! invalidate full cache objects + void invalidateFull(); + + //! invalidate reduced cache objects + void invalidateReduced(); + + //! invalidate show related cache objects + void invalidateShow(); + +private: + //! Static callback for cache misses to send a request to the server interface + static void onCacheMiss(CacheObject* obj); + + //! Static callback for cache object creation calls + static void onAllocation(CacheObject* obj, unsigned int size); + + //! Static callback for cache object invalidation + static void onInvalidation(CacheObject* obj); +}; + +#endif //SHARED_SCAN_H diff --git a/.svn/pristine/d4/d47898a6f11b520604ed059c5724d9011b082f1a.svn-base b/.svn/pristine/d4/d47898a6f11b520604ed059c5724d9011b082f1a.svn-base new file mode 100644 index 0000000..022171b --- /dev/null +++ b/.svn/pristine/d4/d47898a6f11b520604ed059c5724d9011b082f1a.svn-base @@ -0,0 +1,212 @@ +/* + * scan_io_riegl_txt implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher + */ + +#include "scanio/scan_io_riegl_txt.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 ".txt" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".dat" + + + +std::list ScanIO_riegl_txt::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_riegl_txt::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()) { + double rPos[3], rPosTheta[16]; + double inMatrix[16], tMatrix[16]; + + for (i = 0; i < 16; ++i) + pose_file >> inMatrix[i]; + pose_file.close(); + + // transform input pose + tMatrix[0] = inMatrix[5]; + tMatrix[1] = -inMatrix[9]; + tMatrix[2] = -inMatrix[1]; + tMatrix[3] = -inMatrix[13]; + tMatrix[4] = -inMatrix[6]; + tMatrix[5] = inMatrix[10]; + tMatrix[6] = inMatrix[2]; + tMatrix[7] = inMatrix[14]; + tMatrix[8] = -inMatrix[4]; + tMatrix[9] = inMatrix[8]; + tMatrix[10] = inMatrix[0]; + tMatrix[11] = inMatrix[12]; + tMatrix[12] = -inMatrix[7]; + tMatrix[13] = inMatrix[11]; + tMatrix[14] = inMatrix[3]; + tMatrix[15] = inMatrix[15]; + + Matrix4ToEuler(tMatrix, rPosTheta, rPos); + + pose[0] = 100*rPos[0]; + pose[1] = 100*rPos[1]; + pose[2] = 100*rPos[2]; + pose[3] = rPosTheta[0]; + pose[4] = rPosTheta[1]; + pose[5] = rPosTheta[2]; + } else { + throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]"); + } +} + +bool ScanIO_riegl_txt::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_REFLECTANCE)); +} + +void ScanIO_riegl_txt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 || reflectance != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // read the point count + unsigned int count; + data_file >> count; + + // reserve enough space for faster reading + xyz->reserve(3*count); + + // read points + // z x y range theta phi reflectance + double point[7]; + double tmp; + while(data_file.good()) { + try { + for(i = 0; i < 7; ++i) data_file >> point[i]; + } catch(std::ios_base::failure& e) { + break; + } + + // the enemy's x/y/z is mapped to slam's z/x/y, shuffle time! + // invert x axis + // convert coordinate to cm + tmp = point[2]; + point[2] = 100.0 * point[0]; + point[0] = -100.0 * point[1]; + point[1] = 100.0 * tmp; + + // apply filter and insert point + if(filter.check(point)) { + if(xyz != 0) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + if(reflectance != 0) { + reflectance->push_back(point[6]); + } + } + } + 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_riegl_txt; +} + + +/** + * 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/d5/d5ed1a9c335f39e399358cbea8a7dd3d4fa78c04.svn-base b/.svn/pristine/d5/d5ed1a9c335f39e399358cbea8a7dd3d4fa78c04.svn-base new file mode 100644 index 0000000..dd2e5b1 --- /dev/null +++ b/.svn/pristine/d5/d5ed1a9c335f39e399358cbea8a7dd3d4fa78c04.svn-base @@ -0,0 +1,30 @@ +/** + * @file + * @brief IO of a 3D scan in rts file format + * @author Thomas Escher + */ + +#ifndef __SCAN_IO_RTS_H__ +#define __SCAN_IO_RTS_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for RTS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_rts : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +private: + std::string cached_dir; + std::vector cached_poses; +}; + +#endif diff --git a/.svn/pristine/dd/ddefa9bff7fd57daa456850e1bb62d848dd5dd14.svn-base b/.svn/pristine/dd/ddefa9bff7fd57daa456850e1bb62d848dd5dd14.svn-base new file mode 100644 index 0000000..4b97c8c --- /dev/null +++ b/.svn/pristine/dd/ddefa9bff7fd57daa456850e1bb62d848dd5dd14.svn-base @@ -0,0 +1,220 @@ +/* + * scan_io_riegl_rgb implementation + * + * Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher + */ + +#include "scanio/scan_io_riegl_rgb.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 ".rgb" +#define POSE_PATH_PREFIX "scan" +#define POSE_PATH_SUFFIX ".dat" + + + +std::list ScanIO_riegl_rgb::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_riegl_rgb::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()) { + double rPos[3], rPosTheta[16]; + double inMatrix[16], tMatrix[16]; + + for (i = 0; i < 16; ++i) + pose_file >> inMatrix[i]; + pose_file.close(); + + // transform input pose + tMatrix[0] = inMatrix[5]; + tMatrix[1] = -inMatrix[9]; + tMatrix[2] = -inMatrix[1]; + tMatrix[3] = -inMatrix[13]; + tMatrix[4] = -inMatrix[6]; + tMatrix[5] = inMatrix[10]; + tMatrix[6] = inMatrix[2]; + tMatrix[7] = inMatrix[14]; + tMatrix[8] = -inMatrix[4]; + tMatrix[9] = inMatrix[8]; + tMatrix[10] = inMatrix[0]; + tMatrix[11] = inMatrix[12]; + tMatrix[12] = -inMatrix[7]; + tMatrix[13] = inMatrix[11]; + tMatrix[14] = inMatrix[3]; + tMatrix[15] = inMatrix[15]; + + Matrix4ToEuler(tMatrix, rPosTheta, rPos); + + pose[0] = 100*rPos[0]; + pose[1] = 100*rPos[1]; + pose[2] = 100*rPos[2]; + pose[3] = rPosTheta[0]; + pose[4] = rPosTheta[1]; + pose[5] = rPosTheta[2]; + } else { + throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]"); + } +} + +bool ScanIO_riegl_rgb::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_RGB | DATA_REFLECTANCE)); +} + +void ScanIO_riegl_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 || rgb != 0 || reflectance != 0) { + // open data file + ifstream data_file(data_path); + data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); + + // read the point count + unsigned int count; + data_file >> count; + + // reserve enough space for faster reading + if(xyz != 0) xyz->reserve(3*count); + if(rgb != 0) rgb->reserve(3*count); + + // read points + // z x y range theta phi r g b reflectance + double point[7]; + unsigned int color[3]; + double tmp; + while(data_file.good()) { + try { + for(i = 0; i < 6; ++i) data_file >> point[i]; + for(i = 0; i < 3; ++i) data_file >> color[i]; + data_file >> point[6]; + } catch(std::ios_base::failure& e) { + break; + } + + // the enemy's x/y/z is mapped to slam's z/x/y, shuffle time! + // invert x axis + // convert coordinate to cm + tmp = point[2]; + point[2] = 100.0 * point[0]; + point[0] = -100.0 * point[1]; + point[1] = 100.0 * tmp; + + // apply filter and insert point + if(filter.check(point)) { + if(xyz != 0) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + } + if(rgb != 0) { + for(i = 0; i < 3; ++i) rgb->push_back( + static_cast(color[i])); + } + if(reflectance != 0) { + reflectance->push_back(point[6]); + } + } + } + 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_riegl_rgb; +} + + +/** + * 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/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base b/.svn/pristine/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base new file mode 100644 index 0000000..a41cd2d --- /dev/null +++ b/.svn/pristine/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base @@ -0,0 +1,376 @@ +/* + * managedScan implementation + * + * Copyright (C) Kai Lingemann, Thomas Escher + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/managedScan.h" + +#include "scanserver/clientInterface.h" +#include "slam6d/Boctree.h" +#include "slam6d/kdManaged.h" + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif //WITH_METRICS + +#include +using std::stringstream; + +#include +using namespace boost::filesystem; + + + +SharedScanVector* ManagedScan::shared_scans = 0; + + + +void ManagedScan::openDirectory(const std::string& path, IOType type, int start, int end) +{ + // start the client first + try { + ClientInterface::create(); + } catch(std::runtime_error& e) { + cerr << "ClientInterface could not be created: " << e.what() << endl; + cerr << "Start the scanserver first." << endl; + exit(-1); + } + +#ifdef WITH_METRICS + Timer t = ClientMetric::read_scan_time.start(); +#endif //WITH_METRICS + + ClientInterface* client = ClientInterface::getInstance(); + shared_scans = client->readDirectory(path.c_str(), type, start, end); + + for(SharedScanVector::iterator it = shared_scans->begin(); it != shared_scans->end(); ++it) { + // add a scan with reference on the shared scan + SharedScan* shared = it->get(); + ManagedScan* scan = new ManagedScan(shared); + Scan::allScans.push_back(scan); + } + +#ifdef WITH_METRICS + ClientMetric::read_scan_time.end(t); +#endif //WITH_METRICS +} + +void ManagedScan::closeDirectory() +{ + // clean up the scan vector + for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) + delete *it; + allScans.clear(); + // remove the shared scan vector + ClientInterface* client = ClientInterface::getInstance(); +#ifdef WITH_METRICS + ClientInterface::getInstance()->printMetrics(); +#endif //WITH_METRICS + client->closeDirectory(shared_scans); +} + +std::size_t ManagedScan::getMemorySize() +{ + ClientInterface* client = ClientInterface::getInstance(); + return client->getCacheSize(); +} + + + +ManagedScan::ManagedScan(SharedScan* shared_scan) : + m_shared_scan(shared_scan), + m_reduced_ready(false), + m_reset_frames_on_write(true) +{ + // request pose from the shared scan + double* euler = m_shared_scan->getPose(); + rPos[0] = euler[0]; + rPos[1] = euler[1]; + rPos[2] = euler[2]; + rPosTheta[0] = euler[3]; + rPosTheta[1] = euler[4]; + rPosTheta[2] = euler[5]; + + // write original pose matrix + EulerToMatrix4(euler, &euler[3], transMatOrg); + + // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead + transformMatrix(transMatOrg); + + // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one + M4identity(dalignxf); +} + +ManagedScan::~ManagedScan() +{ + // TODO: something to do? +} + +void ManagedScan::setRangeFilter(double max, double min) +{ + m_shared_scan->setRangeParameters(max, min); +} + +void ManagedScan::setHeightFilter(double top, double bottom) +{ + m_shared_scan->setHeightParameters(top, bottom); +} + +void ManagedScan::setRangeMutation(double range) +{ + m_shared_scan->setRangeMutationParameters(range); +} + + +void ManagedScan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype) +{ + Scan::setReductionParameter(voxelSize, nrpts, pointtype); + + // set parameters to invalidate old cache data + stringstream s; + s << voxelSize << " " << nrpts << " " << transMatOrg; + m_shared_scan->setReductionParameters(s.str().c_str()); +} + +void ManagedScan::setShowReductionParameter(double voxelSize, int nrpts, PointType pointtype) +{ + show_reduction_voxelSize = voxelSize; + show_reduction_nrpts = nrpts; + show_reduction_pointtype = pointtype; + + // set parameters to invalidate old cache data + stringstream s; + s << voxelSize << " " << nrpts; + m_shared_scan->setShowReductionParameters(s.str().c_str()); +} + +void ManagedScan::setOcttreeParameter(double reduction_voxelSize, double octtree_voxelSize, PointType pointtype, bool loadOct, bool saveOct) +{ + Scan::setOcttreeParameter(reduction_voxelSize, octtree_voxelSize, pointtype, loadOct, saveOct); + + // set octtree parameters to invalidate cached ones with other parameters (changing range/height is already handled) + stringstream s; + s << reduction_voxelSize << " " << octtree_voxelSize << " " << pointtype.toFlags(); + m_shared_scan->setOcttreeParameters(s.str().c_str()); +} + +DataPointer ManagedScan::get(const std::string& identifier) +{ + if(identifier == "xyz") { + return m_shared_scan->getXYZ(); + } else + if(identifier == "rgb") { + return m_shared_scan->getRGB(); + } else + if(identifier == "reflectance") { + return m_shared_scan->getReflectance(); + } else + if(identifier == "temperature") { + return m_shared_scan->getTemperature(); + } else + if(identifier == "amplitude") { + return m_shared_scan->getAmplitude(); + } else + if(identifier == "type") { + return m_shared_scan->getType(); + } else + if(identifier == "deviation") { + return m_shared_scan->getDeviation(); + } else + if(identifier == "xyz reduced") { + // if this is a fresh run, initialize reduced properly via original or creating it anew + if(!m_reduced_ready) { + calcReducedOnDemand(); + } + return m_shared_scan->getXYZReduced(); + } else + if(identifier == "xyz reduced original") { + // if reduction has completed, original will exist (either from last run or created in this run) + if(!m_reduced_ready) { + calcReducedOnDemand(); + } + return m_shared_scan->getXYZReducedOriginal(); + } else + if(identifier == "xyz reduced show") { + if(m_shared_scan->getXYZReducedShow().valid()) + return m_shared_scan->getXYZReducedShow(); + calcReducedShow(); + return m_shared_scan->getXYZReducedShow(); + } else + if(identifier == "octtree") { + if(m_shared_scan->getOcttree().valid()) + return m_shared_scan->getOcttree(); + createOcttree(); + return m_shared_scan->getOcttree(); + } + { + throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::get. Upgrade SharedScan for this data field."); + } +} + +void ManagedScan::get(unsigned int types) +{ + m_shared_scan->prefetch(types); +} + +DataPointer ManagedScan::create(const std::string& identifier, unsigned int size) +{ + // map identifiers to functions in SharedScan and scale back size from bytes to number of points + if(identifier == "xyz reduced") { + return m_shared_scan->createXYZReduced(size / (3*sizeof(double))); + } else + if(identifier == "xyz reduced original") { + return m_shared_scan->createXYZReducedOriginal(size / (3*sizeof(double))); + } else + { + throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::create. Upgrade SharedScan for this data field."); + } +} + +void ManagedScan::clear(const std::string& identifier) +{ + // nothing to do here + // TODO: mark CacheObjects with a low priority for faster removal by the manager +} + +void ManagedScan::createSearchTreePrivate() +{ + switch(searchtree_nnstype) + { + case simpleKD: + kd = new KDtreeManaged(this); + break; + case BOCTree: + kd = new BOctTree(PointerArray(get("xyz reduced original")).get(), size("xyz reduced original"), 10.0, PointType(), true); + break; + case -1: + throw runtime_error("Cannot create a SearchTree without setting a type."); + default: + throw runtime_error("SearchTree type not implemented for ManagedScan"); + } + + // TODO: look into CUDA compability +} + +void ManagedScan::calcReducedOnDemandPrivate() +{ + // either copy from original or create them like BasicScan + DataXYZ xyz_orig(m_shared_scan->getXYZReducedOriginal()); + if(xyz_orig.valid()) { + // set true to inform further get("xyz reduced original") calls to get the data instead of looping calcReducedOnDemand + m_reduced_ready = true; + copyOriginalToReduced(); + } else { + // create reduced points and transform to initial position, save a copy of this for SearchTree + calcReducedPoints(); + // set true to inform further get("xyz reduced") calls to get the data instead of looping calcReducedOnDemand + m_reduced_ready = true; + transformReduced(transMatOrg); + copyReducedToOriginal(); + } +} + +void ManagedScan::calcReducedShow() +{ + // create an octtree reduction from full points + DataXYZ xyz(get("xyz")); + BOctTree* oct = new BOctTree(PointerArray(xyz).get(), xyz.size(), show_reduction_voxelSize); + + vector center; + center.clear(); + + if(show_reduction_nrpts > 0) { + if(show_reduction_nrpts == 1) { + oct->GetOctTreeRandom(center); + } else { + oct->GetOctTreeRandom(center, show_reduction_nrpts); + } + } else { + oct->GetOctTreeCenter(center); + } + + unsigned int size = center.size(); + TripleArray xyz_r(m_shared_scan->createXYZReducedShow(size)); + for(unsigned int i = 0; i < size; ++i) { + for(unsigned int j = 0; j < 3; ++j) { + xyz_r[i][j] = center[i][j]; + } + } + + delete oct; +} + +void ManagedScan::createOcttree() +{ + string scanFileName = string(m_shared_scan->getDirPath()) + "scan" + getIdentifier() + ".oct"; + BOctTree* btree = 0; + + // if loadOct is given, load the octtree under blind assumption that parameters match + if(octtree_loadOct && exists(scanFileName)) { + btree = new BOctTree(scanFileName); + } else { + if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points + TripleArray xyz_r(get("xyz reduced show")); + btree = new BOctTree(PointerArray(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true); + } else { // without reduction, xyz + attribute points + float** pts = octtree_pointtype.createPointArray(this); + unsigned int nrpts = size("xyz"); + btree = new BOctTree(pts, nrpts, octtree_voxelSize, octtree_pointtype, true); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + } + // save created octtree + if(octtree_saveOct) { + cout << "Saving octree " << scanFileName << endl; + btree->serialize(scanFileName); + } + } + + // copy tree into cache + try { + unsigned int size = btree->getMemorySize(); + unsigned char* mem_ptr = m_shared_scan->createOcttree(size).get_raw_pointer(); + new(mem_ptr) BOctTree(*btree, mem_ptr, size); + delete btree; btree = 0; + } catch(runtime_error& e) { + // delete tree if copy to cache failed + delete btree; + throw e; + } +} + + +unsigned int ManagedScan::readFrames() +{ + // automatically read on getFrames + return m_shared_scan->getFrames().size(); +} + +void ManagedScan::saveFrames() +{ + m_shared_scan->saveFrames(); +} + +unsigned int ManagedScan::getFrameCount() +{ + return m_shared_scan->getFrames().size(); +} + +void ManagedScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) +{ + const Frame& frame(m_shared_scan->getFrames().at(i)); + pose_matrix = frame.transformation; + type = static_cast(frame.type); +} + +void ManagedScan::addFrame(AlgoType type) +{ + if(m_reset_frames_on_write) { + m_shared_scan->clearFrames(); + m_reset_frames_on_write = false; + } + m_shared_scan->addFrame(transMat, static_cast(type)); +} diff --git a/.svn/pristine/e9/e997c5ebf8c9d5197819fb044063ad31d33d4580.svn-base b/.svn/pristine/e9/e997c5ebf8c9d5197819fb044063ad31d33d4580.svn-base new file mode 100644 index 0000000..ebb4877 --- /dev/null +++ b/.svn/pristine/e9/e997c5ebf8c9d5197819fb044063ad31d33d4580.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/pristine/fb/fb1dec70f3bde4e42b7ec439ec8d54720f0658cc.svn-base b/.svn/pristine/fb/fb1dec70f3bde4e42b7ec439ec8d54720f0658cc.svn-base new file mode 100644 index 0000000..1145467 --- /dev/null +++ b/.svn/pristine/fb/fb1dec70f3bde4e42b7ec439ec8d54720f0658cc.svn-base @@ -0,0 +1,177 @@ +/* + * scan_io_uos_rrgbt implementation + * + * Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Dorit Borrmann. School of Engineering and Science, Jacobs University * Bremen, Germany. + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_uos_rrgbt.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_uos_rrgbt::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_uos_rrgbt::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_uos_rrgbt::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB | DATA_TEMPERATURE)); +} + +void ScanIO_uos_rrgbt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 && rgb != 0 && reflectance != 0 && temperature != 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 + double point[4]; + unsigned int color[3]; + double temp; + while(data_file.good()) { + try { + for(i = 0; i < 4; ++i) data_file >> point[i]; + for(i = 0; i < 3; ++i) data_file >> color[i]; + data_file >> temp; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter and insert point + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + reflectance->push_back(point[3]); + for(i = 0; i < 3; ++i) rgb->push_back( + static_cast(color[i])); + temperature->push_back(temp); + } + } + 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_uos_rrgbt; +} + + +/** + * 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/wc.db b/.svn/wc.db index 9cc516d..c584bf1 100644 Binary files a/.svn/wc.db and b/.svn/wc.db differ diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ccfadb..35aed9d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,25 +166,6 @@ ENDIF(WITH_TOOLS) -## Scan reduction -OPTION(WITH_SCAN_REDUCTION "Whether to build the scan reduction binary scan_red ON/OFF" OFF) - -IF(WITH_SCAN_REDUCTION) - MESSAGE(STATUS "With scan_red") -ELSE(WITH_SCAN_REDUCTION) - MESSAGE(STATUS "Without scan_red") -ENDIF(WITH_SCAN_REDUCTION) - -## Scan difference -OPTION(WITH_SCAN_DIFF "Whether to build the scan_diff binary ON/OFF" OFF) - -IF(WITH_SCAN_DIFF) - MESSAGE(STATUS "With scan_diff") -ELSE(WITH_SCAN_DIFF) - MESSAGE(STATUS "Without scan_diff") -ENDIF(WITH_SCAN_DIFF) - - ## CAD matching OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF) diff --git a/include/scanio/scan_io.h b/include/scanio/scan_io.h index 1d06b39..ff4fa84 100644 --- a/include/scanio/scan_io.h +++ b/include/scanio/scan_io.h @@ -53,7 +53,7 @@ public: * @param identifier IO-specific identifier for the particular scan * @param filter Filter object which each point is tested on by its position */ - virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz = 0, std::vector* rgb = 0, std::vector* reflectance = 0, std::vector* amplitude = 0, std::vector* type = 0, std::vector* deviation = 0) = 0; + virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz = 0, std::vector* rgb = 0, std::vector* reflectance = 0, std::vector* temperature = 0, std::vector* amplitude = 0, std::vector* type = 0, std::vector* deviation = 0) = 0; /** * Returns whether this ScanIO can load the requested data from a scan. diff --git a/include/scanio/scan_io_ks.h b/include/scanio/scan_io_ks.h index 5316afa..03e5674 100644 --- a/include/scanio/scan_io_ks.h +++ b/include/scanio/scan_io_ks.h @@ -20,7 +20,7 @@ class ScanIO_ks : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_ks_rgb.h b/include/scanio/scan_io_ks_rgb.h index 8fe69ef..8223e43 100644 --- a/include/scanio/scan_io_ks_rgb.h +++ b/include/scanio/scan_io_ks_rgb.h @@ -20,7 +20,7 @@ class ScanIO_ks_rgb : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_riegl_rgb.h b/include/scanio/scan_io_riegl_rgb.h index 4cd5dbd..a828cd2 100644 --- a/include/scanio/scan_io_riegl_rgb.h +++ b/include/scanio/scan_io_riegl_rgb.h @@ -1,6 +1,6 @@ /** * @file - * @brief IO of a 3D scan in uos file format + * @brief IO of a 3D scan in riegl_txt file format with added color information * @author Thomas Escher */ @@ -20,7 +20,7 @@ class ScanIO_riegl_rgb : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_riegl_txt.h b/include/scanio/scan_io_riegl_txt.h index 6fbe67e..2cff39c 100644 --- a/include/scanio/scan_io_riegl_txt.h +++ b/include/scanio/scan_io_riegl_txt.h @@ -1,6 +1,6 @@ /** * @file - * @brief IO of a 3D scan in uos file format + * @brief IO of a 3D scan in riegl_txt file format * @author Thomas Escher */ @@ -20,7 +20,7 @@ class ScanIO_riegl_txt : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_rts.h b/include/scanio/scan_io_rts.h index e64d813..dd2e5b1 100644 --- a/include/scanio/scan_io_rts.h +++ b/include/scanio/scan_io_rts.h @@ -4,8 +4,8 @@ * @author Thomas Escher */ -#ifndef __SCAN_IO_UOS_H__ -#define __SCAN_IO_UOS_H__ +#ifndef __SCAN_IO_RTS_H__ +#define __SCAN_IO_RTS_H__ #include "scan_io.h" @@ -20,7 +20,7 @@ class ScanIO_rts : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); private: std::string cached_dir; diff --git a/include/scanio/scan_io_rxp.h b/include/scanio/scan_io_rxp.h index 156461f..d815fb1 100644 --- a/include/scanio/scan_io_rxp.h +++ b/include/scanio/scan_io_rxp.h @@ -28,7 +28,7 @@ class ScanIO_rxp : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); ScanIO_rxp() : dec(0), imp(0) {} diff --git a/include/scanio/scan_io_uos.h b/include/scanio/scan_io_uos.h index fbb5424..a38145b 100644 --- a/include/scanio/scan_io_uos.h +++ b/include/scanio/scan_io_uos.h @@ -20,7 +20,7 @@ class ScanIO_uos : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_uos_rgb.h b/include/scanio/scan_io_uos_rgb.h index 0bb004a..383b9af 100644 --- a/include/scanio/scan_io_uos_rgb.h +++ b/include/scanio/scan_io_uos_rgb.h @@ -20,7 +20,7 @@ class ScanIO_uos_rgb : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_uos_rrgbt.h b/include/scanio/scan_io_uos_rrgbt.h new file mode 100644 index 0000000..cb6e511 --- /dev/null +++ b/include/scanio/scan_io_uos_rrgbt.h @@ -0,0 +1,28 @@ +/** + * @file + * @brief IO of a 3D scan in uos file format with reflectance, color and + * temperature information + * @author Dorit Borrmann + */ + +#ifndef __SCAN_IO_UOS_RRGBT_H__ +#define __SCAN_IO_UOS_RRGBT_H__ + +#include "scan_io.h" + + + +/** + * @brief 3D scan loader for UOS scans + * + * The compiled class is available as shared object file + */ +class ScanIO_uos_rrgbt : 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* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); + virtual bool supports(IODataType type); +}; + +#endif diff --git a/include/scanio/scan_io_uosr.h b/include/scanio/scan_io_uosr.h index 462b788..1fa3917 100644 --- a/include/scanio/scan_io_uosr.h +++ b/include/scanio/scan_io_uosr.h @@ -22,7 +22,7 @@ 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); }; diff --git a/include/scanio/scan_io_velodyne.h b/include/scanio/scan_io_velodyne.h index 3b41b0c..f94f46a 100644 --- a/include/scanio/scan_io_velodyne.h +++ b/include/scanio/scan_io_velodyne.h @@ -23,7 +23,7 @@ class ScanIO_velodyne : 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 void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation); virtual bool supports(IODataType type); int fileCounter; diff --git a/include/scanserver/multiArray.h b/include/scanserver/multiArray.h index cb9556c..b365a4e 100644 --- a/include/scanserver/multiArray.h +++ b/include/scanserver/multiArray.h @@ -64,6 +64,7 @@ public: typedef TripleArray DataXYZ; typedef TripleArray DataRGB; typedef SingleArray DataReflectance; +typedef SingleArray DataTemperature; typedef SingleArray DataAmplitude; typedef SingleArray DataType; typedef SingleArray DataDeviation; diff --git a/include/scanserver/scanHandler.h b/include/scanserver/scanHandler.h index 47847d4..bc8b6e2 100644 --- a/include/scanserver/scanHandler.h +++ b/include/scanserver/scanHandler.h @@ -3,6 +3,7 @@ * @brief * * @author Thomas Escher + * @author Dorit Borrmann */ #ifndef SCAN_HANDLER_H @@ -48,6 +49,7 @@ private: static std::map* > m_prefetch_xyz; static std::map* > m_prefetch_rgb; static std::map* > m_prefetch_reflectance; + static std::map* > m_prefetch_temperature; static std::map* > m_prefetch_amplitude; static std::map* > m_prefetch_type; static std::map* > m_prefetch_deviation; diff --git a/include/scanserver/sharedScan.h b/include/scanserver/sharedScan.h index 1e5f744..9b24d38 100644 --- a/include/scanserver/sharedScan.h +++ b/include/scanserver/sharedScan.h @@ -102,6 +102,7 @@ public: DataXYZ getXYZ(); DataRGB getRGB(); DataReflectance getReflectance(); + DataTemperature getTemperature(); DataAmplitude getAmplitude(); DataType getType(); DataDeviation getDeviation(); @@ -168,7 +169,7 @@ private: protected: ip::offset_ptr m_pose; - ip::offset_ptr m_xyz, m_rgb, m_reflectance, m_amplitude, m_type, m_deviation, + ip::offset_ptr m_xyz, m_rgb, m_reflectance, m_temperature, m_amplitude, m_type, m_deviation, m_xyz_reduced, m_xyz_reduced_original, m_show_reduced, m_octtree; FrameVector m_frames; diff --git a/include/show/colormanager.h b/include/show/colormanager.h index 035c2cb..e8c97a3 100644 --- a/include/show/colormanager.h +++ b/include/show/colormanager.h @@ -21,7 +21,8 @@ class ColorMap { HSV = 2, JET = 3, HOT = 4, - SHSV = 5 + SHSV = 5, + TEMP = 6 }; virtual ~ColorMap() {}; @@ -96,6 +97,34 @@ class HotMap : public ColorMap { } }; +class TempMap : public ColorMap { + public: + virtual void calcColor(float *d, unsigned int i, unsigned int buckets) { + float t = 1.0 - (float)i/(float)buckets; + + if(t <= 1.0/5.0) { + d[1] = d[2] = 0.0; + d[0] = t/(1.0/5.0); + } else if(t <=2.0/5.0) { + d[0] = 1.0; + d[1] = (t-(1.0/5.0))/(1.0/5.0); + d[2] = 0.0; + } else if(t <=3.0/5.0) { + d[0] = 1.0 - ((t-(2.0/5.0))/(1.0/5.0)); + d[1] = 1.0; + d[2] = (t-(2.0/5.0))/(1.0/5.0); + } else if(t <=4.0/5.0) { + d[0] = 0.0; + d[1] = 1.0 - ((t-(3.0/5.0))/(1.0/5.0)); + d[2] = 1.0; + } else { + d[0] = 0.0; + d[1] = 0.0; + d[2] = 1.0 - ((t-(4.0/5.0))/(1.3/5.0)); + } + } +}; + class DiffMap : public ColorMap { public: virtual void calcColor(float *d, unsigned int i, unsigned int buckets); diff --git a/include/slam6d/data_types.h b/include/slam6d/data_types.h index b13ec1f..3625728 100644 --- a/include/slam6d/data_types.h +++ b/include/slam6d/data_types.h @@ -218,6 +218,7 @@ typedef TripleArray DataXYZ; typedef TripleArray DataXYZFloat; typedef TripleArray DataRGB; typedef SingleArray DataReflectance; +typedef SingleArray DataTemperature; typedef SingleArray DataAmplitude; typedef SingleArray DataType; typedef SingleArray DataDeviation; diff --git a/include/slam6d/fbr/fbr_global.h b/include/slam6d/fbr/fbr_global.h index 4fc7e85..b9193ac 100644 --- a/include/slam6d/fbr/fbr_global.h +++ b/include/slam6d/fbr/fbr_global.h @@ -36,7 +36,7 @@ namespace fbr{ PANNINI, STEREOGRAPHIC, ZAXIS, - CONIC, + CONIC }; /** * @enum panorama_map_method diff --git a/include/slam6d/fbr/panorama.h b/include/slam6d/fbr/panorama.h index 0591988..f58f936 100644 --- a/include/slam6d/fbr/panorama.h +++ b/include/slam6d/fbr/panorama.h @@ -59,6 +59,13 @@ namespace fbr{ * @brief creates the panorama reflectance image and map. */ void createPanorama(cv::Mat scan); + /** + * @brief recovers the point cloud from the panorama image and range information + * @param image - input range image to be converted to point cloud + * @param file - destination of .3d file containing the point cloud + */ + void recoverPointCloud(const cv::Mat& range_image, const std::string& file); + unsigned int getImageWidth(); unsigned int getImageHeight(); projection_method getProjectionMethod(); diff --git a/include/slam6d/io_types.h b/include/slam6d/io_types.h index dc95bb1..5942bac 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, Billy Okal + * @author Thomas Escher, Billy Okal, Dorit Borrmann */ #ifndef IO_TYPES_H @@ -10,7 +10,7 @@ //! IO types for file formats, distinguishing the use of ScanIOs enum IOType { - 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 + UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, UOS_RRGBT, 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 @@ -18,9 +18,10 @@ enum IODataType { DATA_XYZ = 1<<0, DATA_RGB = 1<<1, DATA_REFLECTANCE = 1<<2, - DATA_AMPLITUDE = 1<<3, - DATA_TYPE = 1<<4, - DATA_DEVIATION = 1<<5 + DATA_TEMPERATURE = 1<<3, + DATA_AMPLITUDE = 1<<4, + DATA_TYPE = 1<<5, + DATA_DEVIATION = 1<<6 }; IOType formatname_to_io_type(const char * string); diff --git a/include/slam6d/point.h b/include/slam6d/point.h index 41b247b..24e4772 100644 --- a/include/slam6d/point.h +++ b/include/slam6d/point.h @@ -25,24 +25,24 @@ public: /** * Default constructor */ - inline Point() { x = y = z = 0.0; point_id = 0; type = 0; reflectance = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; + inline Point() { x = y = z = 0.0; point_id = 0; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; /** * Copy constructor */ inline Point(const Point& p) { x = p.x; y = p.y; z = p.z; type = p.type; point_id = p.point_id; - reflectance = p.reflectance; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];}; + reflectance = p.reflectance; temperature = p.temperature; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];}; /** * Constructor with an array, i.e., vecctor of coordinates */ - inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; amplitude = 0.0; deviation = 0.0; - rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; - inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];}; + inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; + inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];}; /** * Constructor with three double values */ - inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; }; - inline Point(const double _x, const double _y, const double _z, const char _r, const char _g, const char _b) { x = _x; y = _y; z = _z; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;}; + inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;}; + inline Point(const double _x, const double _y, const double _z, const char _r, + const char _g, const char _b) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;}; static inline Point cross(const Point &X, const Point &Y) { Point res; @@ -118,6 +118,7 @@ public: unsigned char rgb[3]; float reflectance; + float temperature; float amplitude; float deviation; }; diff --git a/include/slam6d/point_type.h b/include/slam6d/point_type.h index 036c110..5c0fd82 100644 --- a/include/slam6d/point_type.h +++ b/include/slam6d/point_type.h @@ -30,6 +30,7 @@ public: static const unsigned int USE_NONE; static const unsigned int USE_REFLECTANCE; + static const unsigned int USE_TEMPERATURE; static const unsigned int USE_AMPLITUDE; static const unsigned int USE_DEVIATION; static const unsigned int USE_HEIGHT; @@ -43,6 +44,7 @@ public: PointType(unsigned int _types); bool hasReflectance(); + bool hasTemperature(); bool hasAmplitude(); bool hasDeviation(); bool hasType(); @@ -51,6 +53,7 @@ public: bool hasIndex(); unsigned int getReflectance(); + unsigned int getTemperature(); unsigned int getAmplitude(); unsigned int getDeviation(); unsigned int getTime(); @@ -104,7 +107,7 @@ private: /** * Derived from types, to map type to the array index for each point **/ - int dimensionmap[8]; + int dimensionmap[9]; bool hasType(unsigned int type); @@ -113,6 +116,7 @@ private: DataXYZ* m_xyz; DataRGB* m_rgb; DataReflectance* m_reflectance; + DataTemperature* m_temperature; DataAmplitude* m_amplitude; DataType* m_type; DataDeviation* m_deviation; @@ -130,6 +134,9 @@ T *PointType::createPoint(const Point &P, unsigned int index ) { if (types & USE_REFLECTANCE) { p[counter++] = P.reflectance; } + if (types & USE_TEMPERATURE) { + p[counter++] = P.temperature; + } if (types & USE_AMPLITUDE) { p[counter++] = P.amplitude; } @@ -164,6 +171,9 @@ Point PointType::createPoint(T *p) { if (types & USE_REFLECTANCE) { P.reflectance = p[counter++]; } + if (types & USE_TEMPERATURE) { + P.temperature = p[counter++]; + } if (types & USE_AMPLITUDE) { P.amplitude = p[counter++]; } @@ -196,6 +206,9 @@ T *PointType::createPoint(unsigned int i, unsigned int index) { if (types & USE_REFLECTANCE) { p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0); } + if (types & USE_TEMPERATURE) { + p[counter++] = (m_temperature? (*m_temperature)[i]: 0); + } if (types & USE_AMPLITUDE) { p[counter++] = (m_amplitude? (*m_amplitude)[i]: 0); } diff --git a/src/scanio/CMakeLists.txt b/src/scanio/CMakeLists.txt index 6a897c4..6e15f8c 100644 --- a/src/scanio/CMakeLists.txt +++ b/src/scanio/CMakeLists.txt @@ -5,7 +5,7 @@ else(WIN32) endif(WIN32) set(SCANIO_LIBNAMES - uos uosr uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne + uos uosr uos_rgb uos_rrgbt ks ks_rgb riegl_txt riegl_rgb rts velodyne ) if(WITH_RIVLIB) diff --git a/src/scanio/scan_io_ks.cc b/src/scanio/scan_io_ks.cc index 5871add..7864a1f 100644 --- a/src/scanio/scan_io_ks.cc +++ b/src/scanio/scan_io_ks.cc @@ -102,7 +102,7 @@ bool ScanIO_ks::supports(IODataType type) return !!(type & (DATA_XYZ)); } -void ScanIO_ks::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) +void ScanIO_ks::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; diff --git a/src/scanio/scan_io_ks_rgb.cc b/src/scanio/scan_io_ks_rgb.cc index 4b96439..8f98beb 100644 --- a/src/scanio/scan_io_ks_rgb.cc +++ b/src/scanio/scan_io_ks_rgb.cc @@ -104,7 +104,7 @@ bool ScanIO_ks_rgb::supports(IODataType type) return !!(type & (DATA_XYZ | DATA_RGB | DATA_REFLECTANCE | DATA_AMPLITUDE)); } -void ScanIO_ks_rgb::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) +void ScanIO_ks_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; diff --git a/src/scanio/scan_io_riegl_rgb.cc b/src/scanio/scan_io_riegl_rgb.cc index d45cda3..4b97c8c 100644 --- a/src/scanio/scan_io_riegl_rgb.cc +++ b/src/scanio/scan_io_riegl_rgb.cc @@ -118,7 +118,7 @@ bool ScanIO_riegl_rgb::supports(IODataType type) return !!(type & (DATA_XYZ | DATA_RGB | DATA_REFLECTANCE)); } -void ScanIO_riegl_rgb::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) +void ScanIO_riegl_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; diff --git a/src/scanio/scan_io_riegl_txt.cc b/src/scanio/scan_io_riegl_txt.cc index 0a9eb9d..022171b 100644 --- a/src/scanio/scan_io_riegl_txt.cc +++ b/src/scanio/scan_io_riegl_txt.cc @@ -118,7 +118,7 @@ bool ScanIO_riegl_txt::supports(IODataType type) return !!(type & (DATA_XYZ | DATA_REFLECTANCE)); } -void ScanIO_riegl_txt::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) +void ScanIO_riegl_txt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; diff --git a/src/scanio/scan_io_rts.cc b/src/scanio/scan_io_rts.cc index f2b1e95..cc52c38 100644 --- a/src/scanio/scan_io_rts.cc +++ b/src/scanio/scan_io_rts.cc @@ -125,7 +125,7 @@ bool ScanIO_rts::supports(IODataType type) return !!(type & (DATA_XYZ)); } -void ScanIO_rts::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) +void ScanIO_rts::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { // TODO: Type and other columns? unsigned int i; diff --git a/src/scanio/scan_io_rxp.cc b/src/scanio/scan_io_rxp.cc index 0f84079..e8f07ad 100644 --- a/src/scanio/scan_io_rxp.cc +++ b/src/scanio/scan_io_rxp.cc @@ -122,7 +122,7 @@ bool ScanIO_rxp::supports(IODataType type) return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_AMPLITUDE | DATA_DEVIATION | DATA_TYPE)); } -void ScanIO_rxp::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) +void ScanIO_rxp::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { path data_path(dir_path); diff --git a/src/scanio/scan_io_uos.cc b/src/scanio/scan_io_uos.cc index 049aefb..8978441 100644 --- a/src/scanio/scan_io_uos.cc +++ b/src/scanio/scan_io_uos.cc @@ -92,7 +92,7 @@ bool ScanIO_uos::supports(IODataType type) return !!(type & (DATA_XYZ)); } -void ScanIO_uos::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) +void ScanIO_uos::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; diff --git a/src/scanio/scan_io_uos_rgb.cc b/src/scanio/scan_io_uos_rgb.cc index b209cb5..263d15c 100644 --- a/src/scanio/scan_io_uos_rgb.cc +++ b/src/scanio/scan_io_uos_rgb.cc @@ -91,7 +91,7 @@ bool ScanIO_uos_rgb::supports(IODataType type) return !!(type & (DATA_XYZ | DATA_RGB)); } -void ScanIO_uos_rgb::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) +void ScanIO_uos_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; @@ -101,7 +101,7 @@ void ScanIO_uos_rgb::readScan(const char* dir_path, const char* identifier, Poin if(!exists(data_path)) throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]"); - if(xyz != 0 || rgb != 0) { + if(xyz != 0 && rgb != 0) { // open data file ifstream data_file(data_path); data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); diff --git a/src/scanio/scan_io_uos_rrgbt.cc b/src/scanio/scan_io_uos_rrgbt.cc new file mode 100644 index 0000000..1145467 --- /dev/null +++ b/src/scanio/scan_io_uos_rrgbt.cc @@ -0,0 +1,177 @@ +/* + * scan_io_uos_rrgbt implementation + * + * Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter + * + * Released under the GPL version 3. + * + */ + + +/** + * @file + * @brief Implementation of reading 3D scans + * @author Dorit Borrmann. School of Engineering and Science, Jacobs University * Bremen, Germany. + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#include "scanio/scan_io_uos_rrgbt.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_uos_rrgbt::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_uos_rrgbt::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_uos_rrgbt::supports(IODataType type) +{ + return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB | DATA_TEMPERATURE)); +} + +void ScanIO_uos_rrgbt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, 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 && rgb != 0 && reflectance != 0 && temperature != 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 + double point[4]; + unsigned int color[3]; + double temp; + while(data_file.good()) { + try { + for(i = 0; i < 4; ++i) data_file >> point[i]; + for(i = 0; i < 3; ++i) data_file >> color[i]; + data_file >> temp; + } catch(std::ios_base::failure& e) { + break; + } + + // apply filter and insert point + if(filter.check(point)) { + for(i = 0; i < 3; ++i) xyz->push_back(point[i]); + reflectance->push_back(point[3]); + for(i = 0; i < 3; ++i) rgb->push_back( + static_cast(color[i])); + temperature->push_back(temp); + } + } + 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_uos_rrgbt; +} + + +/** + * 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/scanio/scan_io_uosr.cc b/src/scanio/scan_io_uosr.cc index 088c27a..a86e317 100644 --- a/src/scanio/scan_io_uosr.cc +++ b/src/scanio/scan_io_uosr.cc @@ -91,7 +91,7 @@ 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) +void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) { unsigned int i; diff --git a/src/scanio/scan_io_velodyne.cc b/src/scanio/scan_io_velodyne.cc index 9202c0f..6bf4c63 100644 --- a/src/scanio/scan_io_velodyne.cc +++ b/src/scanio/scan_io_velodyne.cc @@ -515,6 +515,7 @@ void ScanIO_velodyne::readScan( std::vector* xyz, std::vector* rgb, std::vector* reflectance, + std::vector* temperature, std::vector* amplitude, std::vector* type, std::vector* deviation) diff --git a/src/scanserver/CMakeLists.txt b/src/scanserver/CMakeLists.txt index 9a15b60..ebb4877 100644 --- a/src/scanserver/CMakeLists.txt +++ b/src/scanserver/CMakeLists.txt @@ -18,7 +18,7 @@ set(CLIENT_LIBS ${Boost_LIBRARIES} pointfilter) if(UNIX AND NOT APPLE) # boost::interprocess uses pthread, requiring librt - set(CLIENT_LIBS ${CLIENT_LIBS} rt) + #set(CLIENT_LIBS ${CLIENT_LIBS} rt) endif(UNIX AND NOT APPLE) target_link_libraries(scanclient ${CLIENT_LIBS}) @@ -42,7 +42,7 @@ set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio) if(UNIX) # boost::interprocess uses pthread, requiring librt - set(SERVER_LIBS ${SERVER_LIBS} rt) + #set(SERVER_LIBS ${SERVER_LIBS} rt) endif(UNIX) if(WIN32) diff --git a/src/scanserver/scanHandler.cc b/src/scanserver/scanHandler.cc index 99e5bb9..db8b480 100644 --- a/src/scanserver/scanHandler.cc +++ b/src/scanserver/scanHandler.cc @@ -30,6 +30,7 @@ bool ScanHandler::binary_caching = false; std::map* > ScanHandler::m_prefetch_xyz; std::map* > ScanHandler::m_prefetch_rgb; std::map* > ScanHandler::m_prefetch_reflectance; +std::map* > ScanHandler::m_prefetch_temperature; std::map* > ScanHandler::m_prefetch_amplitude; std::map* > ScanHandler::m_prefetch_type; std::map* > ScanHandler::m_prefetch_deviation; @@ -158,6 +159,7 @@ bool ScanHandler::load() PrefetchVector xyz(m_scan, m_prefetch_xyz); PrefetchVector rgb(m_scan, m_prefetch_rgb); PrefetchVector reflectance(m_scan, m_prefetch_reflectance); + PrefetchVector temperature(m_scan, m_prefetch_temperature); PrefetchVector amplitude(m_scan, m_prefetch_amplitude); PrefetchVector type(m_scan, m_prefetch_type); PrefetchVector deviation(m_scan, m_prefetch_deviation); @@ -167,6 +169,7 @@ bool ScanHandler::load() if(m_data == DATA_XYZ) vec = &xyz; else if(m_data == DATA_RGB) vec = &rgb; else if(m_data == DATA_REFLECTANCE) vec = &reflectance; + else if(m_data == DATA_TEMPERATURE) vec = &temperature; else if(m_data == DATA_AMPLITUDE) vec = &litude; else if(m_data == DATA_TYPE) vec = &type; else if(m_data == DATA_DEVIATION) vec = &deviation; @@ -187,6 +190,7 @@ bool ScanHandler::load() if(prefetch & DATA_XYZ) xyz.create(); if(prefetch & DATA_RGB) rgb.create(); if(prefetch & DATA_REFLECTANCE) reflectance.create(); + if(prefetch & DATA_TEMPERATURE) temperature.create(); if(prefetch & DATA_AMPLITUDE) amplitude.create(); if(prefetch & DATA_TYPE) type.create(); if(prefetch & DATA_DEVIATION) deviation.create(); @@ -196,7 +200,7 @@ bool ScanHandler::load() PointFilter filter(m_scan->getPointFilter()); sio->readScan(m_scan->getDirPath(), m_scan->getIdentifier(), filter, - xyz.get(), rgb.get(), reflectance.get(), amplitude.get(), type.get(), deviation.get()); + xyz.get(), rgb.get(), reflectance.get(), temperature.get(), amplitude.get(), type.get(), deviation.get()); } catch(std::runtime_error& e) { // INFO // cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanIO runtime_error: " << e.what() << endl; @@ -229,6 +233,7 @@ bool ScanHandler::load() xyz.save(); rgb.save(); reflectance.save(); + temperature.save(); amplitude.save(); type.save(); deviation.save(); diff --git a/src/scanserver/serverScan.cc b/src/scanserver/serverScan.cc index dfd4762..fbe510f 100644 --- a/src/scanserver/serverScan.cc +++ b/src/scanserver/serverScan.cc @@ -25,6 +25,8 @@ ServerScan::ServerScan(const ip::allocator & allocator, m_rgb->setCacheHandler(new ScanHandler(m_rgb.get(), cm, this, DATA_RGB)); m_reflectance = cm->createCacheObject(); m_reflectance->setCacheHandler(new ScanHandler(m_reflectance.get(), cm, this, DATA_REFLECTANCE)); + m_temperature = cm->createCacheObject(); + m_temperature->setCacheHandler(new ScanHandler(m_temperature.get(), cm, this, DATA_TEMPERATURE)); m_amplitude = cm->createCacheObject(); m_amplitude->setCacheHandler(new ScanHandler(m_amplitude.get(), cm, this, DATA_AMPLITUDE)); m_type = cm->createCacheObject(); diff --git a/src/scanserver/sharedScan.cc b/src/scanserver/sharedScan.cc index 1405e13..93ede82 100644 --- a/src/scanserver/sharedScan.cc +++ b/src/scanserver/sharedScan.cc @@ -213,6 +213,10 @@ DataReflectance SharedScan::getReflectance() { return m_reflectance->getCacheData(); } +DataTemperature SharedScan::getTemperature() { + return m_temperature->getCacheData(); +} + DataAmplitude SharedScan::getAmplitude() { return m_amplitude->getCacheData(); } diff --git a/src/show/show_common.cc b/src/show/show_common.cc index 271e9f9..3b45148 100644 --- a/src/show/show_common.cc +++ b/src/show/show_common.cc @@ -379,6 +379,10 @@ void usage(char* prog) << " use reflectivity values for coloring point clouds" << endl << " only works when using octree display" << endl << endl + << bold << " -D, --temperature, --degree" << normal << endl + << " use temperature 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 @@ -457,6 +461,8 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD { "octree", optional_argument, 0, 'O' }, { "reflectance", no_argument, 0, 'R' }, { "reflectivity", no_argument, 0, 'R' }, + { "temperature", no_argument, 0, 'D' }, + { "degree", no_argument, 0, 'D' }, { "amplitude", no_argument, 0, 'a' }, { "deviation", no_argument, 0, 'd' }, { "height", no_argument, 0, 'h' }, @@ -471,7 +477,7 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD { 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:SwtRadhTcb", longopts, NULL)) != -1) { + while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRDadhTcb", longopts, NULL)) != -1) { switch (c) { case 's': w_start = atoi(optarg); @@ -515,6 +521,9 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD case 'R': types |= PointType::USE_REFLECTANCE; break; + case 'D': + types |= PointType::USE_TEMPERATURE; + break; case 'a': types |= PointType::USE_AMPLITUDE; break; diff --git a/src/show/show_gl.cc b/src/show/show_gl.cc index 2e6c53c..9b5bafd 100644 --- a/src/show/show_gl.cc +++ b/src/show/show_gl.cc @@ -1905,15 +1905,18 @@ void mapColorToValue(int dummy) { cm->setCurrentType(PointType::USE_REFLECTANCE); break; case 2: - cm->setCurrentType(PointType::USE_AMPLITUDE); + cm->setCurrentType(PointType::USE_TEMPERATURE); break; case 3: - cm->setCurrentType(PointType::USE_DEVIATION); + cm->setCurrentType(PointType::USE_AMPLITUDE); break; case 4: - cm->setCurrentType(PointType::USE_TYPE); + cm->setCurrentType(PointType::USE_DEVIATION); break; case 5: + cm->setCurrentType(PointType::USE_TYPE); + break; + case 6: cm->setCurrentType(PointType::USE_COLOR); break; default: @@ -1930,6 +1933,7 @@ void changeColorMap(int dummy) { JetMap jm; HotMap hot; DiffMap diff; + TempMap temp; switch (listboxColorMapVal) { case 0: @@ -1954,6 +1958,9 @@ void changeColorMap(int dummy) { case 6: cm->setColorMap(shsv); break; + case 7: + cm->setColorMap(temp); + break; default: break; } diff --git a/src/show/show_menu.cc b/src/show/show_menu.cc index 62c70a6..19112b6 100644 --- a/src/show/show_menu.cc +++ b/src/show/show_menu.cc @@ -259,6 +259,7 @@ void newMenu() GLUI_RadioGroup *color_rog = glui1->add_radiogroup_to_panel( color_ro, &listboxColorVal, 0, &mapColorToValue ); glui1->add_radiobutton_to_group( color_rog, "height"); GLUI_RadioButton *rbrefl = glui1->add_radiobutton_to_group( color_rog, "reflectance"); + GLUI_RadioButton *rbtemp = glui1->add_radiobutton_to_group( color_rog, "temperature"); GLUI_RadioButton *rbampl = glui1->add_radiobutton_to_group( color_rog, "amplitude"); GLUI_RadioButton *rbdevi = glui1->add_radiobutton_to_group( color_rog, "deviation"); GLUI_RadioButton *rbtype = glui1->add_radiobutton_to_group( color_rog, "type"); @@ -267,6 +268,7 @@ void newMenu() //if (!(types & PointType::USE_DEVIATION)) rbdevi->disable(); //if (!(types & PointType::USE_TYPE)) rbtype->disable(); if (!(pointtype.hasReflectance())) rbrefl->disable(); + if (!(pointtype.hasTemperature())) rbtemp->disable(); if (!(pointtype.hasAmplitude())) rbampl->disable(); if (!(pointtype.hasDeviation())) rbdevi->disable(); if (!(pointtype.hasType())) rbtype->disable(); @@ -282,6 +284,7 @@ void newMenu() glui1->add_radiobutton_to_group(colorm_rog, "Hot"); glui1->add_radiobutton_to_group(colorm_rog, "Rand"); glui1->add_radiobutton_to_group(colorm_rog, "SHSV"); + glui1->add_radiobutton_to_group(colorm_rog, "TEMP"); GLUI_Panel *scans_color = glui1->add_rollout_to_panel(color_panel, "Color type:", false); scans_color->set_alignment(GLUI_ALIGN_LEFT); diff --git a/src/show/wxshow.cc b/src/show/wxshow.cc index ee0dc5c..70e6bb4 100644 --- a/src/show/wxshow.cc +++ b/src/show/wxshow.cc @@ -21,6 +21,7 @@ class SelectionImpl : public Selection { SelectionImpl( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Selection"), bool advanced_controls = false, const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( -1,-1 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL ) : Selection(parent, id, title, pos, size, style, advanced_controls) { if (pointtype.hasReflectance()) m_choice11->Append(wxT("reflectance")); + if (pointtype.hasTemperature()) m_choice11->Append(wxT("temperature")); if (pointtype.hasAmplitude()) m_choice11->Append(wxT("amplitude")); if (pointtype.hasDeviation()) m_choice11->Append(wxT("deviation")); if (pointtype.hasType()) m_choice11->Append(wxT("type")); @@ -89,16 +90,21 @@ class SelectionImpl : public Selection { break; } case 2: + if (pointtype.hasTemperature()) { + cm->setCurrentType(PointType::USE_TEMPERATURE); + break; + } + case 3: if (pointtype.hasAmplitude()) { cm->setCurrentType(PointType::USE_AMPLITUDE); break; } - case 3: + case 4: if (pointtype.hasDeviation()) { cm->setCurrentType(PointType::USE_DEVIATION); break; } - case 4: + case 5: if (pointtype.hasType()) { cm->setCurrentType(PointType::USE_TYPE); break; diff --git a/src/slam6d/CMakeLists.txt b/src/slam6d/CMakeLists.txt index cad527e..7469d0c 100644 --- a/src/slam6d/CMakeLists.txt +++ b/src/slam6d/CMakeLists.txt @@ -1,36 +1,32 @@ -### SCAN_RED +### TOOLS -IF(WITH_SCAN_REDUCTION) - add_executable(scan_red scan_red.cc) +IF(WITH_TOOLS) + FIND_PACKAGE(OpenCV REQUIRED) + ### SCAN_RED + add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc) IF(UNIX) - target_link_libraries(scan_red scan dl ANN) + target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS}) ENDIF(UNIX) IF (WIN32) target_link_libraries(scan_red scan ANN XGetopt) ENDIF(WIN32) -ENDIF(WITH_SCAN_REDUCTION) -### SCAN_DIFF -IF(WITH_SCAN_DIFF) + ### SCAN_DIFF add_executable(scan_diff scan_diff.cc) - add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc) + # add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc) IF(UNIX) target_link_libraries(scan_diff scan dl ANN) - target_link_libraries(scan_diff2d scan dl ANN) + # target_link_libraries(scan_diff2d scan dl ANN) ENDIF(UNIX) IF (WIN32) target_link_libraries(scan_diff scan ANN XGetopt) - target_link_libraries(scan_diff2d scan ANN XGetopt) + # target_link_libraries(scan_diff2d scan ANN XGetopt) ENDIF(WIN32) -ENDIF(WITH_SCAN_DIFF) -### TOOLS - -IF(WITH_TOOLS) add_executable(frame_to_graph frame_to_graph.cc) add_executable(convergence convergence.cc) add_executable(graph_balancer graph_balancer.cc) diff --git a/src/slam6d/basicScan.cc b/src/slam6d/basicScan.cc index b94b316..56179aa 100644 --- a/src/slam6d/basicScan.cc +++ b/src/slam6d/basicScan.cc @@ -137,6 +137,7 @@ void BasicScan::get(unsigned int types) vector xyz; vector rgb; vector reflectance; + vector temperature; vector amplitude; vector type; vector deviation; @@ -155,6 +156,7 @@ void BasicScan::get(unsigned int types) &xyz, &rgb, &reflectance, + &temperature, &litude, &type, &deviation); @@ -172,6 +174,10 @@ void BasicScan::get(unsigned int types) float* data = reinterpret_cast(create("reflectance", sizeof(float) * reflectance.size()).get_raw_pointer()); for(unsigned int i = 0; i < reflectance.size(); ++i) data[i] = reflectance[i]; } + if(types & DATA_TEMPERATURE && !temperature.empty()) { + float* data = reinterpret_cast(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer()); + for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i]; + } if(types & DATA_AMPLITUDE && !amplitude.empty()) { int* data = reinterpret_cast(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer()); for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i]; @@ -197,6 +203,7 @@ DataPointer BasicScan::get(const std::string& identifier) if(identifier == "xyz") get(DATA_XYZ); else if(identifier == "rgb") get(DATA_RGB); else if(identifier == "reflectance") get(DATA_REFLECTANCE); else + if(identifier == "temperature") get(DATA_TEMPERATURE); else if(identifier == "amplitude") get(DATA_AMPLITUDE); else if(identifier == "type") get(DATA_TYPE); else if(identifier == "deviation") get(DATA_DEVIATION); else diff --git a/src/slam6d/fbr/CMakeLists.txt b/src/slam6d/fbr/CMakeLists.txt index aabf033..03e0264 100644 --- a/src/slam6d/fbr/CMakeLists.txt +++ b/src/slam6d/fbr/CMakeLists.txt @@ -20,4 +20,4 @@ SET(FBR_LIBS scan ANN ${OpenCV_LIBS}) add_executable(featurebasedregistration feature_based_registration.cc fbr_global.cc) target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS}) -ENDIF(WITH_FBR) \ No newline at end of file +ENDIF(WITH_FBR) diff --git a/src/slam6d/fbr/panorama.cc b/src/slam6d/fbr/panorama.cc index fc947fb..6dbf896 100644 --- a/src/slam6d/fbr/panorama.cc +++ b/src/slam6d/fbr/panorama.cc @@ -554,6 +554,155 @@ namespace fbr{ } } + void panorama::recoverPointCloud(const cv::Mat& range_image, const string& file ) { + std::ofstream scan_file (file.c_str()); + //recover from EQUIRECTANGULAR projection + if(pMethod == EQUIRECTANGULAR) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + //int widthMax = range_image.size().width - 1; + double yFactor = (double) range_image.size().height / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI); + double heightLow = (0 - MIN_ANGLE) / 360 * 2 * M_PI; + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float theta = (heightMax - row) / yFactor - heightLow; + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + //recover from CYLINDRICAL projection + if(pMethod == CYLINDRICAL) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + //int widthMax = range_image.size().width - 1; + double yFactor = (double) range_image.size().height / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI)); + double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI; + //int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float theta = atan2(row + yFactor * tan(heightLow), yFactor); + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + //recover from MERCATOR projection + if(pMethod == MERCATOR) { + double xFactor = (double) range_image.size().width / 2 / M_PI; + double yFactor = (double) range_image.size().height / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) ); + double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI))); + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float theta = 2 * atan2(exp((heightMax - row) / yFactor + heightLow), 1.) - M_PI_2; + float phi = col / xFactor; + phi *= 180.0 / M_PI; + phi = 180.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + //recover from CONIC projection + if(pMethod == CONIC) { + // set up maximum latitude and longitude angles of the robot + double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0, + MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI; + // set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html + double Lat0 = 0., Long0 = 0.; + double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0; + double n = (sin(Phi1) + sin(Phi2)) / 2.; + double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1); + double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n; + // set up max values for x and y and add the longitude to x axis and latitude to y axis + double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0)); + double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0)); + double xFactor = (double) range_image.size().width / ( xmax - xmin ); + double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 )); + double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 )); + double yFactor = (double) range_image.size().height / ( ymax - ymin ); + int heightMax = range_image.size().height - 1; + + bool first_seen = true; + for (int row = 0; row < range_image.size().height; ++row) { + for (int col = 0; col < range_image.size().width; ++col) { + float range = range_image.at(row, col); + float x = col * 1. / xFactor - fabs(xmin); + float y = (heightMax - row) * 1. / yFactor - fabs(ymin); + float theta = asin((C - (x*x + (Rho0 - y) * (Rho0 - y)) * n * n) / (2 * n)); + float phi = Long0 + (1./n) * atan2(x, Rho0 - y); + + phi *= 180.0 / M_PI; + phi = 360.0 - phi; + phi *= M_PI / 180.0; + theta *= 180.0 / M_PI; + theta *= -1; + theta += 90.0; + theta *= M_PI / 180.0; + + double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.}; + toKartesian(polar, cartesian); + //if ( std::isnan(cartesian[0]) || std::isnan(cartesian[1]) || std::isnan(cartesian[2]) ) continue; + if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) { + if (first_seen) first_seen = false; + else continue; + } + scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl; + } + } + } + + scan_file.close(); + } + unsigned int panorama::getImageWidth(){ return iWidth; } @@ -595,8 +744,11 @@ namespace fbr{ } void panorama::getDescription(){ - cout<<"panorama created with width: "<getReflectance(); } else + if(identifier == "temperature") { + return m_shared_scan->getTemperature(); + } else if(identifier == "amplitude") { return m_shared_scan->getAmplitude(); } else diff --git a/src/slam6d/point_type.cc b/src/slam6d/point_type.cc index e659c19..893841f 100644 --- a/src/slam6d/point_type.cc +++ b/src/slam6d/point_type.cc @@ -1,7 +1,7 @@ /* * point_type implementation * - * Copyright (C) Jan Elseberg + * Copyright (C) Jan Elseberg, Dorit Borrmann. * * Released under the GPL version 3. * @@ -36,26 +36,31 @@ PointType::PointType() { types = USE_NONE; pointdim = 3; dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = 1; // choose height per default + dimensionmap[8] = 1; dimensionmap[0] = 1; // height } PointType::PointType(unsigned int _types) : types(_types) { - dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = 1; // choose height per default + dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = dimensionmap[8] = 1; // choose height per default dimensionmap[0] = 1; // height pointdim = 3; if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++; - if (types & PointType::USE_AMPLITUDE) dimensionmap[2] = pointdim++; - if (types & PointType::USE_DEVIATION) dimensionmap[3] = pointdim++; - if (types & PointType::USE_TYPE) dimensionmap[4] = pointdim++; - if (types & PointType::USE_COLOR) dimensionmap[5] = pointdim++; - if (types & PointType::USE_TIME) dimensionmap[6] = pointdim++; - if (types & PointType::USE_INDEX) dimensionmap[7] = pointdim++; + if (types & PointType::USE_TEMPERATURE) dimensionmap[2] = pointdim++; + if (types & PointType::USE_AMPLITUDE) dimensionmap[3] = pointdim++; + if (types & PointType::USE_DEVIATION) dimensionmap[4] = pointdim++; + if (types & PointType::USE_TYPE) dimensionmap[5] = pointdim++; + if (types & PointType::USE_COLOR) dimensionmap[6] = pointdim++; + if (types & PointType::USE_TIME) dimensionmap[7] = pointdim++; + if (types & PointType::USE_INDEX) dimensionmap[8] = pointdim++; } bool PointType::hasReflectance() { return hasType(USE_REFLECTANCE); } +bool PointType::hasTemperature() { + return hasType(USE_TEMPERATURE); +} bool PointType::hasAmplitude() { return hasType(USE_AMPLITUDE); } @@ -80,24 +85,28 @@ unsigned int PointType::getReflectance() { return dimensionmap[1]; } -unsigned int PointType::getAmplitude() { +unsigned int PointType::getTemperature() { return dimensionmap[2]; } -unsigned int PointType::getDeviation() { +unsigned int PointType::getAmplitude() { return dimensionmap[3]; } -unsigned int PointType::getTime() { - return dimensionmap[6]; +unsigned int PointType::getDeviation() { + return dimensionmap[4]; } -unsigned int PointType::getIndex() { +unsigned int PointType::getTime() { return dimensionmap[7]; } +unsigned int PointType::getIndex() { + return dimensionmap[8]; +} + unsigned int PointType::getType() { - return dimensionmap[4]; + return dimensionmap[5]; } unsigned int PointType::getType(unsigned int type) { @@ -107,16 +116,18 @@ unsigned int PointType::getType(unsigned int type) { return dimensionmap[0]; } else if (type == USE_REFLECTANCE) { return dimensionmap[1]; - } else if (type == USE_AMPLITUDE) { + } else if (type == USE_TEMPERATURE) { return dimensionmap[2]; - } else if (type == USE_DEVIATION) { + } else if (type == USE_AMPLITUDE) { return dimensionmap[3]; - } else if (type == USE_TYPE) { + } else if (type == USE_DEVIATION) { return dimensionmap[4]; - } else if (type == USE_COLOR) { + } else if (type == USE_TYPE) { return dimensionmap[5]; - } else if (type == USE_TIME) { + } else if (type == USE_COLOR) { return dimensionmap[6]; + } else if (type == USE_TIME) { + return dimensionmap[7]; } else { return 0; } @@ -144,24 +155,26 @@ bool PointType::hasType(unsigned int type) { const unsigned int PointType::USE_NONE = 0; const unsigned int PointType::USE_REFLECTANCE = 1; -const unsigned int PointType::USE_AMPLITUDE = 2; -const unsigned int PointType::USE_DEVIATION = 4; -const unsigned int PointType::USE_HEIGHT = 8; -const unsigned int PointType::USE_TYPE = 16; -const unsigned int PointType::USE_COLOR = 32; -const unsigned int PointType::USE_TIME = 64; -const unsigned int PointType::USE_INDEX = 128; +const unsigned int PointType::USE_TEMPERATURE = 2; +const unsigned int PointType::USE_AMPLITUDE = 4; +const unsigned int PointType::USE_DEVIATION = 8; +const unsigned int PointType::USE_HEIGHT = 16; +const unsigned int PointType::USE_TYPE = 32; +const unsigned int PointType::USE_COLOR = 64; +const unsigned int PointType::USE_TIME = 128; +const unsigned int PointType::USE_INDEX = 256; void PointType::useScan(Scan* scan) { // clear pointers first - m_xyz = 0; m_rgb = 0; m_reflectance = 0; m_amplitude = 0; m_type = 0; m_deviation = 0; + m_xyz = 0; m_rgb = 0; m_reflectance = 0; m_temperature = 0; m_amplitude = 0; m_type = 0; m_deviation = 0; // collectively load data to avoid unneccessary loading times due to split get("") calls unsigned int types = DATA_XYZ; if(hasColor()) types |= DATA_RGB; if(hasReflectance()) types |= DATA_REFLECTANCE; + if(hasTemperature()) types |= DATA_TEMPERATURE; if(hasAmplitude()) types |= DATA_AMPLITUDE; if(hasType()) types |= DATA_TYPE; if(hasDeviation()) types |= DATA_DEVIATION; @@ -172,6 +185,7 @@ void PointType::useScan(Scan* scan) m_xyz = new DataXYZ(scan->get("xyz")); if(hasColor()) m_rgb = new DataRGB(scan->get("rgb")); if(hasReflectance()) m_reflectance = new DataReflectance(scan->get("reflectance")); + if(hasTemperature()) m_temperature = new DataTemperature(scan->get("temperature")); if(hasAmplitude()) m_amplitude = new DataAmplitude(scan->get("amplitude")); if(hasType()) m_type = new DataType(scan->get("type")); if(hasDeviation()) m_deviation = new DataDeviation(scan->get("deviation")); @@ -179,6 +193,7 @@ void PointType::useScan(Scan* scan) // check if data is available, otherwise reset pointer to indicate that the scan doesn't prove this value if(m_rgb && !m_rgb->valid()) { delete m_rgb; m_rgb = 0; } if(m_reflectance && !m_reflectance->valid()) { delete m_reflectance; m_reflectance = 0; } + if(m_temperature && !m_temperature->valid()) { delete m_temperature; m_temperature = 0; } if(m_amplitude && !m_amplitude->valid()) { delete m_amplitude; m_amplitude = 0; } if(m_type && !m_type->valid()) { delete m_type; m_type = 0; } if(m_deviation && !m_deviation->valid()) { delete m_deviation; m_deviation = 0; } @@ -195,6 +210,7 @@ void PointType::clearScan() if(m_xyz) delete m_xyz; if(hasColor() && m_rgb) delete m_rgb; if(hasReflectance() && m_reflectance) delete m_reflectance; + if(hasTemperature() && m_temperature) delete m_temperature; if(hasAmplitude() && m_amplitude) delete m_amplitude; if(hasType() && m_type) delete m_type; if(hasDeviation() && m_deviation) delete m_deviation; diff --git a/src/slam6d/scan.cc b/src/slam6d/scan.cc index 38b47d9..f8e61a6 100644 --- a/src/slam6d/scan.cc +++ b/src/slam6d/scan.cc @@ -120,6 +120,7 @@ void Scan::clear(unsigned int types) if(types & DATA_XYZ) clear("xyz"); if(types & DATA_RGB) clear("rgb"); if(types & DATA_REFLECTANCE) clear("reflectance"); + if(types & DATA_TEMPERATURE) clear("temperature"); if(types & DATA_AMPLITUDE) clear("amplitude"); if(types & DATA_TYPE) clear("type"); if(types & DATA_DEVIATION) clear("deviation"); diff --git a/src/slam6d/scan_diff.cc b/src/slam6d/scan_diff.cc index ba3783a..82418ea 100644 --- a/src/slam6d/scan_diff.cc +++ b/src/slam6d/scan_diff.cc @@ -40,19 +40,18 @@ using std::endl; using std::ofstream; using std::ifstream; #include - -#include "slam6d/scan.h" -#ifdef WITH_SCANSERVER -#include "scanserver/clientInterface.h" -#endif //WITH_SCANSERVER +#include #include "slam6d/globals.icc" +#include "slam6d/io_utils.h" +#include "slam6d/scan.h" + +#include "scanserver/clientInterface.h" #ifdef _OPENMP #include #endif - #ifndef _MSC_VER #include #else @@ -60,15 +59,15 @@ using std::ifstream; #endif #ifdef _MSC_VER -#define strcasecmp _stricmp -#define strncasecmp _strnicmp -#include -#include + #define strcasecmp _stricmp + #define strncasecmp _strnicmp + #include + #include #else -#include -#include -#include -#include + #include + #include + #include + #include #endif @@ -108,6 +107,8 @@ void usage(char* prog) << endl << bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR" << endl << " write all points that have no corresponding point closer than NR 'units'" << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl << endl << endl; cout << bold << "EXAMPLES " << normal << endl @@ -132,13 +133,16 @@ void usage(char* prog) */ int parseArgs(int argc, char **argv, string &dir, int &start, int &end, int &maxDist, int &minDist, double &dist, - IOType &type, bool &desc) + IOType &type, bool &desc, bool scanserver) { int c; // from unistd.h: extern char *optarg; extern int optind; + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + /* options descriptor */ // 0: no arguments, 1: required argument, 2: optional argument static struct option longopts[] = { @@ -148,6 +152,7 @@ int parseArgs(int argc, char **argv, string &dir, { "start", required_argument, 0, 's' }, { "end", required_argument, 0, 'e' }, { "dist", required_argument, 0, 'd' }, + { "scanserver", no_argument, 0, 'S' }, { 0, 0, 0, 0} // needed, cf. getopt.h }; @@ -159,11 +164,11 @@ int parseArgs(int argc, char **argv, string &dir, dist = atof(optarg); break; case 's': - start = atoi(optarg); + w_start = atoi(optarg); if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } break; case 'e': - end = atoi(optarg); + w_end = atoi(optarg); if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } break; case 'f': @@ -180,9 +185,12 @@ int parseArgs(int argc, char **argv, string &dir, case 'M': minDist = atoi(optarg); break; - case '?': + case '?': usage(argv[0]); return 1; + case 'S': + scanserver = true; + break; default: abort (); } @@ -209,6 +217,8 @@ int parseArgs(int argc, char **argv, string &dir, desc = true; } + parseFormatFile(dir, w_type, w_start, w_end); + return 0; } @@ -225,7 +235,7 @@ int parseArgs(int argc, char **argv, string &dir, int main(int argc, char **argv) { - cout << "(c) Jacobs University Bremen, gGmbH, 2010" << endl << endl; + cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl; if (argc <= 1) { usage(argv[0]); @@ -240,23 +250,20 @@ int main(int argc, char **argv) int minDist = -1; IOType type = RIEGL_TXT; bool desc = false; + bool scanserver = false; - parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc); + parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc, scanserver); -#ifdef WITH_SCANSERVER - try { - ClientInterface::create(); - } catch(std::runtime_error& e) { - cerr << "ClientInterface could not be created: " << e.what() << endl; - cerr << "Start the scanserver first." << endl; - exit(-1); + if (scanserver) { + try { + ClientInterface::create(); + } catch(std::runtime_error& e) { + cerr << "ClientInterface could not be created: " << e.what() << endl; + cerr << "Start the scanserver first." << endl; + exit(-1); + } } -#endif //WITH_SCANSERVER - // Get Scans (all scans between start and end) -#ifndef WITH_SCANSERVER - Scan::dir = dir; -#endif //WITH_SCANSERVER string diffdir = dir + "diff"; #ifdef _MSC_VER @@ -278,7 +285,7 @@ int main(int argc, char **argv) double inMatrix0[17]; double inMatrix1[17]; - cout << "Reading Scan No. " << start; + cout << "Reading Scan No. " << start << endl; framesFileName = dir + "scan" + to_string(start,3) + ".frames"; frames_in.open(framesFileName.c_str()); @@ -296,7 +303,7 @@ int main(int argc, char **argv) } cout << endl; - cout << "Reading Scan No. " << end; + cout << "Reading Scan No. " << end << endl; framesFileName = dir + "scan" + to_string(end,3) + ".frames"; frames_in.open(framesFileName.c_str()); @@ -313,32 +320,40 @@ int main(int argc, char **argv) cout << inMatrix1[i] << " "; } cout << endl; - - Scan::readScans(type, start, end, dir, maxDist, minDist, 0); - int endIndex = Scan::allScans.size() - 1; - Scan::allScans[0]->calcReducedPoints(-1, 0); - Scan::allScans[0]->transform(inMatrix0, Scan::INVALID); - Scan::allScans[endIndex]->calcReducedPoints(-1, 0); - Scan::allScans[endIndex]->transform(inMatrix1, Scan::INVALID); - - cout << Scan::allScans[0]->get_points_red_size() - << " " << Scan::allScans[endIndex]->get_points_red_size() << endl; + 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); + } + + Scan* scan_first = *(Scan::allScans.begin()); + Scan* scan_second = *(Scan::allScans.end()-1); + + scan_first->transform(inMatrix0, Scan::INVALID); + scan_second->transform(inMatrix1, Scan::INVALID); + + DataXYZ xyz_first(scan_first->get("xyz")); + DataXYZ xyz_second(scan_second->get("xyz")); + + cout << "Scan " << scan_first->getIdentifier() << " with " << xyz_first.size() << " points" << endl; + cout << "Scan " << scan_second->getIdentifier() << " with " << xyz_second.size() << " points" << endl; int thread_num = 0; - vector diff; + std::vector diff; double transMat[16]; if(desc) { - Scan::getNoPairsSimple(diff, Scan::allScans[endIndex], Scan::allScans[0], thread_num, dist); + Scan::getNoPairsSimple(diff, scan_second, scan_first, thread_num, dist); M4inv(inMatrix1, transMat); scanFileName = dir + "diff/scan" + to_string(end,3) + ".3d"; } else { - Scan::getNoPairsSimple(diff, Scan::allScans[0], Scan::allScans[endIndex], thread_num, dist); + Scan::getNoPairsSimple(diff, scan_first, scan_second, thread_num, dist); M4inv(inMatrix0, transMat); scanFileName = dir + "diff/scan" + to_string(start,3) + ".3d"; } - + cout << endl; for(int i = 0; i < 16; i++) { cout << transMat[i] << " "; @@ -359,10 +374,18 @@ int main(int argc, char **argv) ptsout.close(); ptsout.clear(); - - delete Scan::allScans[0]; - Scan::allScans.clear(); + + if (scanserver) { + scan_first->clear("xyz"); + scan_second->clear("xyz"); + } cout << endl << endl; cout << "Normal program end." << endl << endl; + + if (scanserver) { + Scan::closeDirectory(); + } + + return 0; } diff --git a/src/slam6d/scan_red.cc b/src/slam6d/scan_red.cc index 1c72af8..5c05f84 100644 --- a/src/slam6d/scan_red.cc +++ b/src/slam6d/scan_red.cc @@ -1,7 +1,7 @@ /* * scan_red implementation * - * Copyright (C) Dorit Borrmann + * Copyright (C) Dorit Borrmann, Razvan-George Mihalyi, Remus Dumitru * * Released under the GPL version 3. * @@ -37,10 +37,14 @@ using std::endl; using std::ofstream; #include +#include "slam6d/metaScan.h" +#include "slam6d/io_utils.h" #include "slam6d/scan.h" -#ifdef WITH_SCANSERVER +#include "slam6d/fbr/fbr_global.h" +#include "slam6d/fbr/panorama.h" +#include "slam6d/fbr/scan_cv.h" + #include "scanserver/clientInterface.h" -#endif //WITH_SCANSERVER #include "slam6d/globals.icc" @@ -67,6 +71,22 @@ using std::ofstream; #include #endif +//Vertical angle of view of scanner +#define MAX_ANGLE 60.0 +#define MIN_ANGLE -40.0 + +#define IMAGE_HEIGHT 1000 +#define IMAGE_WIDTH 3600 + +using namespace fbr; + +projection_method strToPMethod(string method){ + if(strcasecmp(method.c_str(), "EQUIRECTANGULAR") == 0) return EQUIRECTANGULAR; + else if(strcasecmp(method.c_str(), "CYLINDRICAL") == 0) return CYLINDRICAL; + else if(strcasecmp(method.c_str(), "MERCATOR") == 0) return MERCATOR; + else if(strcasecmp(method.c_str(), "CONIC") == 0) return CONIC; + else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown")); +} /** * Explains the usage of this program's command line parameters @@ -84,13 +104,16 @@ void usage(char* prog) << bold << "USAGE " << normal << endl << " " << prog << " [options] -r directory" << endl << endl; cout << bold << "OPTIONS" << normal << 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 << " -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})" << endl + << " (choose 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})" << endl << endl << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl << " neglegt all data points with a distance larger than NR 'units'" << endl @@ -99,18 +122,27 @@ void usage(char* prog) << " neglegt all data points with a distance smaller than NR 'units'" << endl << endl << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl - << " turns on octree based point reduction (voxel size=)" << endl + << " if NR >= 0, turns on octree based point reduction (voxel size=)" << endl + << " if NR < 0, turns on rescaling based reduction" << 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 + << bold << " -I" << normal << " NR," << bold << "--rangeimage=" << normal << "NR" << endl + << " use rescaling of the range image as reduction method" << endl + << " if NR = 1 recovers ranges from range image" << endl + << " if NR = 2 interpolates 3D points in the image map" << endl + << " if NR is omitted, then NR=1 is selected" << endl << endl - << endl << endl; + << bold << " -p" << normal << " MET," << bold << "--projection=" << normal << "MET" << endl + << " create range image using the MET projection method" << endl + << " (choose MET from [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|CONIC])" << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl << endl; cout << bold << "EXAMPLES " << normal << endl << " " << prog << " -m 500 -r 5 dat" << endl << " " << prog << " --max=5000 -r 10.2 dat" << endl - << " " << prog << " -s 2 -e 10 -r dat" << endl << endl; + << " " << prog << " -s 2 -e 10 -r dat" << endl + << " " << prog << " -s 0 -e 1 -r 10 -I=1 dat " << endl << endl; exit(1); } @@ -124,13 +156,15 @@ void usage(char* prog) * @param end stopping at scan number 'end' * @param maxDist - maximal distance of points being loaded * @param minDist - minimal distance of points being loaded + * @param projection - projection method for building range image * @param quiet switches on/off the quiet mode * @param veryQuiet switches on/off the 'very quiet' mode * @return 0, if the parsing was successful. 1 otherwise */ int parseArgs(int argc, char **argv, string &dir, double &red, - int &start, int &end, int &maxDist, int &minDist, int &octree, - IOType &type) + int &start, int &end, int &maxDist, int &minDist, + string &projection, int &octree, IOType &type, + int &rangeimage, bool &scanserver) { bool reduced = false; int c; @@ -138,6 +172,9 @@ int parseArgs(int argc, char **argv, string &dir, double &red, extern char *optarg; extern int optind; + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + /* options descriptor */ // 0: no arguments, 1: required argument, 2: optional argument static struct option longopts[] = { @@ -148,11 +185,14 @@ int parseArgs(int argc, char **argv, string &dir, double &red, { "end", required_argument, 0, 'e' }, { "reduce", required_argument, 0, 'r' }, { "octree", optional_argument, 0, 'O' }, + { "rangeimage", optional_argument, 0, 'I' }, + { "projection", required_argument, 0, 'p' }, + { "scanserver", no_argument, 0, 'S' }, { 0, 0, 0, 0} // needed, cf. getopt.h }; cout << endl; - while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:O:", longopts, NULL)) != -1) + while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:O:p:", longopts, NULL)) != -1) switch (c) { case 'r': @@ -160,36 +200,49 @@ int parseArgs(int argc, char **argv, string &dir, double &red, reduced = true; break; case 's': - start = atoi(optarg); - if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + w_start = atoi(optarg); + if (w_start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } break; case 'e': - 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); } + w_end = atoi(optarg); + if (w_end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (w_end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } break; case 'f': - try { - type = formatname_to_io_type(optarg); - } catch (...) { // runtime_error - cerr << "Format " << optarg << " unknown." << endl; - abort(); - } - break; + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; case 'm': maxDist = atoi(optarg); break; case 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } break; case 'M': minDist = atoi(optarg); break; - case '?': + case 'I': + if (optarg) { + rangeimage = atoi(optarg); + } else { + rangeimage = 1; + } + break; + case 'p': + projection = optarg; + break; + case 'S': + scanserver = true; + break; + case '?': usage(argv[0]); return 1; default: @@ -212,6 +265,8 @@ int parseArgs(int argc, char **argv, string &dir, double &red, if (dir[dir.length()-1] != '\\') dir = dir + "\\"; #endif + parseFormatFile(dir, w_type, w_start, w_end); + return 0; } @@ -227,7 +282,7 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int main(int argc, char **argv) { - cout << "(c) Jacobs University Bremen, gGmbH, 2010" << endl << endl; + cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl; if (argc <= 1) { usage(argv[0]); @@ -236,30 +291,30 @@ int main(int argc, char **argv) // parsing the command line parameters // init, default values if not specified string dir; - double red = -1.0; - int start = 0, end = -1; - int maxDist = -1; - int minDist = -1; - int octree = 0; - IOType type = RIEGL_TXT; + double red = -1.0; + int start = 0, end = -1; + int maxDist = -1; + int minDist = -1; + string projection = "EQUIRECTANGULAR"; + int octree = 0; + IOType type = RIEGL_TXT; + int rangeimage = 0; + bool scanserver = false; - parseArgs(argc, argv, dir, red, start, end, maxDist, minDist, octree, type); + parseArgs(argc, argv, dir, red, start, end, maxDist, minDist, projection, + octree, type, rangeimage, scanserver); -#ifdef WITH_SCANSERVER - try { - ClientInterface::create(); - } catch(std::runtime_error& e) { - cerr << "ClientInterface could not be created: " << e.what() << endl; - cerr << "Start the scanserver first." << endl; - exit(-1); + if (scanserver) { + try { + ClientInterface::create(); + } catch(std::runtime_error& e) { + cerr << "ClientInterface could not be created: " << e.what() << endl; + cerr << "Start the scanserver first." << endl; + exit(-1); + } } -#endif //WITH_SCANSERVER - + // Get Scans -#ifndef WITH_SCANSERVER - Scan::dir = dir; - int fileNr = start; -#endif //WITH_SCANSERVER string reddir = dir + "reduced"; #ifdef _MSC_VER @@ -276,90 +331,136 @@ int main(int argc, char **argv) exit(1); } -#ifndef WITH_SCANSERVER - while (fileNr <= end) { - Scan::readScans(type, fileNr, fileNr, dir, maxDist, minDist, 0); - const double* rPos = Scan::allScans[0]->get_rPos(); - const double* rPosTheta = Scan::allScans[0]->get_rPosTheta(); - - // reduction filter for current scan! - - cout << "Reducing Scan No. " << fileNr << endl; - Scan::allScans[0]->calcReducedPoints(red, octree); - - - cout << "Writing Scan No. " << fileNr ; - cout << " with " << Scan::allScans[0]->get_points_red_size() << " points" << endl; - string scanFileName; - string poseFileName; - - scanFileName = dir + "reduced/scan" + to_string(fileNr,3) + ".3d"; - poseFileName = dir + "reduced/scan" + to_string(fileNr,3) + ".pose"; - - - ofstream redptsout(scanFileName.c_str()); - for (int j = 0; j < Scan::allScans[0]->get_points_red_size(); j++) { - Point p(Scan::allScans[0]->get_points_red()[j]); - //Points in global coordinate system - redptsout << p.x << " " << p.y << " " << p.z << endl; - - } - redptsout.close(); - redptsout.clear(); -#else //WITH_SCANSERVER - Scan::readScans(type, start, end, dir, maxDist, minDist); - for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) - { - Scan* scan = *it; - const double* rPos = scan->get_rPos(); - const double* rPosTheta = scan->get_rPosTheta(); + 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); + } - scan->calcReducedPoints(red, octree); + string scanFileName; + string poseFileName; - const char* id = scan->getIdentifier(); - cout << "Writing Scan No. " << id; - cout << " with " << scan->getCountReduced() << " points" << endl; - string scanFileName; - string poseFileName; + /// Use the OCTREE based reduction + if (rangeimage == 0) { + cout << endl << "Reducing point cloud using octrees" << endl; + int scan_number = start; + for(std::vector::iterator it = Scan::allScans.begin(); + it != Scan::allScans.end(); + ++it, ++scan_number) { + Scan* scan = *it; + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + scan->setRangeFilter(maxDist, minDist); + scan->setReductionParameter(red, octree); + // get reduced points + DataXYZ xyz_r(scan->get("xyz reduced")); + unsigned int nPoints = xyz_r.size(); - scanFileName = dir + "reduced/scan" + id + ".3d"; - poseFileName = dir + "reduced/scan" + id + ".pose"; + const char* id = scan->getIdentifier(); + cout << "Writing Scan No. " << id; + cout << " with " << xyz_r.size() << " points" << endl; + scanFileName = reddir + "/scan" + id + ".3d"; + poseFileName = reddir + "/scan" + id + ".pose"; - ofstream redptsout(scanFileName.c_str()); - DataXYZ xyz_r(scan->getXYZReduced()); - for(unsigned int j = 0; j < xyz_r.size(); j++) { - //Points in global coordinate system - redptsout << xyz_r[j][0] << " " << xyz_r[j][1] << " " << xyz_r[j][2] << endl; + ofstream redptsout(scanFileName.c_str()); + for(unsigned int j = 0; j < nPoints; j++) { + redptsout << xyz_r[j][0] << " " + << xyz_r[j][1] << " " + << xyz_r[j][2] << endl; + } + redptsout.close(); + redptsout.clear(); + + ofstream posout(poseFileName.c_str()); + posout << rPos[0] << " " + << rPos[1] << " " + << rPos[2] << endl + << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + + posout.close(); + posout.clear(); + if (scanserver) { + scan->clear("xyz reduced"); + } } - redptsout.close(); - redptsout.clear(); -#endif //WITH_SCANSERVER - - ofstream posout(poseFileName.c_str()); - - posout << rPos[0] << " " - << rPos[1] << " " - << rPos[2] << endl - << deg(rPosTheta[0]) << " " - << deg(rPosTheta[1]) << " " - << deg(rPosTheta[2]) << endl; - - posout.close(); - posout.clear(); + } else { /// use the RESIZE based reduction + cout << endl << "Reducing point cloud by rescaling the range image" << endl; -#ifndef WITH_SCANSERVER - delete Scan::allScans[0]; - Scan::allScans.clear(); - fileNr++; -#endif //WITH_SCANSERVER + Scan::openDirectory(false, dir, type, start, end); + if (Scan::allScans.size() <= 0) { + cerr << "No scans found!" << endl; + exit(-1); + } + + for (int scan_number = start; scan_number <= end; scan_number++) { + + Scan* scan = Scan::allScans[scan_number]; + scan->setRangeFilter(maxDist, minDist); + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + scanFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".3d"; + poseFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".pose"; + // Create a panorama. The iMap inside does all the tricks for us. + scan_cv sScan(dir, scan_number, type); + sScan.convertScanToMat(); + + /// Project point cloud using the selected projection method + panorama image(IMAGE_WIDTH, IMAGE_HEIGHT, strToPMethod(projection)); + image.createPanorama(sScan.getMatScan()); + image.getDescription(); + + /// Resize the range image, specify desired interpolation method + double scale = 1.0/red; + cv::Mat range_image_resized; // reflectance_image_resized; + string ofilename; + stringstream ss; + ss << setw(3) << setfill('0') << (scan_number); + ofilename = reddir + "/scan" + ss.str() + ".3d"; + if (rangeimage == 1) { + resize(image.getRangeImage(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + // Recover point cloud from image and write scan to file + stringstream ss; + ss << setw(3) << setfill('0') << (scan_number); + image.recoverPointCloud(range_image_resized, ofilename); + } else { + resize(image.getMap(), range_image_resized, cv::Size(), + scale, scale, cv::INTER_NEAREST); + ofstream redptsout(ofilename.c_str()); + // Convert back to 3D. + for(int i = 0; i < range_image_resized.rows; i++) { + for(int j = 0; j < range_image_resized.cols; j++) { + cv::Vec3f vec = range_image_resized.at(i, j); + double x = vec[0]; + double y = vec[1]; + double z = vec[2]; + redptsout << x << " " << y << " " << z << endl; + } + } + } + + ofstream posout(poseFileName.c_str()); + posout << rPos[0] << " " + << rPos[1] << " " + << rPos[2] << endl + << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + posout.clear(); + posout.close(); + } } cout << endl << endl; cout << "Normal program end." << endl << endl; -#ifdef WITH_SCANSERVER - Scan::clearScans(); - ClientInterface::destroy(); -#endif //WITH_SCANSERVER + if (scanserver) { + Scan::closeDirectory(); + ClientInterface::destroy(); + } }