diff --git a/.svn/pristine/00/005448ebd9d9464d1a40fef89097904af691347a.svn-base b/.svn/pristine/00/005448ebd9d9464d1a40fef89097904af691347a.svn-base deleted file mode 100644 index cb9556c..0000000 --- a/.svn/pristine/00/005448ebd9d9464d1a40fef89097904af691347a.svn-base +++ /dev/null @@ -1,96 +0,0 @@ -/** - * @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 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/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base b/.svn/pristine/01/01306314b7639abedb5e9adb7c97d92417714645.svn-base similarity index 76% rename from .svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base rename to .svn/pristine/01/01306314b7639abedb5e9adb7c97d92417714645.svn-base index b9193ac..749e9d2 100644 --- a/.svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base +++ b/.svn/pristine/01/01306314b7639abedb5e9adb7c97d92417714645.svn-base @@ -12,6 +12,10 @@ #include #include #include +//for opencv 2.4 +#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4) +#include +#endif #include #include #include "slam6d/io_types.h" @@ -78,6 +82,21 @@ namespace fbr{ RANSAC, DISABLE, }; + /** + * @enum feature_filtration_method + */ + enum feature_filtration_method{ + OCCLUSION, + STANDARD_DEVIATION, + DISABLE_FILTER, + }; + /** + * @enum matching_filtration_method + */ + enum matching_filtration_method{ + FUNDEMENTAL_MATRIX, + DISABLE_MATCHING_FILTER, + }; //RANSAC iteration #define RANSACITR 20000 //Inlier influence @@ -97,5 +116,9 @@ namespace fbr{ matcher_method stringToMatcherMethod(string method); string registrationMethodToString(registration_method method); registration_method stringToRegistrationMethod(string method); + string featureFiltrationMethodToString(feature_filtration_method method); + feature_filtration_method stringToFeatureFiltrationMethod(string method); + string matchingFiltrationMethodToString(matching_filtration_method method); + matching_filtration_method stringToMatchingFiltrationMethod(string method); } #endif /* FBR_GLOBAL_H_ */ diff --git a/.svn/pristine/01/01bcde80a6566f62bb36950729e0ab538661d811.svn-base b/.svn/pristine/01/01bcde80a6566f62bb36950729e0ab538661d811.svn-base deleted file mode 100644 index 1d06b39..0000000 --- a/.svn/pristine/01/01bcde80a6566f62bb36950729e0ab538661d811.svn-base +++ /dev/null @@ -1,90 +0,0 @@ -/** - * @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* 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/02/023acba8dc7f8a304cfe4c94511903d976659f48.svn-base b/.svn/pristine/02/023acba8dc7f8a304cfe4c94511903d976659f48.svn-base deleted file mode 100644 index 0591988..0000000 --- a/.svn/pristine/02/023acba8dc7f8a304cfe4c94511903d976659f48.svn-base +++ /dev/null @@ -1,75 +0,0 @@ -/** - * @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); - 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/02/026e80e057a6d9159d25c5e8e26f83bbd0193ce8.svn-base b/.svn/pristine/02/026e80e057a6d9159d25c5e8e26f83bbd0193ce8.svn-base deleted file mode 100644 index 0292987..0000000 --- a/.svn/pristine/02/026e80e057a6d9159d25c5e8e26f83bbd0193ce8.svn-base +++ /dev/null @@ -1,38 +0,0 @@ -Project admins - -Andreas Nuechter andreas@nuechti.de -Kai Lingemann kai.lingemann@gmx.de -Dorit Borrmann d.borrmann@jacobs-university.de - -List of contributors - -Andreas Nuechter andreas@nuechti.de -Kai Lingemann kai.lingemann@gmx.de -Dorit Borrmann d.borrmann@jacobs-university.de -Jan Elseberg j.elseberg@jacobs-university.de -Jochen Sprickerhof jochen@sprickerhof.de -HamidReza Houshiar h.houshiar@jacobs-university.de -Sven Albrecht sven.albrecht@uni-osnabrueck.de -Stan Serebryakov cfr.ssv@gmail.com -Thomas Escher tescher@uni-osnabrueck.de -Thomas Wiemann twiemann@uni-osnabrueck.de -Alexandru Tandrau alexandru@tandrau.com -Alexandru Eugen Ichim eugen@alexichim.com -Flavia Grosan me@flaviagrosan.com -Deyuan Qiu deyuan.qiu@googlemail.com -Darko Makreshanski d.makreshanski@jacobs-university.de -Mohammad Faisal Abdullah m.faisal@jacobs-university.de -Li Ming liming751218@whu.edu.cn -Li Wei xpaulee@gmail.com -Shams Feyzabadi sh.feyzabadi@gmail.co -Vladislav Perelmann v.perelman@jacobs-university.de -Chen Long lchen.whu@gmail.com -Remuas Dumitru r.dumitru@jaocbs-university.de -Billy Okal okal.billy@googlemail.com - -Further contributors - -Uwe Hebbelmann, Sebastian Stock, Andre Schemschat -Hartmut Surmann -Amuz Tamrakars, Ulugbek Makhmudov -Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz \ No newline at end of file diff --git a/.svn/pristine/03/03ccc14e340c414865346c135b0fb69f2b68f058.svn-base b/.svn/pristine/03/03ccc14e340c414865346c135b0fb69f2b68f058.svn-base deleted file mode 100644 index aabf033..0000000 --- a/.svn/pristine/03/03ccc14e340c414865346c135b0fb69f2b68f058.svn-base +++ /dev/null @@ -1,23 +0,0 @@ -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) \ No newline at end of file diff --git a/.svn/pristine/05/056f32a80b96bc1cd572918b1f14195c26777b0e.svn-base b/.svn/pristine/05/056f32a80b96bc1cd572918b1f14195c26777b0e.svn-base deleted file mode 100644 index d43c458..0000000 --- a/.svn/pristine/05/056f32a80b96bc1cd572918b1f14195c26777b0e.svn-base +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file - * @brief IO of a 3D scan in rts file format - * @author Thomas Escher - */ - -#ifndef __SCAN_IO_UOS_H__ -#define __SCAN_IO_UOS_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* 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/78/78173974d7adc42895bd5ed6c64cf3d66c8813a7.svn-base b/.svn/pristine/0d/0daad64cb6c1333f17a971b251ba9dabf0e903ac.svn-base similarity index 98% rename from .svn/pristine/78/78173974d7adc42895bd5ed6c64cf3d66c8813a7.svn-base rename to .svn/pristine/0d/0daad64cb6c1333f17a971b251ba9dabf0e903ac.svn-base index 9bb4638..b3c0966 100644 --- a/.svn/pristine/78/78173974d7adc42895bd5ed6c64cf3d66c8813a7.svn-base +++ b/.svn/pristine/0d/0daad64cb6c1333f17a971b251ba9dabf0e903ac.svn-base @@ -30,6 +30,7 @@ struct information{ feature_descriptor_method dMethod; matcher_method mMethod; registration_method rMethod; + bool scanServer; int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum; double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime; @@ -60,6 +61,7 @@ void usage(int argc, char** argv){ printf("\t\t-r registration \t registration method [ALL|ransac]\n"); printf("\t\t-V verbose \t\t level of verboseness\n"); printf("\t\t-O outDir \t\t output directory if not stated same as input\n"); + printf("\t\t-S scanServer \t\t Scan Server\n"); printf("\n"); printf("\tExamples:\n"); printf("\tUsing Bremen City dataset:\n"); @@ -101,11 +103,12 @@ void parssArgs(int argc, char** argv, information& info){ info.mMethod = RATIO; info.rMethod = RANSAC; info.outDir = ""; + info.scanServer = false; int c; opterr = 0; //reade the command line and get the options - while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:s:e:")) != -1) + while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:s:e:S")) != -1) switch (c) { case 's': @@ -162,6 +165,9 @@ void parssArgs(int argc, char** argv, information& info){ case 'O': info.outDir = optarg; break; + case 'S': + info.scanServer = true; + break; case '?': cout<<"Unknown option character "<= 1) informationDescription(info); - scan_cv fScan (info.dir, info.fScanNumber, info.sFormat); + scan_cv fScan (info.dir, info.fScanNumber, info.sFormat, info.scanServer); if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount(); fScan.convertScanToMat(); if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency(); @@ -328,7 +335,7 @@ int main(int argc, char** argv){ if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency(); if(info.verbose >= 2) fFeature.getDescription(); - scan_cv sScan (info.dir, info.sScanNumber, info.sFormat); + scan_cv sScan (info.dir, info.sScanNumber, info.sFormat, info.scanServer); if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount(); sScan.convertScanToMat(); if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency(); @@ -358,7 +365,7 @@ int main(int argc, char** argv){ sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod); if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency(); if(info.verbose >= 2) sFeature.getDescription(); - + feature_matcher matcher (info.mMethod, info.mParam); if(info.verbose >= 4) info.mTime = (double)cv::getTickCount(); matcher.match(fFeature, sFeature); diff --git a/.svn/pristine/10/1030b86587f750dc525d03b394a0c558d9d62919.svn-base b/.svn/pristine/10/1030b86587f750dc525d03b394a0c558d9d62919.svn-base deleted file mode 100644 index e6f5206..0000000 --- a/.svn/pristine/10/1030b86587f750dc525d03b394a0c558d9d62919.svn-base +++ /dev/null @@ -1,23 +0,0 @@ -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} fbr_global.cc) - -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/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base b/.svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base deleted file mode 100644 index 03f6817..0000000 --- a/.svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base +++ /dev/null @@ -1,150 +0,0 @@ -/* - * 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, "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 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/1b/1b506d343f0ac655ba69c79f83780b07c77f8b50.svn-base b/.svn/pristine/1b/1b506d343f0ac655ba69c79f83780b07c77f8b50.svn-base deleted file mode 100644 index d45cda3..0000000 --- a/.svn/pristine/1b/1b506d343f0ac655ba69c79f83780b07c77f8b50.svn-base +++ /dev/null @@ -1,220 +0,0 @@ -/* - * 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* 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/1b/1bf353f1f69296fe2826ceb5a77f200b98f37b02.svn-base b/.svn/pristine/1b/1bf353f1f69296fe2826ceb5a77f200b98f37b02.svn-base deleted file mode 100644 index 41b247b..0000000 --- a/.svn/pristine/1b/1bf353f1f69296fe2826ceb5a77f200b98f37b02.svn-base +++ /dev/null @@ -1,136 +0,0 @@ -/** - * @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; 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];}; - /** - * 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];}; - - /** - * 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;}; - - 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 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/1c/1cd1acf52aa3601767e206e9fbee1863ce165799.svn-base b/.svn/pristine/1c/1cd1acf52aa3601767e206e9fbee1863ce165799.svn-base deleted file mode 100644 index 6ccfadb..0000000 --- a/.svn/pristine/1c/1cd1acf52aa3601767e206e9fbee1863ce165799.svn-base +++ /dev/null @@ -1,429 +0,0 @@ -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) - - - -## 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) - -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/1d/1d575d9db1fdef6c354a816d14a84b1c1e453eda.svn-base b/.svn/pristine/1d/1d575d9db1fdef6c354a816d14a84b1c1e453eda.svn-base deleted file mode 100644 index bda709e..0000000 --- a/.svn/pristine/1d/1d575d9db1fdef6c354a816d14a84b1c1e453eda.svn-base +++ /dev/null @@ -1,839 +0,0 @@ -/* - * viewcull implementation - * - * Copyright (C) Jan Elseberg - * - * Released under the GPL version 3. - * - */ - -/** - * @file - * @brief Frustrum culling routines - * - * @author Jan Elseberg. Automation Group, Jacobs University Bremen gGmbH, Germany. - */ -#include "show/viewcull.h" -#include -#include -#include - -#include -#include -#include "slam6d/globals.icc" - -/** The 6 planes of the viewing frustum */ -float frustum[6][4]; - -/** the modelview * projection matrix to map a model point to an onscreen coordinate */ -float matrix[16]; -/** some useful variables for faster calculation of the pixel coordinates */ -short VP[4]; -/** a unit vector pointing to the right of the screen */ -float right[3]; -/** how much detail is shown, 0 means everything is plotted */ -short DETAIL; - -double SX, SY, SZ, EX, EY, EZ; -float origin[3], dir[3]; /*ray */ -float dist; -int rayX,rayY; -float rayVP[4]; - -void calcRay(int x, int y, double znear, double zfar) { - - GLdouble modelMatrix[16]; - GLdouble projMatrix[16]; - int viewport[4]; - glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix); - glGetDoublev(GL_PROJECTION_MATRIX,projMatrix); - glGetIntegerv(GL_VIEWPORT,viewport); - - gluUnProject(x, viewport[3]-y, zfar, modelMatrix, projMatrix, viewport, &SX, &SY, &SZ); - gluUnProject(x, viewport[3]-y, znear, modelMatrix, projMatrix, viewport, &EX, &EY, &EZ); - - origin[0] = SX; - origin[1] = SY; - origin[2] = SZ; - dir[0] = EX-SX; - dir[1] = EY-SY; - dir[2] = EZ-SZ; - double t = sqrt( dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2] ); - dir[0] /= t; - dir[1] /= t; - dir[2] /= t; - - dist = SX * dir[0] + SY * dir[1] + SZ * dir[2]; - rayVP[0] = 0.5*viewport[2]; - rayVP[1] = 0.5*viewport[2] + viewport[0]; - - rayVP[2] = 0.5*viewport[3]; - rayVP[3] = 0.5*viewport[3] + viewport[1]; - rayX = x; - rayY = viewport[3]-y; - - remViewport(); -} - -void remViewport() { - GLdouble modelMatrix[16]; - GLdouble projMatrix[16]; - int viewport[4]; - glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix); - glGetDoublev(GL_PROJECTION_MATRIX,projMatrix); - glGetIntegerv(GL_VIEWPORT,viewport); - - MMult( projMatrix, modelMatrix, matrix ); - VP[0] = 0.5*viewport[2]; - VP[1] = 0.5*viewport[2] + viewport[0]; - - VP[2] = 0.5*viewport[3]; - VP[3] = 0.5*viewport[3] + viewport[1]; - - right[0] = modelMatrix[0]; - right[1] = modelMatrix[4]; - right[2] = modelMatrix[8]; -} - -void ExtractFrustum(short detail) -{ - DETAIL = detail + 1; - remViewport(); - - float proj[16]; - float modl[16]; - float clip[16]; - float t; - - /* Get the current PROJECTION matrix from OpenGL */ - glGetFloatv( GL_PROJECTION_MATRIX, proj ); - - /* Get the current MODELVIEW matrix from OpenGL */ - glGetFloatv( GL_MODELVIEW_MATRIX, modl ); - - /* Combine the two matrices (multiply projection by modelview) */ - clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12]; - clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13]; - clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14]; - clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15]; - - clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12]; - clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13]; - clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14]; - clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15]; - - clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12]; - clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13]; - clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14]; - clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15]; - - clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12]; - clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13]; - clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14]; - clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15]; - - /* Extract the numbers for the RIGHT plane */ - frustum[0][0] = clip[ 3] - clip[ 0]; - frustum[0][1] = clip[ 7] - clip[ 4]; - frustum[0][2] = clip[11] - clip[ 8]; - frustum[0][3] = clip[15] - clip[12]; - - /* Normalize the result */ - t = sqrt( frustum[0][0] * frustum[0][0] + frustum[0][1] * frustum[0][1] + frustum[0][2] * frustum[0][2] ); - frustum[0][0] /= t; - frustum[0][1] /= t; - frustum[0][2] /= t; - frustum[0][3] /= t; - - /* Extract the numbers for the LEFT plane */ - frustum[1][0] = clip[ 3] + clip[ 0]; - frustum[1][1] = clip[ 7] + clip[ 4]; - frustum[1][2] = clip[11] + clip[ 8]; - frustum[1][3] = clip[15] + clip[12]; - - /* Normalize the result */ - t = sqrt( frustum[1][0] * frustum[1][0] + frustum[1][1] * frustum[1][1] + frustum[1][2] * frustum[1][2] ); - frustum[1][0] /= t; - frustum[1][1] /= t; - frustum[1][2] /= t; - frustum[1][3] /= t; - - /* Extract the BOTTOM plane */ - frustum[2][0] = clip[ 3] + clip[ 1]; - frustum[2][1] = clip[ 7] + clip[ 5]; - frustum[2][2] = clip[11] + clip[ 9]; - frustum[2][3] = clip[15] + clip[13]; - - /* Normalize the result */ - t = sqrt( frustum[2][0] * frustum[2][0] + frustum[2][1] * frustum[2][1] + frustum[2][2] * frustum[2][2] ); - frustum[2][0] /= t; - frustum[2][1] /= t; - frustum[2][2] /= t; - frustum[2][3] /= t; - - /* Extract the TOP plane */ - frustum[3][0] = clip[ 3] - clip[ 1]; - frustum[3][1] = clip[ 7] - clip[ 5]; - frustum[3][2] = clip[11] - clip[ 9]; - frustum[3][3] = clip[15] - clip[13]; - - /* Normalize the result */ - t = sqrt( frustum[3][0] * frustum[3][0] + frustum[3][1] * frustum[3][1] + frustum[3][2] * frustum[3][2] ); - frustum[3][0] /= t; - frustum[3][1] /= t; - frustum[3][2] /= t; - frustum[3][3] /= t; - - /* Extract the FAR plane */ - frustum[4][0] = clip[ 3] - clip[ 2]; - frustum[4][1] = clip[ 7] - clip[ 6]; - frustum[4][2] = clip[11] - clip[10]; - frustum[4][3] = clip[15] - clip[14]; - - /* Normalize the result */ - t = sqrt( frustum[4][0] * frustum[4][0] + frustum[4][1] * frustum[4][1] + frustum[4][2] * frustum[4][2] ); - frustum[4][0] /= t; - frustum[4][1] /= t; - frustum[4][2] /= t; - frustum[4][3] /= t; - - /* Extract the NEAR plane */ - frustum[5][0] = clip[ 3] + clip[ 2]; - frustum[5][1] = clip[ 7] + clip[ 6]; - frustum[5][2] = clip[11] + clip[10]; - frustum[5][3] = clip[15] + clip[14]; - - /* Normalize the result */ - t = sqrt( frustum[5][0] * frustum[5][0] + frustum[5][1] * frustum[5][1] + frustum[5][2] * frustum[5][2] ); - frustum[5][0] /= t; - frustum[5][1] /= t; - frustum[5][2] /= t; - frustum[5][3] /= t; - -} - - -void ExtractFrustum(float *frust[6]) -{ - remViewport(); - - float proj[16]; - float modl[16]; - float clip[16]; - float t; - - /* Get the current PROJECTION matrix from OpenGL */ - glGetFloatv( GL_PROJECTION_MATRIX, proj ); - - /* Get the current MODELVIEW matrix from OpenGL */ - glGetFloatv( GL_MODELVIEW_MATRIX, modl ); - - /* Combine the two matrices (multiply projection by modelview) */ - clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12]; - clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13]; - clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14]; - clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15]; - - clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12]; - clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13]; - clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14]; - clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15]; - - clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12]; - clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13]; - clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14]; - clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15]; - - clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12]; - clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13]; - clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14]; - clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15]; - - /* Extract the numbers for the RIGHT plane */ - frust[0][0] = clip[ 3] - clip[ 0]; - frust[0][1] = clip[ 7] - clip[ 4]; - frust[0][2] = clip[11] - clip[ 8]; - frust[0][3] = clip[15] - clip[12]; - - /* Normalize the result */ - t = sqrt( frust[0][0] * frust[0][0] + frust[0][1] * frust[0][1] + frust[0][2] * frust[0][2] ); - frust[0][0] /= t; - frust[0][1] /= t; - frust[0][2] /= t; - frust[0][3] /= t; - - /* Extract the numbers for the LEFT plane */ - frust[1][0] = clip[ 3] + clip[ 0]; - frust[1][1] = clip[ 7] + clip[ 4]; - frust[1][2] = clip[11] + clip[ 8]; - frust[1][3] = clip[15] + clip[12]; - - /* Normalize the result */ - t = sqrt( frust[1][0] * frust[1][0] + frust[1][1] * frust[1][1] + frust[1][2] * frust[1][2] ); - frust[1][0] /= t; - frust[1][1] /= t; - frust[1][2] /= t; - frust[1][3] /= t; - - /* Extract the BOTTOM plane */ - frust[2][0] = clip[ 3] + clip[ 1]; - frust[2][1] = clip[ 7] + clip[ 5]; - frust[2][2] = clip[11] + clip[ 9]; - frust[2][3] = clip[15] + clip[13]; - - /* Normalize the result */ - t = sqrt( frust[2][0] * frust[2][0] + frust[2][1] * frust[2][1] + frust[2][2] * frust[2][2] ); - frust[2][0] /= t; - frust[2][1] /= t; - frust[2][2] /= t; - frust[2][3] /= t; - - /* Extract the TOP plane */ - frust[3][0] = clip[ 3] - clip[ 1]; - frust[3][1] = clip[ 7] - clip[ 5]; - frust[3][2] = clip[11] - clip[ 9]; - frust[3][3] = clip[15] - clip[13]; - - /* Normalize the result */ - t = sqrt( frust[3][0] * frust[3][0] + frust[3][1] * frust[3][1] + frust[3][2] * frust[3][2] ); - frust[3][0] /= t; - frust[3][1] /= t; - frust[3][2] /= t; - frust[3][3] /= t; - - /* Extract the FAR plane */ - frust[4][0] = clip[ 3] - clip[ 2]; - frust[4][1] = clip[ 7] - clip[ 6]; - frust[4][2] = clip[11] - clip[10]; - frust[4][3] = clip[15] - clip[14]; - - /* Normalize the result */ - t = sqrt( frust[4][0] * frust[4][0] + frust[4][1] * frust[4][1] + frust[4][2] * frust[4][2] ); - frust[4][0] /= t; - frust[4][1] /= t; - frust[4][2] /= t; - frust[4][3] /= t; - - /* Extract the NEAR plane */ - frust[5][0] = clip[ 3] + clip[ 2]; - frust[5][1] = clip[ 7] + clip[ 6]; - frust[5][2] = clip[11] + clip[10]; - frust[5][3] = clip[15] + clip[14]; - - /* Normalize the result */ - t = sqrt( frust[5][0] * frust[5][0] + frust[5][1] * frust[5][1] + frust[5][2] * frust[5][2] ); - frust[5][0] /= t; - frust[5][1] /= t; - frust[5][2] /= t; - frust[5][3] /= t; - -} - - - -void myProject(float x, float y, float z, short &Xi ) { - float pn[2]; - // x coordinate on screen, not normalized - pn[0] = x * matrix[0] + y * matrix[4] + z * matrix[8] + matrix[12]; - // normalization - pn[1] = x * matrix[3] + y * matrix[7] + z * matrix[11] + matrix[15]; - - // normalized x coordinate on screen - pn[0] /= pn[1]; - - // true x coordinate in viewport coordinate system - Xi = pn[0]*VP[0] + VP[1]; -} - -int LOD2(float x, float y, float z, float size) -{ - size = sqrt(3*size*size); - - short X1; - short X2; - - // onscreen position of the leftmost point - myProject(x - size*right[0], y - size*right[1], z - size*right[2], X1); - // onscreen position of the rightmost point - myProject(x + size*right[0], y + size*right[1], z + size*right[2], X2); - - if (X1 > X2) { - return (X1-X2); - } else { - return (X2-X1); - } -} - -bool LOD(float x, float y, float z, float size) -{ - size = sqrt(3*size*size); - - short X1; - short X2; - - // onscreen position of the leftmost point - myProject(x - size*right[0], y - size*right[1], z - size*right[2], X1); - // onscreen position of the rightmost point - myProject(x + size*right[0], y + size*right[1], z + size*right[2], X2); - - if (X1 > X2) { - return (X1-X2) > DETAIL; - } else { - return (X2-X1) > DETAIL; - } -} - - -/** - * 0 if not in frustrum - * 1 if partial overlap - * 2 if entirely within frustrum - */ -int CubeInFrustum2( float x, float y, float z, float size ) -{ - int p; - - float xm, xp, ym, yp, zm, zp; - float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; - xm = x - size; - xp = x + size; - ym = y - size; - yp = y + size; - zm = z - size; - zp = z + size; - - for( p = 0; p < 6; p++ ) - { - Fxm = frustum[p][0] * xm; - Fym = frustum[p][1] * ym; - Fzm = frustum[p][2] * zm; - if( Fxm + Fym + Fzm + frustum[p][3] > 0 ) - continue; - - Fxp = frustum[p][0] * xp; - if( Fxp + Fym + Fzm + frustum[p][3] > 0 ) - continue; - - Fyp = frustum[p][1] * yp; - if( Fxm + Fyp + Fzm + frustum[p][3] > 0 ) - continue; - - if( Fxp + Fyp + Fzm + frustum[p][3] > 0 ) - continue; - - Fzp = frustum[p][2] * zp; - if( Fxm + Fym + Fzp + frustum[p][3] > 0 ) - continue; - - if( Fxp + Fym + Fzp + frustum[p][3] > 0 ) - continue; - if( Fxm + Fyp + Fzp + frustum[p][3] > 0 ) - continue; - if( Fxp + Fyp + Fzp + frustum[p][3] > 0 ) - continue; - return 0; - } - - // box is in frustrum, now check wether all corners are within. - // If one is outside return 1 otherwise 2 - for( p = 0; p < 6; p++ ) - { - Fxm = frustum[p][0] * xm; - Fym = frustum[p][1] * ym; - Fzm = frustum[p][2] * zm; - if( Fxm + Fym + Fzm + frustum[p][3] < 0 ) - return 1; - - Fxp = frustum[p][0] * xp; - if( Fxp + Fym + Fzm + frustum[p][3] < 0 ) - return 1; - - Fyp = frustum[p][1] * yp; - if( Fxm + Fyp + Fzm + frustum[p][3] < 0 ) - return 1; - - if( Fxp + Fyp + Fzm + frustum[p][3] < 0 ) - return 1; - - Fzp = frustum[p][2] * zp; - if( Fxm + Fym + Fzp + frustum[p][3] < 0 ) - return 1; - - if( Fxp + Fym + Fzp + frustum[p][3] < 0 ) - return 1; - if( Fxm + Fyp + Fzp + frustum[p][3] < 0 ) - return 1; - if( Fxp + Fyp + Fzp + frustum[p][3] < 0 ) - return 1; - } - return 2; - -} - -/* -char PlaneAABB( float x, float y, float z, float size, float *plane ) -{ - float dist = plane[3]; - - float xm, xp, ym, yp, zm, zp; - float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; - - float dmmm, dpmm, dmpm, dppm, dmmp, dpmp, dmpp, dppp; - xm = x - size; - xp = x + size; - ym = y - size; - yp = y + size; - zm = z - size; - zp = z + size; - - { - Fxm = plane[0] * xm; - Fym = plane[1] * ym; - Fzm = plane[2] * zm; - dmmm = Fxm + Fym + Fzm + dist; - if( dmmm > 0 ) - goto intersects; - - Fxp = plane[0] * xp; - dpmm = Fxp + Fym + Fzm + dist; - if( dpmm > 0 ) - goto intersects; - - Fyp = plane[1] * yp; - dmpm = Fxm + Fyp + Fzm + dist; - if( dmpm > 0 ) - goto intersects; - - dppm = Fxp + Fyp + Fzm + dist; - if( dppm > 0 ) - goto intersects; - - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp > 0 ) - goto intersects; - - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp > 0 ) - goto intersects; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp > 0 ) - goto intersects; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp > 0 ) - goto intersects; - return 0; // outside - } - - intersects: - - // cube is inside, determine if plane intersects the cube - { - if( dmmm < 0 ) - return 1; - Fxp = plane[0] * xp; - dpmm = Fxp + Fym + Fzm + dist; - if( dpmm < 0 ) - return 1; - Fyp = plane[1] * yp; - dmpm = Fxm + Fyp + Fzm + dist; - if( dmpm < 0 ) - return 1; - dppm = Fxp + Fyp + Fzm + dist; - if( dppm < 0 ) - return 1; - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp < 0 ) - return 1; - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - } - - // plane is outside - return 2; - -} -*/ -char PlaneAABB( float x, float y, float z, float size, float *plane ) -{ - float dist = plane[3]; - - float xm, xp, ym, yp, zm, zp; - float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; - - float dmmm, dpmm, dmpm, dppm, dmmp, dpmp, dmpp, dppp; - xm = x - size; - xp = x + size; - ym = y - size; - yp = y + size; - zm = z - size; - zp = z + size; - - Fxm = plane[0] * xm; - Fym = plane[1] * ym; - Fzm = plane[2] * zm; - dmmm = Fxm + Fym + Fzm + dist; - if( dmmm > 0 ) - { - Fxp = plane[0] * xp; - dpmm = Fxp + Fym + Fzm + dist; - if( dpmm < 0 ) - return 1; - Fyp = plane[1] * yp; - dmpm = Fxm + Fyp + Fzm + dist; - if( dmpm < 0 ) - return 1; - dppm = Fxp + Fyp + Fzm + dist; - if( dppm < 0 ) - return 1; - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp < 0 ) - return 1; - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - - Fxp = plane[0] * xp; - dpmm = Fxp + Fym + Fzm + dist; - if( dpmm > 0 ) - { - if( dmmm < 0 ) - return 1; - Fyp = plane[1] * yp; - dmpm = Fxm + Fyp + Fzm + dist; - if( dmpm < 0 ) - return 1; - dppm = Fxp + Fyp + Fzm + dist; - if( dppm < 0 ) - return 1; - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp < 0 ) - return 1; - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - - Fyp = plane[1] * yp; - dmpm = Fxm + Fyp + Fzm + dist; - if( dmpm > 0 ) - { - if( dmmm < 0 || dpmm < 0 ) - return 1; - dppm = Fxp + Fyp + Fzm + dist; - if( dppm < 0 ) - return 1; - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp < 0 ) - return 1; - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - - dppm = Fxp + Fyp + Fzm + dist; - if( dppm > 0 ) - { - if( dmmm < 0 || dpmm < 0 || dmpm < 0 ) - return 1; - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp < 0 ) - return 1; - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - - Fzp = plane[2] * zp; - dmmp = Fxm + Fym + Fzp + dist; - if( dmmp > 0 ) - { - if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 ) - return 1; - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - - dpmp = Fxp + Fym + Fzp + dist; - if( dpmp > 0 ) - { - if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 ) - return 1; - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - dmpp = Fxm + Fyp + Fzp + dist; - if( dmpp > 0 ) - { - if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 || dpmp < 0 ) - return 1; - dppp = Fxp + Fyp + Fzp + dist; - if( dppp < 0 ) - return 1; - return 2; - } - dppp = Fxp + Fyp + Fzp + dist; - if( dppp > 0 ) - { - if( dmmm < 0 || dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 || dpmp < 0 || dmpp < 0) - return 1; - return 2; - } - return 0; // outside -} - -int QuadInFrustrum2old( float x, float y, float z, float size ) -{ - int p; - - for( p = 0; p < 6; p++ ) - { - if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) - continue; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 ) - continue; - return 0; - } - - // box is in frustrum, now check wether all corners are within. If one is outside return 1 otherwise 2 - for( p = 0; p < 6; p++ ) - { - if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) - return 1; - if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 ) - return 1; - } - return 2; - -} - -bool CubeInFrustum( float x, float y, float z, float size ) -{ - int p; - - float xm, xp, ym, yp, zm, zp; - float Fxm, Fxp, Fym, Fyp, Fzm, Fzp; - xm = x - size; - xp = x + size; - ym = y - size; - yp = y + size; - zm = z - size; - zp = z + size; - - for( p = 0; p < 6; p++ ) - { - Fxm = frustum[p][0] * xm; - Fym = frustum[p][1] * ym; - Fzm = frustum[p][2] * zm; - if( Fxm + Fym + Fzm + frustum[p][3] > 0 ) - continue; - - Fxp = frustum[p][0] * xp; - if( Fxp + Fym + Fzm + frustum[p][3] > 0 ) - continue; - - Fyp = frustum[p][1] * yp; - if( Fxm + Fyp + Fzm + frustum[p][3] > 0 ) - continue; - - if( Fxp + Fyp + Fzm + frustum[p][3] > 0 ) - continue; - - Fzp = frustum[p][2] * zp; - if( Fxm + Fym + Fzp + frustum[p][3] > 0 ) - continue; - - if( Fxp + Fym + Fzp + frustum[p][3] > 0 ) - continue; - if( Fxm + Fyp + Fzp + frustum[p][3] > 0 ) - continue; - if( Fxp + Fyp + Fzp + frustum[p][3] > 0 ) - continue; - return false; - } - return true; -} - - - - -float minB[NUMDIM], maxB[NUMDIM]; /*box */ -float coord[NUMDIM]; /* hit point */ diff --git a/.svn/pristine/26/266314f603cf18978cb6caed1562f09a116a9d8c.svn-base b/.svn/pristine/26/266314f603cf18978cb6caed1562f09a116a9d8c.svn-base deleted file mode 100644 index d339451..0000000 --- a/.svn/pristine/26/266314f603cf18978cb6caed1562f09a116a9d8c.svn-base +++ /dev/null @@ -1,451 +0,0 @@ -/* - * 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, "Apex Angle : ", 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 *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.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"); - - 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/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base b/.svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base deleted file mode 100644 index 6a897c4..0000000 --- a/.svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base +++ /dev/null @@ -1,54 +0,0 @@ -if(WIN32) - add_library(pointfilter STATIC ../slam6d/pointfilter.cc) -else(WIN32) - add_library(pointfilter SHARED ../slam6d/pointfilter.cc) -endif(WIN32) - -set(SCANIO_LIBNAMES - uos uosr uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne -) - -if(WITH_RIVLIB) - set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} rxp) - if(LIBXML2_FOUND) - include_directories(${LIBXML2_INCLUDE_DIR}) -# set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} riegl_project) -# target_link_libraries(scan_io_riegl_project ${RIVLIB} scan_io_rxp ${LIBXML2_LIBRARIES}) - endif(LIBXML2_FOUND) -endif(WITH_RIVLIB) - -#IF (WITH_CAD) -# IF(NOT WIN32) -# add_library(scan_io_cad SHARED scan_io_cad.cc) -# target_link_libraries(scan_io_cad ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) -# ENDIF(NOT WIN32) -#ENDIF (WITH_CAD) - -foreach(libname ${SCANIO_LIBNAMES}) -if(WIN32) - #add_library(scan_io_${libname} STATIC scan_io_${libname}.cc) - add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) -else(WIN32) - add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) -endif(WIN32) - target_link_libraries(scan_io_${libname} pointfilter ${Boost_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) -endforeach(libname) - -if(WITH_RIVLIB) - target_link_libraries(scan_io_rxp ${RIVLIB}) - if(LIBXML2_FOUND) - target_link_libraries(scan_io_rxp ${LIBXML2_LIBRARIES}) #scan_io_riegl_project ${RIVLIB}) - endif(LIBXML2_FOUND) -endif(WITH_RIVLIB) - -if(WIN32) - add_library(scanio STATIC scan_io.cc ../slam6d/io_types.cc) -else(WIN32) - add_library(scanio SHARED scan_io.cc ../slam6d/io_types.cc) -endif(WIN32) - -if(UNIX) - target_link_libraries(scanio dl) -endif(UNIX) - - diff --git a/.svn/pristine/28/2878eac1a186f2e71956fd631c8f6434a75ca8da.svn-base b/.svn/pristine/28/2878eac1a186f2e71956fd631c8f6434a75ca8da.svn-base deleted file mode 100644 index 4fc7e85..0000000 --- a/.svn/pristine/28/2878eac1a186f2e71956fd631c8f6434a75ca8da.svn-base +++ /dev/null @@ -1,101 +0,0 @@ -/** - * @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/2b/2bf57854b6f6ed6517f40c82f222db3540d9ddc8.svn-base b/.svn/pristine/2b/2bf57854b6f6ed6517f40c82f222db3540d9ddc8.svn-base deleted file mode 100644 index b32aff8..0000000 --- a/.svn/pristine/2b/2bf57854b6f6ed6517f40c82f222db3540d9ddc8.svn-base +++ /dev/null @@ -1,199 +0,0 @@ -/* - * fbr_global implementation - * - * Copyright (C) HamidReza Houshiar - * - * Released under the GPL version 3. - * - */ - -#include "slam6d/fbr/fbr_global.h" -#include - -namespace fbr{ - string scanFormatToString(IOType format){ - return io_type_to_libname(format); - } - - IOType stringToScanFormat(string format){ - return formatname_to_io_type(format.c_str()); - } - - string projectionMethodToString(projection_method method){ - string sMethod; - switch(method){ - case EQUIRECTANGULAR: - sMethod = "EQUIRECTANGULAR"; - break; - case CYLINDRICAL: - sMethod = "CYLINDRICAL"; - break; - case MERCATOR: - sMethod = "MERCATOR"; - break; - case RECTILINEAR: - sMethod = "RECTILINEAR"; - break; - case PANNINI: - sMethod = "PANNINI"; - break; - case STEREOGRAPHIC: - sMethod = "STEREOGRAPHIC"; - break; - case ZAXIS: - sMethod = "ZAXIS"; - break; - default: - throw std::runtime_error(std::string("projection method ") + to_string(method) + std::string(" could not be matched to a projection method")); - } - return sMethod; - } - - projection_method stringToProjectionMethod(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(), "RECTILINEAR") == 0) return RECTILINEAR; - else if(strcasecmp(method.c_str(), "PANNINI") == 0) return PANNINI; - else if(strcasecmp(method.c_str(), "STEREOGRAPHIC") == 0) return STEREOGRAPHIC; - else if(strcasecmp(method.c_str(), "ZAXIS") == 0) return ZAXIS; - else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown")); - } - - string panoramaMapMethodToString(panorama_map_method method){ - string sMethod; - switch(method){ - case FARTHEST: - sMethod = "FARTHEST"; - break; - case EXTENDED: - sMethod = "EXTENDED"; - break; - default: - throw std::runtime_error(std::string("panorama map method ") + to_string(method) + std::string(" could not be matched to a panorama map method")); - } - return sMethod; - } - - panorama_map_method stringToPanoramaMapMethod(string method){ - if(strcasecmp(method.c_str(), "FARTHEST") == 0) return FARTHEST; - else if(strcasecmp(method.c_str(), "EXTENDED") == 0) return EXTENDED; - else throw std::runtime_error(std::string("panorama map method ") + method + std::string(" is unknown")); - } - - string featureDetectorMethodToString(feature_detector_method method){ - string sMethod; - switch(method){ - case SIFT_DET: - sMethod = "SIFT_DET"; - break; - case SURF_DET: - sMethod = "SURF_DET"; - break; - case ORB_DET: - sMethod = "ORB_DET"; - break; - case FAST_DET: - sMethod = "FAST_DET"; - break; - case STAR_DET: - sMethod = "STAR_DET"; - break; - default: - throw std::runtime_error(std::string("feature detector method ") + to_string(method) + std::string(" could not be matched to a feature detector method")); - } - return sMethod; - } - - feature_detector_method stringToFeatureDetectorMethod(string method){ - if(strcasecmp(method.c_str(), "SIFT") == 0) return SIFT_DET; - else if(strcasecmp(method.c_str(), "SURF") == 0) return SURF_DET; - else if(strcasecmp(method.c_str(), "ORB") == 0) return ORB_DET; - else if(strcasecmp(method.c_str(), "FAST") == 0) return FAST_DET; - else if(strcasecmp(method.c_str(), "STAR") == 0) return STAR_DET; - else throw std::runtime_error(std::string("feature detector method ") + method + std::string(" is unknown")); - } - - string featureDescriptorMethodToString(feature_descriptor_method method){ - string sMethod; - switch(method){ - case SIFT_DES: - sMethod = "SIFT_DES"; - break; - case SURF_DES: - sMethod = "SURF_DES"; - break; - case ORB_DES: - sMethod = "ORB_DES"; - break; - default: - throw std::runtime_error(std::string("feature descriptor method ") + to_string(method) + std::string(" could not be matched to a feature descriptor method")); - } - return sMethod; - } - - feature_descriptor_method stringToFeatureDescriptorMethod(string method){ - if(strcasecmp(method.c_str(), "SIFT") == 0) return SIFT_DES; - else if(strcasecmp(method.c_str(), "SURF") == 0) return SURF_DES; - else if(strcasecmp(method.c_str(), "ORB") == 0) return ORB_DES; - else throw std::runtime_error(std::string("feature descriptor method ") + method + std::string(" is unknown")); - } - - string matcherMethodToString(matcher_method method){ - string sMethod; - switch(method){ - case BRUTEFORCE: - sMethod = "BRUTEFORCE"; - break; - case FLANN: - sMethod = "FLANN"; - break; - case KNN: - sMethod = "KNN"; - break; - case RADIUS: - sMethod = "RADIUS"; - break; - case RATIO: - sMethod = "RATIO"; - break; - default: - throw std::runtime_error(std::string("matcher method ") + to_string(method) + std::string(" could not be matched to a matcher method")); - } - return sMethod; - } - - matcher_method stringToMatcherMethod(string method){ - if(strcasecmp(method.c_str(), "BRUTEFORCE") == 0) return BRUTEFORCE; - else if(strcasecmp(method.c_str(), "FLANN") == 0) return FLANN; - else if(strcasecmp(method.c_str(), "KNN") == 0) return KNN; - else if(strcasecmp(method.c_str(), "RADIUS") == 0) return RADIUS; - else if(strcasecmp(method.c_str(), "RATIO") == 0) return RATIO; - else throw std::runtime_error(std::string("matcher method ") + method + std::string(" is unknown")); - } - - string registrationMethodToString(registration_method method){ - string sMethod; - switch(method){ - case ALL: - sMethod = "ALL"; - break; - case RANSAC: - sMethod = "RANSAC"; - break; - case DISABLE: - sMethod = "DISABLE"; - break; - default: - throw std::runtime_error(std::string("registration method ") + to_string(method) + std::string(" could not be matched to a registration method")); - } - return sMethod; - } - - registration_method stringToRegistrationMethod(string method){ - if(strcasecmp(method.c_str(), "ALL") == 0) return ALL; - else if(strcasecmp(method.c_str(), "RANSAC") == 0) return RANSAC; - else if(strcasecmp(method.c_str(), "DISABLE") == 0) return DISABLE; - else throw std::runtime_error(std::string("registration method ") + method + std::string(" is unknown")); - } -} diff --git a/.svn/pristine/2e/2e1762223ed226d142512a9173b2f0f09d4ac842.svn-base b/.svn/pristine/2e/2e1762223ed226d142512a9173b2f0f09d4ac842.svn-base deleted file mode 100644 index 0a9eb9d..0000000 --- a/.svn/pristine/2e/2e1762223ed226d142512a9173b2f0f09d4ac842.svn-base +++ /dev/null @@ -1,212 +0,0 @@ -/* - * 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* 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/2e/2ed449ccbf7fa33992f53488365eebb1fb24647d.svn-base b/.svn/pristine/2e/2ed449ccbf7fa33992f53488365eebb1fb24647d.svn-base deleted file mode 100644 index 2a6144b..0000000 --- a/.svn/pristine/2e/2ed449ccbf7fa33992f53488365eebb1fb24647d.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/32/32250353bae1cc0945b11ff22f9065c0f50af294.svn-base b/.svn/pristine/32/32250353bae1cc0945b11ff22f9065c0f50af294.svn-base deleted file mode 100644 index 0f84079..0000000 --- a/.svn/pristine/32/32250353bae1cc0945b11ff22f9065c0f50af294.svn-base +++ /dev/null @@ -1,287 +0,0 @@ -/* - * 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* 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/32/325e477fa262d358d99bade889afebec00301298.svn-base b/.svn/pristine/32/325e477fa262d358d99bade889afebec00301298.svn-base deleted file mode 100644 index 99e5bb9..0000000 --- a/.svn/pristine/32/325e477fa262d358d99bade889afebec00301298.svn-base +++ /dev/null @@ -1,260 +0,0 @@ -/* - * 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_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 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_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_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(), 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(); - 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/88/88d7ba7604baa0151912d115b5883d7847f0a39d.svn-base b/.svn/pristine/33/333506af4f1aa220b56eba089abbbc9af8a8e67e.svn-base similarity index 77% rename from .svn/pristine/88/88d7ba7604baa0151912d115b5883d7847f0a39d.svn-base rename to .svn/pristine/33/333506af4f1aa220b56eba089abbbc9af8a8e67e.svn-base index 20773fc..50ee4df 100644 --- a/.svn/pristine/88/88d7ba7604baa0151912d115b5883d7847f0a39d.svn-base +++ b/.svn/pristine/33/333506af4f1aa220b56eba089abbbc9af8a8e67e.svn-base @@ -11,14 +11,14 @@ #include namespace fbr{ + string scanFormatToString(IOType format){ return io_type_to_libname(format); } - + IOType stringToScanFormat(string format){ return formatname_to_io_type(format.c_str()); - } - + } string projectionMethodToString(projection_method method){ string sMethod; switch(method){ @@ -200,4 +200,50 @@ namespace fbr{ else if(strcasecmp(method.c_str(), "DISABLE") == 0) return DISABLE; else throw std::runtime_error(std::string("registration method ") + method + std::string(" is unknown")); } + + string featureFiltrationMethodToString(feature_filtration_method method){ + string fFiltrationMethod; + switch(method){ + case OCCLUSION: + fFiltrationMethod = "OCCLUSION"; + break; + case STANDARD_DEVIATION: + fFiltrationMethod = "STANDARD_DEVIATION"; + break; + case DISABLE_FILTER: + fFiltrationMethod = "DISABLE_FILTER"; + break; + default: + throw std::runtime_error(std::string("feature filtration method ") + to_string(method) + std::string(" could not be matched to a feature filtration method")); + } + return fFiltrationMethod; + } + + feature_filtration_method stringToFeatureFiltrationMethod(string method){ + if(strcasecmp(method.c_str(), "OCCLUSION") == 0) return OCCLUSION; + else if(strcasecmp(method.c_str(), "STANDARD_DEVIATION") == 0) return STANDARD_DEVIATION; + else if(strcasecmp(method.c_str(), "DISABLE_FILTER") == 0) return DISABLE_FILTER; + else throw std::runtime_error(std::string("feature filtration method ") + method + std::string(" is unknown")); + } + + string matchingFiltrationMethodToString(matching_filtration_method method){ + string mFiltrationMethod; + switch(method){ + case FUNDEMENTAL_MATRIX: + mFiltrationMethod = "FUNDEMENTAL_MATRIX"; + break; + case DISABLE_MATCHING_FILTER: + mFiltrationMethod = "DISABLE_MATCHING_FILTER"; + break; + default: + throw std::runtime_error(std::string("matching filtration method ") + to_string(method) + std::string(" could not be matched to a matching filtration method")); + } + return mFiltrationMethod; + } + + matching_filtration_method stringToMatchingFiltrationMethod(string method){ + if(strcasecmp(method.c_str(), "FUNDEMENTAL_MATRIX") == 0) return FUNDEMENTAL_MATRIX; + else if(strcasecmp(method.c_str(), "DISABLE_MATCHING_FILTER") == 0) return DISABLE_MATCHING_FILTER; + else throw std::runtime_error(std::string("matching filtration method ") + method + std::string(" is unknown")); + } } diff --git a/.svn/pristine/3a/3aebe3063370637c79be880b9229b7a0506eec33.svn-base b/.svn/pristine/3a/3aebe3063370637c79be880b9229b7a0506eec33.svn-base deleted file mode 100644 index d5a48c3..0000000 --- a/.svn/pristine/3a/3aebe3063370637c79be880b9229b7a0506eec33.svn-base +++ /dev/null @@ -1,1101 +0,0 @@ -/* - * show_common implementation - * - * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann - * - * Released under the GPL version 3. - * - */ - -#ifdef WITH_GLEE -#include -#endif - -#include "show/show.h" -#include "show/show_Boctree.h" -#include "show/compacttree.h" -#include "show/NurbsPath.h" -#include "show/vertexarray.h" -#ifndef DYNAMIC_OBJECT_REMOVAL -#include "slam6d/scan.h" -#include "slam6d/managedScan.h" -#else -#include "veloslam/veloscan.h" -#endif -#include "glui/glui.h" /* Header File For The glui functions */ -#include -using std::ifstream; -#include -using std::exception; -#include - -#ifdef WITH_METRICS -#include "slam6d/metrics.h" -#endif - -#ifdef _MSC_VER -#include "XGetopt.h" -#else -#include -#endif - -#ifdef _MSC_VER -#define strcasecmp _stricmp -#define strncasecmp _strnicmp -#else -#include -#endif - - -#ifdef _MSC_VER -#if !defined _OPENMP && defined OPENMP -#define _OPENMP -#endif -#endif - -#ifdef _OPENMP -#include -#endif - -#include "slam6d/point_type.h" -#include "slam6d/io_utils.h" -#include "show/display.h" - -/** - * This vector contains the pointer to a vertex array for - * all colors (inner vector) and all scans (outer vector) - */ -vector< vector > vvertexArrayList; - -vector< ::SDisplay*> displays; -/** - * the octrees that store the points for each scan - */ -//Show_BOctTree **octpts; -vector octpts; -/** - * Storing the base directory - */ -string scan_dir; - -/** - * Storing the ID of the main windows - */ -int window_id; - -/** - * Size of points - */ -GLfloat pointsize = 1.0; -int anim_delay = 5; - -/** - * Select Color Buffer - */ -GLenum buffermode = GL_BACK; - -/** - * Indicator whether and how the drawing window - * has to be updated. - * - * haveToUpdate == 1 redisplay - * haveToUpdate == 2 reshape - * haveToUpdate == 3 animation scan matching - * haveToUpdate == 4 stop animation scan matching - * haveToUpdate == 6 path animation - * haveToUpdate == 7 force redisplay with all points - */ -int haveToUpdate = 0; - -/** - * Flag for invert the scene - */ -bool invert = true; - -/** - * Flag for indicating brid eyes view - */ -bool showTopView = false; - -/** - * Flag for idicating camera add mode - */ -bool addCameraView = false; //Is the view in add box mode? - -/** - * Storing the apex angle of the camera - */ -GLfloat cangle = 60.0; // Current camera opening mode -GLfloat cangle_old = cangle; - -/** - * Current rotation axis of the scene as quaternion - */ -GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; -GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; - -/** - * Current translation of the scene - */ -GLdouble X = 0.0, Y = 0.0, Z = 0.0; -GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; - -/** - * parallel zoom (similar to apex angle) for parallel projection - */ -GLfloat pzoom = 2000.0; -GLfloat pzoom_old = pzoom; - - -/** - * Mode of the fog (exp, exp2, linear) - */ -GLint fogMode = GL_EXP; - -/** - * Indicates if fog should be shown - */ -int show_fog = 1; - -/** - * Indicates if the points should be shown - */ -int show_points = 1; // Show data points in the viewer? - -/** - * Indicates if camera boxes should be shown - */ -int show_cameras = 1; // Show the camera boxes in the viewer? - -/** - * Indicates if camera path or robot path should be shown - */ -int show_path = 1; // Show the camera movement path ? - -/** - * Camera navigation by mouse or by panel - */ -int cameraNavMouseMode = 1; - -int mouseNavX, mouseNavY; -int mouseNavButton = -1; - -double mouseRotX = 0.0; -double mouseRotY = 0.0; -double mouseRotZ = 0.0; - -bool keymap[256]; - -//@@@ -//int animate_both = 0; // Animate both scan matchin and path? - -int frameNr = 0; - -/** - * Storing of all transformation (frames for animation) of all scans - */ -vector < vector > MetaMatrix; - -/** - * Storing of AlgoType for all frames - */ -vector < vector > MetaAlgoType; - -/** - * Window position - */ -int START_X = 0; -int START_Y = 0; -int START_WIDTH = 720; -int START_HEIGHT = 576; -GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio -bool advanced_controls = false; - -bool fullscreen = false; -int current_width = START_WIDTH; -int current_height = START_HEIGHT; - - -// the following values are scale dependant, i.e. all values are in m -float neardistance = 0.10; -double oldneardistance = 0.10; -float maxfardistance = 400.0; -double fardistance = 400.0; -double oldfardistance = 40000.0; -double movementSpeed = 0.1; -double defaultZoom = 20.0; -GLfloat fogDensity = 0.1; -double voxelSize = 0.20; - - -float adaption_rate = 1.0; -float LevelOfDetail = 0.0001; - - -// Defines for Point Semantic -#define TYPE_UNKNOWN 0x0000 -#define TYPE_OBJECT 0x0001 -#define TYPE_GROUND 0x0002 -#define TYPE_CEILING 0x0003 - -unsigned int cam_choice = 0; - -static unsigned int path_iterator = 0; -static int oldcamNavMode = 0; - -/** - * Animation sould be saved to file - */ -int save_animation = 0; -/** - * If true, interpolation for camera path is based on distance, else always - * the same number of images between two cameras. - */ -int inter_by_dist = 1; - -/**some variables for the camera path**/ -vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; -vector cams; -vector lookats; -vector ups; - -NurbsPath cam_nurbs_path; -char *path_file_name; -char *pose_file_name; - -/** Factor for saved image size */ -int factor = 1; - -/** - * program tries to have this framerate - */ -float idealfps = 20.0; -/** - * value of the listBox fo Color Value and Colormap - */ -int listboxColorVal = 0; -int listboxColorMapVal = 0; -int colorScanVal = 0; -ScanColorManager *cm; -float mincolor_value = 0.0; -float maxcolor_value = 0.0; -//unsigned int types = Point::USE_HEIGHT; -PointType pointtype; - -/** - * Contains the selected points for each scan - */ -set *selected_points; -/** - * Select single points? - */ -int select_voxels = 0; -/** - * Select or unselect points ? - */ -int selectOrunselect = 1; -/** octree depth for selecting groups of points */ -int selection_depth = 1; -int brush_size = 0; -char *selection_file_name; - -int current_frame = 0; -#include "show_menu.cc" -#include "show_animate.cc" -#include "show_gl.cc" - -/** - * Explains the usage of this program's command line parameters - * @param prog name of the program - */ -void usage(char* prog) -{ -#ifndef _MSC_VER - const string bold("\033[1m"); - const string normal("\033[m"); -#else - const string bold(""); - const string normal(""); -#endif - - cout << endl - << bold << "USAGE " << normal << endl - << " " << prog << " [options] directory" << endl << endl; - cout << bold << "OPTIONS" << normal << endl - - << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl - << " end after scan NR" << endl - << endl - << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl - << " using shared library F for input" << endl - << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl - << endl - << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl - << " will attempt to display points with a framerate of NR" << endl - << endl - << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << - "FILE" << endl - << " load objects specified in " << endl - << endl - << endl - << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl - << " neglegt all data points with a distance larger than NR 'units'" << endl - << endl - << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl - << " neglegt all data points with a distance smaller than NR 'units'" << endl - << endl - << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl - << " use randomized octree based point reduction (pts per voxel=)" << endl - << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl - << endl - << bold << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl - << " sets the starting and reset position to: " << endl - << " 0 = the origin of the coordinate system (default)" << endl - << " 1 = the position of the first scan (default if --origin is in argument list)" << endl - << " 2 = the center of the first scan" << endl - << endl - << bold << " -S, --scanserver" << normal << endl - << " Use the scanserver as an input method and handling of scan data" << endl - << endl - << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl - << " turns on octree based point reduction (voxel size=)" << endl - << endl - << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl - << " start at scan NR (i.e., neglects the first NR scans)" << endl - << " [ATTENTION: counting naturally starts with 0]" << endl - << endl - << bold << " -C" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl - << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl - << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl - << " " << endl - << endl - - << bold << " -R, --reflectance, --reflectivity" << normal << endl - << " use reflectivity values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -a, --amplitude" << endl << normal - << " use amplitude values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -d, --deviation" << endl << normal - << " use amplitude values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -h, --height" << endl << normal - << " use y-values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -T, --type" << endl << normal - << " use type values for coloring point clouds" << endl - << " only works when using octree display" << endl - << bold << " -c, --color" << endl << normal - << " use color RGB values for coloring point clouds" << endl - << bold << " --saveOct" << endl << normal - << " stores all used scans as octrees in the given directory" << endl - << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl - << " only works when using octree display" << endl - << bold << " --loadOct" << endl << normal - << " only reads octrees from the given directory" << endl - << " All reflectivity/amplitude/deviation/type settings are read from file." << endl - << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl - << " only works when using octree display" << endl - << endl << endl; - - exit(1); -} - -/** - * A function that parses the command-line arguments and sets the respective flags. - * - * @param argc the number of arguments - * @param argv the arguments - * @param dir parsing result - the directory - * @param start parsing result - starting at scan number 'start' - * @param end parsing result - stopping at scan number 'end' - * @param maxDist parsing result - maximal distance - * @param minDist parsing result - minimal distance - * @param readInitial parsing result - read a file containing a initial transformation matrix - * @param type parsing result - file format to be read - * @return 0, if the parsing was successful, 1 otherwise - */ -int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, - double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, - bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver) -{ - unsigned int types = PointType::USE_NONE; - start = 0; - end = -1; // -1 indicates no limitation - maxDist = -1; // -1 indicates no limitation - int c; - // from unistd.h - extern char *optarg; - extern int optind; - - WriteOnce w_type(type); - WriteOnce w_start(start), w_end(end); - - cout << endl; - static struct option longopts[] = { - { "origin", optional_argument, 0, 'o' }, - { "format", required_argument, 0, 'f' }, - { "fps", required_argument, 0, 'F' }, - { "scale", required_argument, 0, 'S' }, - { "start", required_argument, 0, 's' }, - { "end", required_argument, 0, 'e' }, - { "reduce", required_argument, 0, 'r' }, - { "max", required_argument, 0, 'm' }, - { "min", required_argument, 0, 'M' }, - { "octree", optional_argument, 0, 'O' }, - { "reflectance", no_argument, 0, 'R' }, - { "reflectivity", no_argument, 0, 'R' }, - { "amplitude", no_argument, 0, 'a' }, - { "deviation", no_argument, 0, 'd' }, - { "height", no_argument, 0, 'h' }, - { "type", no_argument, 0, 'T' }, - { "color", no_argument, 0, 'c' }, - { "loadObj", required_argument, 0, 'l' }, - { "saveOct", no_argument, 0, '0' }, - { "loadOct", no_argument, 0, '1' }, - { "advanced", no_argument, 0, '2' }, - { "scanserver", no_argument, 0, 'S' }, - { 0, 0, 0, 0} // needed, cf. getopt.h - }; - - while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRadhTc", longopts, NULL)) != -1) { - switch (c) { - case 's': - w_start = atoi(optarg); - if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } - break; - case 'e': - w_end = atoi(optarg); - if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } - if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } - break; - case 'm': - maxDist = atoi(optarg); - break; - case 'M': - minDist = atoi(optarg); - break; - case 'r': - red = atof(optarg); - break; - case 't': - readInitial = true; - break; - case 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } - break; - case 'f': - try { - w_type = formatname_to_io_type(optarg); - } catch (...) { // runtime_error - cerr << "Format " << optarg << " unknown." << endl; - abort(); - } - break; - case '?': - usage(argv[0]); - return 1; - case 'R': - types |= PointType::USE_REFLECTANCE; - break; - case 'a': - types |= PointType::USE_AMPLITUDE; - break; - case 'd': - types |= PointType::USE_DEVIATION; - break; - case 'h': - types |= PointType::USE_HEIGHT; - break; - case 'T': - types |= PointType::USE_TYPE; - break; - case 'c': - types |= PointType::USE_COLOR; - break; - case 'F': - fps = atof(optarg); - break; - case 'C': - scale = atof(optarg); - break; - case 'S': - scanserver = true; - break; - case 'o': - if (optarg) { - origin = atoi(optarg); - } else { - origin = 1; - } - break; - case '0': - saveOct = true; - break; - case '1': - loadOct = true; - break; - case 'l': - loadObj = optarg; - break; - case '2': - advanced_controls = true; - break; - default: - abort (); - } - } - - if (optind != argc-1) { - cerr << "\n*** Directory missing ***" << endl; - usage(argv[0]); - } - dir = argv[optind]; - -#ifndef _MSC_VER - if (dir[dir.length()-1] != '/') dir = dir + "/"; -#else - if (dir[dir.length()-1] != '\\') dir = dir + "\\"; -#endif - - parseFormatFile(dir, w_type, w_start, w_end); - - ptype = PointType(types); - return 0; -} - -void setResetView(int origin) { - if (origin == 1) { - double *transmat = MetaMatrix[0].back(); - cout << transmat << endl; - - RVX = -transmat[12]; - RVY = -transmat[13]; - RVZ = -transmat[14]; - Matrix4ToQuat(transmat, Rquat); - X = RVX; - Y = RVY; - Z = RVZ; - quat[0] = Rquat[0]; - quat[1] = Rquat[1]; - quat[2] = Rquat[2]; - quat[3] = Rquat[3]; - } else if (origin == 2) { - double center[3]; -#ifdef USE_COMPACT_TREE - ((compactTree*)octpts[0])->getCenter(center); -#else - ((Show_BOctTree*)octpts[0])->getCenter(center); -#endif - RVX = -center[0]; - RVY = -center[1]; - RVZ = -center[2]; - X = RVX; - Y = RVY; - Z = RVZ; - } -} - -/* - * A function that read the .frame files created by slam6D - * - * @param dir the directory - * @param start starting at scan number 'start' - * @param end stopping at scan number 'end' - * @param read a file containing a initial transformation matrix and apply it - */ -int readFrames(string dir, int start, int end, bool readInitial, IOType &type) -{ - - // convert to OpenGL coordinate system - double mirror[16]; - M4identity(mirror); - mirror[10] = -1.0; - - double initialTransform[16]; - if (readInitial) { - cout << "Initial Transform:" << endl; - string initialTransformFileName = dir + "initital.frame"; - ifstream initial_in(initialTransformFileName.c_str()); - if (!initial_in.good()) { - cout << "Error opening " << initialTransformFileName << endl; - exit(-1); - } - initial_in >> initialTransform; - cout << initialTransform << endl; - - // update the mirror to apply the initial frame for all frames - double tempxf[16]; - MMult(mirror, initialTransform, tempxf); - memcpy(mirror, tempxf, sizeof(tempxf)); - } - - for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { - const double* transformation; - Scan::AlgoType algoType; - vector Matrices; - vector algoTypes; - - // iterate over frames (stop if none were created) and pull/convert the frames into local containers - unsigned int frame_count; - try { - frame_count = (*it)->readFrames(); - } catch(std::ios_base::failure& e) { - break; - } - for(unsigned int i = 0; i < frame_count; ++i) { - (*it)->getFrame(i, transformation, algoType); - double* transMatOpenGL = new double[16]; - - // apply mirror to convert (and initial frame if requested) the frame and save in opengl - MMult(mirror, transformation, transMatOpenGL); - - Matrices.push_back(transMatOpenGL); - algoTypes.push_back(algoType); - } - - MetaAlgoType.push_back(algoTypes); - MetaMatrix.push_back(Matrices); - - if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && it == Scan::allScans.begin()) { - MetaAlgoType.push_back(algoTypes); - MetaMatrix.push_back(Matrices); - } - current_frame = MetaMatrix.back().size() - 1; - } - - if (MetaMatrix.size() == 0) { - cerr << "*****************************************" << endl; - cerr << "** ERROR: No .frames could be found! **" << endl; - cerr << "*****************************************" << endl; - cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; - return -1; - } - return 0; -} - -void generateFrames(int start, int end, bool identity) { - if (identity) { - cout << "using Identity for frames " << endl; - } else { - cout << "using pose information for frames " << endl; - } - int fileCounter = start; - int index = 0; - for (;;) { - if (fileCounter > end) break; // 'nuf read - fileCounter++; - - vector Matrices; - vector algoTypes; - - for (int i = 0; i < 3; i++) { - double *transMat = new double[16]; - - if (identity) { - M4identity(transMat); - transMat[10] = -1.0; - } else { - EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); - } - - Matrices.push_back(transMat); - algoTypes.push_back(Scan::ICP); - - } - index++; - MetaAlgoType.push_back(algoTypes); - MetaMatrix.push_back(Matrices); - current_frame = MetaMatrix.back().size() - 1; - } -} - - - -/* - * create display lists - * @to do general framework for color & type definitions - */ -void createDisplayLists(bool reduced) -{ - for(unsigned int s = 0; s < Scan::allScans.size() ; s++) { - Scan* scan = Scan::allScans[s]; - - vertexArray* myvertexArray1; - vertexArray* myvertexArray2; - - // count points - unsigned int color1 = 0, color2 = 0; - if(!reduced) { - scan->get(DATA_XYZ | DATA_TYPE); - DataType type(scan->get("type")); - - if(type.valid()) { - for(unsigned int i = 0; i < type.size(); ++i) { - if(type[i] & TYPE_GROUND) { - color1 += 3; - } else { - color2 += 3; - } - } - } else { - color2 = 3 * scan->size("xyz"); - } - - myvertexArray1 = new vertexArray(color1); - myvertexArray2 = new vertexArray(color2); - - color1 = 0; color2 = 0; - - DataXYZ xyz(scan->get("xyz")); - for(unsigned int i = 0; i < xyz.size(); ++i) { - if(type[i] & TYPE_GROUND) { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray1->array[color1++] = xyz[i][j]; - } - } else { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray2->array[color2++] = xyz[i][j]; - } - } - } - } else { - color2 = 3 * scan->size("xyz reduced"); - - myvertexArray1 = new vertexArray(0); - myvertexArray2 = new vertexArray(color2); - - color2 = 0; - - DataXYZ xyz_r(scan->get("xyz reduced")); - for(unsigned int i = 0; i < xyz_r.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray2->array[color2++] = xyz_r[i][j]; - } - } - } - - - - glNewList(myvertexArray1->name, GL_COMPILE); - //@ - //glColor4d(0.44, 0.44, 0.44, 1.0); - //glColor4d(0.66, 0.66, 0.66, 1.0); - glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array); - glEnableClientState(GL_VERTEX_ARRAY); - glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender); - glDisableClientState(GL_VERTEX_ARRAY); - glEndList(); - - glNewList(myvertexArray2->name, GL_COMPILE); - //glColor4d(1.0, 1.0, 1.0, 1.0); - //glColor4d(0.0, 0.0, 0.0, 1.0); - glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array); - glEnableClientState(GL_VERTEX_ARRAY); - glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender); - glDisableClientState(GL_VERTEX_ARRAY); - glEndList(); - - // append to vector - vector vvertexArray; - vvertexArray.push_back(myvertexArray1); - vvertexArray.push_back(myvertexArray2); - vvertexArrayList.push_back(vvertexArray); - } -} - -void cycleLOD() { - LevelOfDetail = 0.00001; - for (unsigned int i = 0; i < octpts.size(); i++) - octpts[i]->cycleLOD(); -} - - -void initShow(int argc, char **argv){ - - /***************/ - /* init OpenGL */ - /***************/ - glutInit(&argc,argv); - - cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl - << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl - << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; - - if(argc <= 1){ - usage(argv[0]); - } - - double red = -1.0; - int start = 0, end = -1, maxDist = -1, minDist = -1; - string dir; - bool readInitial = false; - IOType type = UOS; - int octree = 0; - bool loadOct = false; - bool saveOct = false; - string loadObj; - int origin = 0; - double scale = 0.01; // in m - bool scanserver = false; - - pose_file_name = new char[1024]; - path_file_name = new char[1024]; - selection_file_name = new char[1024]; - - strncpy(pose_file_name, "pose.dat", 1024); - strncpy(path_file_name, "path.dat", 1024); - strncpy(selection_file_name, "selected.3d", 1024); - - parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, - octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, type, scanserver); - - // modify all scale dependant variables - scale = 1.0 / scale; - movementSpeed *= scale; - neardistance *= scale; - oldneardistance *= scale; - maxfardistance *= scale; - fardistance *= scale; - fogDensity /= scale; - defaultZoom *= scale; - voxelSize *= scale; -// oldfardistance *= scale; - - //////////////////////// - SDisplay::readDisplays(loadObj, displays); - //////////////////// - - if (type == OCT) { - loadOct = true; - } - - // if we want to load display file get pointtypes from the files first - if(loadOct) { - string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; - cout << "Getting point information from " << scanFileName << endl; - cout << "Attention! All subsequent oct-files must be of the same type!" << endl; - - pointtype = BOctTree::readType(scanFileName); - } - scan_dir = dir; - - // init and create display - M4identity(view_rotate_button); - obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; - - // Loading scans, reducing, loading frames and generation if neccessary - - // load all available scans - Scan::openDirectory(scanserver, dir, type, start, end); - - if(Scan::allScans.size() == 0) { - cerr << "No scans found. Did you use the correct format?" << endl; - exit(-1); - } - - for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { - Scan* scan = *it; - scan->setRangeFilter(maxDist, minDist); - if(red > 0) { - // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time - if(scanserver) { - dynamic_cast(scan)->setShowReductionParameter(red, octree); - } else { - scan->setReductionParameter(red, octree); - } - } - } - cm = new ScanColorManager(4096, pointtype); - -#ifdef USE_GL_POINTS // use octtrees -#ifdef USE_COMPACT_TREE - cout << "Creating compact display octrees.." << endl; -#else - cout << "Creating display octrees.." << endl; -#endif - - if(loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; - - // for managed scans the input phase needs to know how much it can handle - std::size_t free_mem = 0; - if(scanserver) - free_mem = ManagedScan::getMemorySize(); - - for(unsigned int i = 0; i < Scan::allScans.size(); ++i) { - Scan* scan = Scan::allScans[i]; - - // create data structures -#ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed - compactTree* tree; - try { - if (red > 0) { // with reduction, only xyz points - DataXYZ xyz_r(scan->get("xyz reduced show")); - tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); - } else { // without reduction, xyz + attribute points - sfloat** pts = pointtype.createPointArray(scan); - unsigned int nrpts = scan->size("xyz"); - tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); - for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; - } - } catch(...) { - cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; - break; - } -#else // FIXME: remove the case above - scan->setOcttreeParameter(red, voxelSize, pointtype, loadOct, saveOct); - - DataOcttree* data_oct; - try { - data_oct = new DataOcttree(scan->get("octtree")); - } catch(runtime_error& e) { - cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; - break; - } - BOctTree* btree = &(data_oct->get()); - unsigned int tree_size = btree->getMemorySize(); - - if(scanserver) { - // check if the octtree would actually fit with all the others - if(tree_size > free_mem) { - delete data_oct; - cout << "Stopping at scan " << i << ", no more octtrees could fit in memory." << endl; - break; - } else { - // subtract available memory - free_mem -= tree_size; - } - } -#endif //FIXME: COMPACT_TREE - -#if !defined USE_COMPACT_TREE - // show structures - // associate show octtree with the scan and hand over octtree pointer ownership - Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); - - // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points - tree->unlockCachedTree(); -#endif - - // octtrees have been created successfully - octpts.push_back(tree); - - // print something -#ifdef USE_COMPACT_TREE // TODO: change compact tree for memory footprint output, remove this case - cout << "Scan " << i << " octree finished." << endl; -#else - cout << "Scan " << i << " octree finished ("; - bool space = false; - if(tree_size/1024/1024 > 0) { - cout << tree_size/1024/1024 << "M"; - space = true; - } - if((tree_size/1024)%1024 > 0) { - if(space) cout << " "; - cout << (tree_size/1024)%1024 << "K"; - space = true; - } - if(tree_size%1024 > 0) { - if(space) cout << " "; - cout << tree_size%1024 << "B"; - } - cout << ")." << endl; -#endif - } - -/* -TODO: to maximize space for octtrees, implement a heuristic to remove all -CacheObjects sequentially from the start and pack octtrees one after another -until it reaches the maximum size allowed, resetting the index, but causing the -first to be locked again and stopping by catching the exception -set heuristic, do locking, catch exception, reset heuristic to default or old -*/ -#if !defined USE_COMPACT_TREE - if(scanserver) { - // activate all octtrees until they don't fit anymore - cout << "Locking managed octtrees in memory " << flush; - bool stop = false; - unsigned int loaded = 0; - unsigned int dots = (octpts.size() / 20); - if(dots == 0) dots = 1; - vector::iterator it_remove_first = octpts.end(); - for(vector::iterator it = octpts.begin(); it != octpts.end(); ++it) { - if(!stop) { - // try to lock the octtree in cache - try { - Show_BOctTree* stree = dynamic_cast*>(*it); - stree->lockCachedTree(); - loaded++; - if(loaded % dots == 0) cout << '.' << flush; - } catch(runtime_error& e) { - stop = true; - it_remove_first = it; - } - } - if(stop) { - // delete the octtree, resize after iteration - delete *it; - } - } - // now remove iterators for deleted octtrees - if(stop) octpts.erase(it_remove_first, octpts.end()); - cout << ' ' << loaded << " octtrees loaded." << endl; - } - -#endif // !COMPACT_TREE - -#else // not using octtrees - createDisplayLists(red > 0); -#endif // USE_GL_POINTS - - // load frames now that we know how many scans we actually loaded - unsigned int real_end = min((unsigned int)(end), - (unsigned int)(start + octpts.size() - 1)); - if(readFrames(dir, start, real_end, readInitial, type)) - generateFrames(start, real_end, true); - - cm->setCurrentType(PointType::USE_HEIGHT); - //ColorMap cmap; - //cm->setColorMap(cmap); - resetMinMax(0); - - selected_points = new set[octpts.size()]; - - // sets (and computes if necessary) the pose that is used for the reset button - setResetView(origin); - - for (unsigned int i = 0; i < 256; i++) { - keymap[i] = false; - } -} - -void deinitShow() -{ - static volatile bool done = false; - if(done) return; - done = true; - - cout << "Cleaning up octtrees and scans." << endl; - if(octpts.size()) { - // delete octtrees to release the cache locks within - for(vector::iterator it = octpts.begin(); it!= octpts.end(); ++it) { - delete *it; - } - } - - Scan::closeDirectory(); -} - -/** - * Global program scope destructor workaround to clean up data regardless of - * the way of program exit. - */ -struct Deinit { ~Deinit() { deinitShow(); } } deinit; diff --git a/.svn/pristine/3f/3febf05b1fb33b70feb950db724d9edc7b2c0924.svn-base b/.svn/pristine/3f/3febf05b1fb33b70feb950db724d9edc7b2c0924.svn-base deleted file mode 100644 index 2e6c53c..0000000 --- a/.svn/pristine/3f/3febf05b1fb33b70feb950db724d9edc7b2c0924.svn-base +++ /dev/null @@ -1,2040 +0,0 @@ -/* - * 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_AMPLITUDE); - break; - case 3: - cm->setCurrentType(PointType::USE_DEVIATION); - break; - case 4: - cm->setCurrentType(PointType::USE_TYPE); - break; - case 5: - 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; - - 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; - 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/40/40986fa5eb03ba3679675bb0c5c956a8bd67fd57.svn-base b/.svn/pristine/40/40986fa5eb03ba3679675bb0c5c956a8bd67fd57.svn-base deleted file mode 100644 index f24dc7c..0000000 --- a/.svn/pristine/40/40986fa5eb03ba3679675bb0c5c956a8bd67fd57.svn-base +++ /dev/null @@ -1,113 +0,0 @@ -/** - * @file - * @brief IO filtering class for ScanIO to discard unwanted points. - * - * @author Thomas Escher - */ - -#ifndef POINT_FILTER_H -#define POINT_FILTER_H - -#include -#include - -class Checker; - - -/** - * Flexible filtering class for parsing a set of points. - * - * This class is configurable with parameters for range and height and can be transferred via the use of a parameter string. - * Use on a point set via repeated use of the check function, which creates the internal Checker structures once for each change to the parameters. The amount of tests is held as minimal as possible. - */ -class PointFilter -{ -public: - // Default empty constructor - PointFilter(); - //! Deserialization constructor, forms parameters back from a string given by getParams - PointFilter(const std::string& params); - ~PointFilter(); - - PointFilter& setRange(float maxDist, float minDist); - PointFilter& setHeight(double top, double bottom); - - //! Serialization function to convert it into a string, usable in the constructor - std::string getParams(); - - //! Check a point, returning success if all contained Checker functions accept that point (implemented in .icc) - inline bool check(double* point); -private: - //! Storage for parameter keys and values - std::map m_params; - - //! If either not created yet or parameters get changed, this flag will cause check to create a new Checker chain - bool m_changed; - - //! created in the first check call with the changed flag set - Checker* m_checker; - - //! Allocation of the checkers - void createCheckers(); - - // factory magic - template friend struct CheckerFactory; - static std::map factory; -}; - -class Checker { -public: - Checker(); - ~Checker(); - - //! Testing function - virtual bool test(double* point) = 0; - - //! Next test in chain - Checker* m_next; -}; - -//! Factory integrating the create-function and adding it to the lookup map in an easy template -template -struct CheckerFactory { - //! Instanciate in the source code with the to be created class as template argument and associated key string as constructor argument - CheckerFactory(const std::string& key) { PointFilter::factory[key] = CheckerFactory::create; } - //! Automated create function, safely returning a nullpointer if the constructor throws for unwanted values - static Checker* create(const std::string& value) { try { return new T(value); } catch(...) { return 0; } } -}; - -class CheckerRangeMax : public Checker { -public: - CheckerRangeMax(const std::string& value); - virtual bool test(double* point); -private: - double m_max; -}; - -class CheckerRangeMin : public Checker { -public: - CheckerRangeMin(const std::string& value); - virtual bool test(double* point); -private: - double m_min; -}; - -class CheckerHeightTop : public Checker { -public: - CheckerHeightTop(const std::string& value); - virtual bool test(double* point); -private: - double m_top; -}; - -class CheckerHeightBottom : public Checker { -public: - CheckerHeightBottom(const std::string& value); - virtual bool test(double* point); -private: - double m_bottom; -}; - -#include "scanserver/pointfilter.icc" - -#endif //POINT_FILTER_H diff --git a/.svn/pristine/45/45a8678ada1c9c0d4a7d5ec515a6aa4d87f50ac2.svn-base b/.svn/pristine/45/45a8678ada1c9c0d4a7d5ec515a6aa4d87f50ac2.svn-base deleted file mode 100644 index cb9f13d..0000000 --- a/.svn/pristine/45/45a8678ada1c9c0d4a7d5ec515a6aa4d87f50ac2.svn-base +++ /dev/null @@ -1,373 +0,0 @@ -/* - * 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 == "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/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base b/.svn/pristine/45/45e2e25572e2899c2bcc3e7f1bdd3c3604393dd7.svn-base similarity index 97% rename from .svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base rename to .svn/pristine/45/45e2e25572e2899c2bcc3e7f1bdd3c3604393dd7.svn-base index 93ede82..49bed21 100644 --- a/.svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base +++ b/.svn/pristine/45/45e2e25572e2899c2bcc3e7f1bdd3c3604393dd7.svn-base @@ -238,6 +238,13 @@ DataXYZ SharedScan::createXYZReduced(unsigned int size) { return m_xyz_reduced->createCacheData(size*3*sizeof(double)); } + +DataReflectance SharedScan::createReflectance(unsigned int size) { + // size is in units of double[1], scale to bytes + return m_reflectance->createCacheData(size*1*sizeof(double)); +} + + DataXYZ SharedScan::getXYZReducedOriginal() { return m_xyz_reduced_original->getCacheData(); } diff --git a/.svn/pristine/4b/4b27fd327b1d20021abb2011400cffc1f9cfa752.svn-base b/.svn/pristine/4b/4b27fd327b1d20021abb2011400cffc1f9cfa752.svn-base deleted file mode 100644 index d7cdcca..0000000 --- a/.svn/pristine/4b/4b27fd327b1d20021abb2011400cffc1f9cfa752.svn-base +++ /dev/null @@ -1,117 +0,0 @@ -/* - * feature implementation - * - * Copyright (C) HamidReza Houshiar - * - * Released under the GPL version 3. - * - */ - -#include "slam6d/fbr/feature.h" - -using namespace std; - -namespace fbr{ - - feature::feature(){ - fDetectorMethod = SIFT_DET; - fDescriptorMethod = SIFT_DES; - } - - void feature::featureDetection(cv::Mat pImage, feature_detector_method method){ - //Detect the keypoints using SURF Detector - if(fDetectorMethod == SURF_DET){ - double minHessian = 400; - cv::SurfFeatureDetector detector(minHessian); - detector.detect(pImage, keypoints); - } - - //Detect the keypoints using SIFT Detector - if(fDetectorMethod == SIFT_DET){ - cv::SiftFeatureDetector detector; - detector.detect(pImage, keypoints); - } - - //Detect the keypoints using ORB Detector - if(fDetectorMethod == ORB_DET){ - cv::OrbFeatureDetector detector; - detector.detect(pImage, keypoints); - } - - //Detect the keypoints using FAST Detector - if(fDetectorMethod == FAST_DET){ - cv::FastFeatureDetector detector; - detector.detect(pImage, keypoints); - } - - //Detect the keypoints using STAR Detector - if(fDetectorMethod == STAR_DET){ - cv::StarFeatureDetector detector; - detector.detect(pImage, keypoints); - } - } - - void feature::featureDetection(cv::Mat pImage){ - featureDetection(pImage, fDetectorMethod); - } - - void feature::featureDescription(cv::Mat pImage, feature_descriptor_method method){ - - if(keypoints.size() == 0) - featureDetection(pImage); - - //Create descriptor using SURF - if(fDescriptorMethod == SURF_DES){ - cv::SurfDescriptorExtractor extractor; - extractor.compute(pImage, keypoints, descriptors); - } - - //Create descriptor using SIFT - if(fDescriptorMethod == SIFT_DES){ - cv::SiftDescriptorExtractor extractor; - extractor.compute(pImage, keypoints, descriptors); - } - - //Create descriptor using ORB - if(fDescriptorMethod == ORB_DES){ - cv::OrbDescriptorExtractor extractor; - extractor.compute(pImage, keypoints, descriptors); - } - } - - void feature::featureDescription(cv::Mat pImage){ - featureDescription(pImage, fDescriptorMethod); - } - - feature_detector_method feature::getDetectorMethod(){ - return fDetectorMethod; - } - - feature_descriptor_method feature::getDescriptorMethod(){ - return fDescriptorMethod; - } - - //check for the keypoints vector not to be empty - vector feature::getFeatures(){ - return keypoints; - } - - //check for the descriptor Mat not to be empty - cv::Mat feature::getDescriptors(){ - return descriptors; - } - - void feature::getDescription(description_method method){ - if(method == FEATURE_DESCRIPTION) - cout<<"fDetectorMethod: "< readDirectory(const char* dir_path, unsigned int start, unsigned int end); - virtual void readPose(const char* dir_path, const char* identifier, double* pose); - virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/4e/4e804d0103295031b87d4267c45d1fd00445ad1b.svn-base b/.svn/pristine/4e/4e804d0103295031b87d4267c45d1fd00445ad1b.svn-base deleted file mode 100644 index b14902a..0000000 --- a/.svn/pristine/4e/4e804d0103295031b87d4267c45d1fd00445ad1b.svn-base +++ /dev/null @@ -1,187 +0,0 @@ -/* - * pointfilter implementation - * - * Copyright (C) Thomas Escher, Dorit Borrmann, Kai Lingemann - * - * Released under the GPL version 3. - * - */ - -#include "scanserver/pointfilter.h" - -using std::string; -using std::map; -#include -using std::stringstream; -#include -using std::runtime_error; - -#include -using std::cout; -using std::endl; - - - -map PointFilter::factory; - - - -PointFilter::PointFilter() : - m_changed(true), m_checker(0) -{ -} - -PointFilter::PointFilter(const std::string& params) : - m_changed(true), m_checker(0) -{ - size_t start = 0, end = string::npos; - while((end = params.find(' ', start)) != string::npos) { - // extract the word (start-end+1) without the space (-1) - string key(params.substr(start, start-end)); - end++; - // get the second word position - start = params.find(' ', end); - // insert - m_params[key] = params.substr(end, (start-end)); - // advance to the character after space - if(start != string::npos) - start++; - else - break; - } -} - -PointFilter::~PointFilter() -{ - if(m_checker) - delete m_checker; -} - -PointFilter& PointFilter::setRange(float maxDist, float minDist) -{ - m_changed = true; - stringstream s_max; s_max << maxDist; - stringstream s_min; s_min << minDist; - m_params["rangemax"] = s_max.str(); - m_params["rangemin"] = s_min.str(); - return *this; -} - -PointFilter& PointFilter::setHeight(double top, double bottom) -{ - m_changed = true; - stringstream s_top; s_top << top; - stringstream s_bottom; s_bottom << bottom; - m_params["heighttop"] = s_top.str(); - m_params["heightbottom"] = s_bottom.str(); - return *this; -} - -std::string PointFilter::getParams() -{ - stringstream s; - for(map::iterator it = m_params.begin(); it != m_params.end(); ++it) { - s << (*it).first << " " << (*it).second << " "; - } - return s.str(); -} - -void PointFilter::createCheckers() -{ - // delete the outdated ones - if(m_checker) { - delete m_checker; - m_checker = 0; - } - - // create new ones - Checker** current = &m_checker; - for(map::iterator it = m_params.begin(); it != m_params.end(); ++it) { - *current = factory[it->first](it->second); - // if a Checker has been successfully created advance to its pointer in the chain - if(*current) { - current = &((*current)->m_next); - } - } -} - - - -Checker::Checker() : - m_next(0) -{ -} - -Checker::~Checker() -{ - if(m_next) - delete m_next; -} - - - -// create factory instaces for key string to factory function mapping -namespace { - CheckerFactory max("rangemax"); - CheckerFactory min("rangemin"); - CheckerFactory top("heighttop"); - CheckerFactory bottom("heightbottom"); -} - - - -CheckerRangeMax::CheckerRangeMax(const std::string& value) { - stringstream s(value); - s >> m_max; - // default value: no check - if(m_max <= 0.0) throw runtime_error("No range filter needed."); - m_max *= m_max; -} - -bool CheckerRangeMax::test(double* point) { - if(point[0]*point[0] + point[1]*point[1] + point[2]*point[2] < m_max) - return true; - return false; -} - - - -CheckerRangeMin::CheckerRangeMin(const std::string& value) { - stringstream s(value); - s >> m_min; - // default value: no check - if(m_min <= 0.0) throw runtime_error("No range filter needed."); - m_min *= m_min; -} - -bool CheckerRangeMin::test(double* point) { - if(point[0]*point[0] + point[1]*point[1] + point[2]*point[2] > m_min) - return true; - return false; -} - - - -CheckerHeightTop::CheckerHeightTop(const std::string& value) { - stringstream s(value); - s >> m_top; -} - -bool CheckerHeightTop::test(double* point) { - if(point[1] < m_top) - return true; - return false; -} - - - -CheckerHeightBottom::CheckerHeightBottom(const std::string& value) { - stringstream s(value); - s >> m_bottom; -} - -bool CheckerHeightBottom::test(double* point) { - if(point[1] > m_bottom) - return true; - return false; -} diff --git a/.svn/pristine/51/510ce7ed25ff56a932fc7c5485d244c0abd8dd80.svn-base b/.svn/pristine/51/510ce7ed25ff56a932fc7c5485d244c0abd8dd80.svn-base deleted file mode 100644 index b94b316..0000000 --- a/.svn/pristine/51/510ce7ed25ff56a932fc7c5485d244c0abd8dd80.svn-base +++ /dev/null @@ -1,398 +0,0 @@ -/* - * 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 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, - &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_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 == "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/51/51f8034e47c326f2f7d893942aeacdf616bbb23a.svn-base b/.svn/pristine/51/51f8034e47c326f2f7d893942aeacdf616bbb23a.svn-base deleted file mode 100644 index 1a5fb6a..0000000 --- a/.svn/pristine/51/51f8034e47c326f2f7d893942aeacdf616bbb23a.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/52/5236522fa5d1061da94980ae6be0cd497574e062.svn-base b/.svn/pristine/52/5236522fa5d1061da94980ae6be0cd497574e062.svn-base deleted file mode 100644 index fc947fb..0000000 --- a/.svn/pristine/52/5236522fa5d1061da94980ae6be0cd497574e062.svn-base +++ /dev/null @@ -1,602 +0,0 @@ -/* - * 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); - } - } - } - - 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: "< -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(); -} - -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/46/461046f9b09c0819a22b163755769774c6596509.svn-base b/.svn/pristine/54/54c5247d90fb88aedce1de8a13a10c0a576a83c0.svn-base similarity index 61% rename from .svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base rename to .svn/pristine/54/54c5247d90fb88aedce1de8a13a10c0a576a83c0.svn-base index 03e0264..1353805 100644 --- a/.svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base +++ b/.svn/pristine/54/54c5247d90fb88aedce1de8a13a10c0a576a83c0.svn-base @@ -5,6 +5,8 @@ add_library(fbr_cv_io STATIC ${FBR_IO_SRC}) SET(FBR_PANORAMA_SRC panorama.cc) add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC}) +#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc) + SET(FBR_FEATURE_SRC feature.cc) add_library(fbr_feature STATIC ${FBR_FEATURE_SRC}) @@ -15,9 +17,12 @@ add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC}) SET(FBR_REGISTRATION_SRC registration.cc) add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC}) +add_library(fbr STATIC ${FBR_IO_SRC} ${FBR_PANORAMA_SRC} ${FBR_FEATURE_SRC} ${FBR_FEATURE_MATCHER_SRC} ${FBR_REGISTRATION_SRC} fbr_global.cc) + 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}) +#target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS}) +target_link_libraries(featurebasedregistration fbr ${FBR_LIBS}) ENDIF(WITH_FBR) diff --git a/.svn/pristine/57/574f5472dde9dff620cd7a4b2b432c86dd1368fd.svn-base b/.svn/pristine/57/574f5472dde9dff620cd7a4b2b432c86dd1368fd.svn-base deleted file mode 100644 index 27fa470..0000000 --- a/.svn/pristine/57/574f5472dde9dff620cd7a4b2b432c86dd1368fd.svn-base +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file - * @brief Scan types and mapping functions. - * - * @author Thomas Escher - */ - -#ifndef IO_TYPES_H -#define IO_TYPES_H - -//! IO types for file formats, distinguishing the use of ScanIOs -enum IOType { - UOS, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES -}; - -//! Data channels in the scans -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 -}; - -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/5c/5cf3012e8582e4ae022c0642566732f071a25fae.svn-base b/.svn/pristine/5c/5cf3012e8582e4ae022c0642566732f071a25fae.svn-base deleted file mode 100644 index 035c2cb..0000000 --- a/.svn/pristine/5c/5cf3012e8582e4ae022c0642566732f071a25fae.svn-base +++ /dev/null @@ -1,293 +0,0 @@ -#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 - }; - - 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 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/5d/5d349c54702145a5568fd48844c3df5cd57f4a63.svn-base b/.svn/pristine/5d/5d349c54702145a5568fd48844c3df5cd57f4a63.svn-base deleted file mode 100644 index b13ec1f..0000000 --- a/.svn/pristine/5d/5d349c54702145a5568fd48844c3df5cd57f4a63.svn-base +++ /dev/null @@ -1,225 +0,0 @@ -/** - * @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 DataAmplitude; -typedef SingleArray DataType; -typedef SingleArray DataDeviation; - -#endif //DATA_TYPES_H diff --git a/.svn/pristine/5d/5d5aa0784271d0773bf3ae3abec0d78473bbaf24.svn-base b/.svn/pristine/5d/5d5aa0784271d0773bf3ae3abec0d78473bbaf24.svn-base new file mode 100644 index 0000000..af41bf1 --- /dev/null +++ b/.svn/pristine/5d/5d5aa0784271d0773bf3ae3abec0d78473bbaf24.svn-base @@ -0,0 +1,209 @@ +/* + * feature implementation + * + * Copyright (C) HamidReza Houshiar + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/fbr/feature.h" + +using namespace std; + +namespace fbr{ + + feature::feature(){ + fDetectorMethod = SIFT_DET; + fDescriptorMethod = SIFT_DES; + fFiltrationMethod = DISABLE_FILTER; + } + feature::feature(feature_detector_method detector, feature_descriptor_method descriptor, feature_filtration_method filtration){ + fDetectorMethod = detector; + fDescriptorMethod = descriptor; + fFiltrationMethod = filtration; + } + void feature::featureDetection(cv::Mat pImage, feature_detector_method method, cv::Mat rImage, feature_filtration_method fMethod){ + fDetectorMethod = method; + fFiltrationMethod = fMethod; + switch(fDetectorMethod){ + //Detect the keypoints using SURF Detector + case SURF_DET:{ + double minHessian = 400; + cv::SurfFeatureDetector detector(minHessian); + detector.detect(pImage, keypoints); + break; + } + //Detect the keypoints using SIFT Detector + case SIFT_DET:{ + cv::SiftFeatureDetector detector; + detector.detect(pImage, keypoints); + break; + } + //Detect the keypoints using ORB Detector + case ORB_DET:{ + cv::OrbFeatureDetector detector; + detector.detect(pImage, keypoints); + break; + } + //Detect the keypoints using FAST Detector + case FAST_DET:{ + cv::FastFeatureDetector detector; + detector.detect(pImage, keypoints); + break; + } + //Detect the keypoints using STAR Detector + case STAR_DET:{ + cv::StarFeatureDetector detector; + detector.detect(pImage, keypoints); + break; + } + } + featureFiltration(pImage, rImage); + } + + void feature::featureDetection(cv::Mat pImage, feature_detector_method method){ + cv::Mat rImage; + featureDetection(pImage, method, rImage, fFiltrationMethod); + } + + void feature::featureDetection(cv::Mat pImage){ + featureDetection(pImage, fDetectorMethod); + } + + void feature::featureDescription(cv::Mat pImage, feature_descriptor_method method){ + + fDescriptorMethod = method; + if(keypoints.size() == 0) + featureDetection(pImage); + switch(fDescriptorMethod){ + case SURF_DES:{ + //Create descriptor using SURF + cv::SurfDescriptorExtractor extractor; + extractor.compute(pImage, keypoints, descriptors); + break; + } + case SIFT_DES:{ + //Create descriptor using SIFT + cv::SiftDescriptorExtractor extractor; + extractor.compute(pImage, keypoints, descriptors); + break; + } + case ORB_DES:{ + //Create descriptor using ORB + cv::OrbDescriptorExtractor extractor; + extractor.compute(pImage, keypoints, descriptors); + break; + } + } + } + + void feature::featureDescription(cv::Mat pImage){ + featureDescription(pImage, fDescriptorMethod); + } + + feature_detector_method feature::getDetectorMethod(){ + return fDetectorMethod; + } + + feature_descriptor_method feature::getDescriptorMethod(){ + return fDescriptorMethod; + } + + feature_filtration_method feature::getFeatureFiltrationMethod(){ + return fFiltrationMethod; + } + + //check for the keypoints vector not to be empty + vector feature::getFeatures(){ + return keypoints; + } + + //check for the descriptor Mat not to be empty + cv::Mat feature::getDescriptors(){ + return descriptors; + } + + void feature::setFeatures(vector keypoint){ + keypoints = keypoint; + } + + void feature::setDescriptors(cv::Mat descriptor){ + descriptors = descriptor; + } + + void feature::getDescription(description_method method){ + if(method == FEATURE_DESCRIPTION) + cout<<"fDetectorMethod: "< filteredKeypoints; + if(fFiltrationMethod == OCCLUSION){ + for(unsigned int i = 0; i < keypoints.size(); i++){ + int x, y; + x = keypoints[i].pt.x; + y = keypoints[i].pt.y; + float range[8]; + if(rImage.at(y,x) != 0){ + range[0] = rImage.at(y,x)-rImage.at(y+1,x+1); + range[1] = rImage.at(y,x)-rImage.at(y,x+1); + range[2] = rImage.at(y,x)-rImage.at(y-1,x+1); + range[3] = rImage.at(y,x)-rImage.at(y-1,x); + range[4] = rImage.at(y,x)-rImage.at(y-1,x-1); + range[5] = rImage.at(y,x)-rImage.at(y,x-1); + range[6] = rImage.at(y,x)-rImage.at(y+1,x-1); + range[7] = rImage.at(y,x)-rImage.at(y+1,x); + + int count=0; + for(unsigned int j = 0; j < 8; j++){ + if(range[j] < 20) + count++; + } + if(count == 8) + filteredKeypoints.push_back(keypoints[i]); + } + } + }else if(fFiltrationMethod == STANDARD_DEVIATION){ + for(unsigned int i = 0; i < keypoints.size(); i++){ + int x, y; + x = keypoints[i].pt.x; + y = keypoints[i].pt.y; + float range[9]; + if(rImage.at(y,x) != 0){ + range[0] = rImage.at(y,x)-rImage.at(y+1,x+1); + range[1] = rImage.at(y,x)-rImage.at(y,x+1); + range[2] = rImage.at(y,x)-rImage.at(y-1,x+1); + range[3] = rImage.at(y,x)-rImage.at(y-1,x); + range[4] = rImage.at(y,x)-rImage.at(y-1,x-1); + range[5] = rImage.at(y,x)-rImage.at(y,x-1); + range[6] = rImage.at(y,x)-rImage.at(y+1,x-1); + range[7] = rImage.at(y,x)-rImage.at(y+1,x); + range[8] = rImage.at(y,x)-rImage.at(y,x); + + double t_std, r=0, temp=0; + for(unsigned int j = 0; j < 9; j++) + r += range[j]; + r /= 9; + for(unsigned int j = 0; j < 9; j++) + temp += ((range[j] - r) * (range[j] - r)); + t_std = sqrt(temp/9); + if(t_std < 0.1) + filteredKeypoints.push_back(keypoints[i]); + } + } + } + if(fFiltrationMethod != DISABLE_FILTER) + keypoints = filteredKeypoints; + } +} + diff --git a/.svn/pristine/5e/5ed8495e8d30d5132dc3c320ee00fb7b4724c43c.svn-base b/.svn/pristine/5e/5ed8495e8d30d5132dc3c320ee00fb7b4724c43c.svn-base deleted file mode 100644 index d3142da..0000000 --- a/.svn/pristine/5e/5ed8495e8d30d5132dc3c320ee00fb7b4724c43c.svn-base +++ /dev/null @@ -1,367 +0,0 @@ -/* - * 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::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 == "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/b2/b2657f2e0912b3ab79b3d8363eaca625fc23a19c.svn-base b/.svn/pristine/5f/5fd7a2a341a6a1042dca7aae8607621b59b18eb4.svn-base similarity index 93% rename from .svn/pristine/b2/b2657f2e0912b3ab79b3d8363eaca625fc23a19c.svn-base rename to .svn/pristine/5f/5fd7a2a341a6a1042dca7aae8607621b59b18eb4.svn-base index 1cdc3d6..f247c52 100644 --- a/.svn/pristine/b2/b2657f2e0912b3ab79b3d8363eaca625fc23a19c.svn-base +++ b/.svn/pristine/5f/5fd7a2a341a6a1042dca7aae8607621b59b18eb4.svn-base @@ -34,6 +34,7 @@ namespace fbr{ double zMax; double zMin; IOType sFormat; + bool scanserver; public: /** @@ -41,7 +42,9 @@ namespace fbr{ * @param dir directory of the input scan file * @param number input scan number * @param format input scan file format + * @param scanServer */ + scan_cv (string dir, unsigned int number, IOType format, bool scanServer); scan_cv (string dir, unsigned int number, IOType format); /** * @brief read scan file and convert it to open cv Mat diff --git a/.svn/pristine/5f/5febe2ca6937f6f48cd72ce0c45940e082763ba5.svn-base b/.svn/pristine/5f/5febe2ca6937f6f48cd72ce0c45940e082763ba5.svn-base deleted file mode 100644 index 8ff6092..0000000 --- a/.svn/pristine/5f/5febe2ca6937f6f48cd72ce0c45940e082763ba5.svn-base +++ /dev/null @@ -1,187 +0,0 @@ -/* - * pointfilter implementation - * - * Copyright (C) Dorit Borrmann, Jan Elseberg - * - * Released under the GPL version 3. - * - */ - -#include "slam6d/pointfilter.h" - -using std::string; -using std::map; -#include -using std::stringstream; -#include -using std::runtime_error; - -#include -using std::cout; -using std::endl; - - - -map* PointFilter::factory = new map; - - - -PointFilter::PointFilter() : - m_changed(true), m_checker(0) -{ -} - -PointFilter::PointFilter(const std::string& params) : - m_changed(true), m_checker(0) -{ - size_t start = 0, end = string::npos; - while((end = params.find(' ', start)) != string::npos) { - // extract the word (start-end+1) without the space (-1) - string key(params.substr(start, start-end)); - end++; - // get the second word position - start = params.find(' ', end); - // insert - m_params[key] = params.substr(end, (start-end)); - // advance to the character after space - if(start != string::npos) - start++; - else - break; - } -} - -PointFilter::~PointFilter() -{ - if(m_checker) - delete m_checker; -} - -PointFilter& PointFilter::setRange(double maxDist, double minDist) -{ - m_changed = true; - stringstream s_max; s_max << maxDist; - stringstream s_min; s_min << minDist; - m_params["rangemax"] = s_max.str(); - m_params["rangemin"] = s_min.str(); - return *this; -} - -PointFilter& PointFilter::setHeight(double top, double bottom) -{ - m_changed = true; - stringstream s_top; s_top << top; - stringstream s_bottom; s_bottom << bottom; - m_params["heighttop"] = s_top.str(); - m_params["heightbottom"] = s_bottom.str(); - return *this; -} - -std::string PointFilter::getParams() -{ - stringstream s; - for(map::iterator it = m_params.begin(); it != m_params.end(); ++it) { - s << (*it).first << " " << (*it).second << " "; - } - return s.str(); -} - -void PointFilter::createCheckers() -{ - // delete the outdated ones - if(m_checker) { - delete m_checker; - m_checker = 0; - } - - // create new ones - Checker** current = &m_checker; - for(map::iterator it = m_params.begin(); it != m_params.end(); ++it) { - *current = (*factory)[it->first](it->second); - // if a Checker has been successfully created advance to its pointer in the chain - if(*current) { - current = &((*current)->m_next); - } - } -} - - - -Checker::Checker() : - m_next(0) -{ -} - -Checker::~Checker() -{ - if(m_next) - delete m_next; -} - - - -// create factory instaces for key string to factory function mapping -namespace { - CheckerFactory max("rangemax"); - CheckerFactory min("rangemin"); - CheckerFactory top("heighttop"); - CheckerFactory bottom("heightbottom"); -} - - - -CheckerRangeMax::CheckerRangeMax(const std::string& value) { - stringstream s(value); - s >> m_max; - // default value: no check - if(m_max <= 0.0) throw runtime_error("No range filter needed."); - m_max *= m_max; -} - -bool CheckerRangeMax::test(double* point) { - if(point[0]*point[0] + point[1]*point[1] + point[2]*point[2] < m_max) - return true; - return false; -} - - - -CheckerRangeMin::CheckerRangeMin(const std::string& value) { - stringstream s(value); - s >> m_min; - // default value: no check - if(m_min <= 0.0) throw runtime_error("No range filter needed."); - m_min *= m_min; -} - -bool CheckerRangeMin::test(double* point) { - if(point[0]*point[0] + point[1]*point[1] + point[2]*point[2] > m_min) - return true; - return false; -} - - - -CheckerHeightTop::CheckerHeightTop(const std::string& value) { - stringstream s(value); - s >> m_top; -} - -bool CheckerHeightTop::test(double* point) { - if(point[1] < m_top) - return true; - return false; -} - - - -CheckerHeightBottom::CheckerHeightBottom(const std::string& value) { - stringstream s(value); - s >> m_bottom; -} - -bool CheckerHeightBottom::test(double* point) { - if(point[1] > m_bottom) - return true; - return false; -} diff --git a/.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base b/.svn/pristine/61/61771213756ec66e2eeacafd08cd99af19bf4be7.svn-base similarity index 98% rename from .svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base rename to .svn/pristine/61/61771213756ec66e2eeacafd08cd99af19bf4be7.svn-base index 9b24d38..ad98d40 100644 --- a/.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base +++ b/.svn/pristine/61/61771213756ec66e2eeacafd08cd99af19bf4be7.svn-base @@ -112,7 +112,12 @@ public: //! Create a new set of reduced points DataXYZ createXYZReduced(unsigned int size); + + + //! Create a new set of reflectance + DataReflectance createReflectance(unsigned int size); + //! Reduced untransformed points DataXYZ getXYZReducedOriginal(); diff --git a/.svn/pristine/61/61dba2dfb5388a94f80f4d66b22b747da8f6f9fa.svn-base b/.svn/pristine/61/61dba2dfb5388a94f80f4d66b22b747da8f6f9fa.svn-base deleted file mode 100644 index c12ec30..0000000 --- a/.svn/pristine/61/61dba2dfb5388a94f80f4d66b22b747da8f6f9fa.svn-base +++ /dev/null @@ -1,132 +0,0 @@ -------------------------------------------------------------------- - -To compile the project simply call "make". This will configure slam6d -using the default settings. If you wish to configure the project using -custom settings do: "make config". This command requires ccmake be -installed on your system. Alternatively you may change into the build -directory ".build" and configure the project with your preferred cmake -configurator, i.e.: - -cd .build && cmake -i ../ - -For Microsoft Windows, use the cmake-gui application provided by cmake -to configure and generate project files for the appropriate version of -Microsoft Visual Studio C++ of your system. Use the INSTALL target to -built the entire project. Executables (and .dll's) will then reside -in the "windows" folder. For running the binaries you need to install -the proper redistributable package. - -Some Boost libraries (graph, regex, serialization, filesystem, -interprocess) are needed to compile the slam6D program. - -If you are using Debian just do: - - aptitude install libboost-graph-dev libboost-regex-dev libboost-serialization-dev freeglut3-dev libxmu-dev libxi-dev - -or, if you are still using Debian stable (lenny): - - aptitude install libboost-graph1.35-dev libboost-regex1.35-dev libboost-serialization1.35-dev freeglut3-dev libxmu-dev libxi-dev - -for Ubuntu this would be: - - sudo aptitude install libboost-graph-dev libboost-regex-dev libboost-serialization-dev libboost1.46-dev libboost-filesystem1.46-dev freeglut3-dev libxmu-dev libxi-dev - -SuSE users please use yast2 for installing the missing packages - - -Additionally for CAD matching support the following dependencies must -be satisfied (exemplarily for Ubuntu): - - sudo aptitude install libboost-program-options-dev - -------------------------------------------------------------------- - -For a detailed explanation of the programm, its usage, etc., please -refer to the high level documentation doc/documentation_HL.pdf -(esp. sections 4-6, for starters). - -IMPORTANT: -Take care to register scans first (bin/slam6D) before trying to -display them (bin/show), and think about using the point reduction -(see section 6) for a much faster scan matching result. Extremely -large scans might need to be reduced (using bin/scan_red) before -registration. This will write reduced scans in the uos format into a -directory "reduced" in the data directory. - -Three example scans are included in the dat directory, several -larger data sets can be downloaded from the data repository, -http://kos.informatik.uni-osnabrueck.de/3Dscans/ -(Alternatively, click on the "Data Repository" link on this project's -web site on Sourceforge, http://slam6d.sourceforge.net/) - -EXAMPLES: -(using the data set in the slam6d repository) -bin/slam6D -m 500 -R 5 -d 25.0 --metascan dat -bin/show dat - -(using the data set in the slam6d repository) -bin/slam6D --max=500 -r 10.2 -i 20 --metascan dat -bin/show dat - -(using hannover1.tgz from http://kos.informatik.uni-osnabrueck.de/3Dscans/) -bin/slam6D -s 1 -e 65 -r 10 -i 100 -d 75 -D 250 --epsICP=0.00001 - -I 50 --cldist=750 -L 0 -G 1 /home/nuechter/dat/dat_hannover1 -bin/show -s 1 -e 65 /home/nuechter/dat/dat_hannover1 - -(using hannover2.tgz from http://kos.informatik.uni-osnabrueck.de/3Dscans/) -bin/slam6D -q -r 10 -f rts -s 23 -d 75 -L 4 --cldist=1500 -G 1 -D -1 - --DlastSLAM 250 --graphDist 200 -I 50 hannover2 -bin/show -f rts -s 23 hannover2 - -(using kvarntorp_mine.tgz (dat_mine1) form http://kos.informatik.uni-osnabrueck.de/3Dscans/) -bin/slam6D -s 1 -e 76 -r 10 -m 3000 -d 50 -i 1000 --epsICP=0.000001 - -I 50 -D 75 --clpairs=5000 -f old /home/nuechter/dat/dat_mine1/ -bin/show -s 1 -e 76 -m 3000 -f old /home/nuechter/dat/dat_mine1/ - -(using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) -bin/scan_red -s 0 -e 12 -r 10 /home/nuechter/dat/bremen_city -bin/slam6D -a 2 -q /home/nuechter/dat/bremen_city/reduced -f uos -d 150 - -s 0 -e 12 --anim=1 -n /home/nuechter/dat/bremen_city/bremen.net - -G 1 -D 100 -i 0 -I 50 -p --epsSLAM=0.0 -bin/show -s 0 -e 12 /home/nuechter/dat/bremen_city/reduced - -(using UniKoblenz_CampusTour3_OsnabrueckFormat.tar.gz from -http://kos.informatik.uni-osnabrueck.de/3Dscans/) -bin/slam6D -s 1 -e 320 -r 20 -i 300 --epsICP=0.000001 -d 45 -D 45 - -f uos --algo=2 -l 10 -L 4 -I 100 -G 1 - /home/nuechter/dat/UniKoblenz_CampusTour3_OsnabrueckFormat/ -bin/show -s 1 -e 320 -f uos /home/nuechter/dat/UniKoblenz_CampusTour3_OsnabrueckFormat/ -------------------------------------------------------------------- - -For detecting planes compile with the WITH_PLANE option. -Adapt the settings in bin/hough.cfg for your data set. - -EXAMPLE: (using the data set in the slam6d repository, no modification -of bin/hough.cfg necessary) - -bin/planes -s 0 dat -bin/show -s 0 -e 0 dat -l dat/planes/planes.list - -(using bremen_city.zip from http://kos.informatik.uni-osnabrueck.de/3Dscans/) -adapt these settings in bin/hough.cfg: -RhoNum 500 -RhoMax 5000 -MaxPointPlaneDist 50.0 -MinPlaneSize 50 -PointDist 100.0 -/bin/planes -f riegl_txt -s 0 /home/nuechter/dat/bremen_city/ -r 50 -O 1 -m 5000 -/bin/show -s 0 -e 0 /home/nuechter/dat/bremen_city/ -f riegl_txt -l dat/planes/planes.list -r 10 -O 1 -m 5000 - -------------------------------------------------------------------- -The IO relevant parameters -f(ormat), -s(tart), -e(nd) can be omitted -in slam6D and show if a 'format' file exists in the directory, which -contains key=value lines (spaces are trimmed automatically) for -format, start, end with the same values as in the commandline. These -format-file parameters will be overwritten by commandline parameters -so that the format-file will provide the right IO type and full index -range and the user can overwrite the index range as he sees fit. - -------------------------------------------------------------------- -A reference manual can be found in doc/refman.pdf resp. -doc/html/index.html (type in 'make docu' to compile the doxygen -documentation for the HTML files). diff --git a/.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base b/.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base deleted file mode 100644 index 775fa92..0000000 --- a/.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base +++ /dev/null @@ -1,1101 +0,0 @@ -/* - * show_common implementation - * - * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann - * - * Released under the GPL version 3. - * - */ - -#ifdef WITH_GLEE -#include -#endif - -#include "show/show.h" -#include "show/show_Boctree.h" -#include "show/compacttree.h" -#include "show/NurbsPath.h" -#include "show/vertexarray.h" -#ifndef DYNAMIC_OBJECT_REMOVAL -#include "slam6d/scan.h" -#include "slam6d/managedScan.h" -#else -#include "veloslam/veloscan.h" -#endif -#include "glui/glui.h" /* Header File For The glui functions */ -#include -using std::ifstream; -#include -using std::exception; -#include - -#ifdef WITH_METRICS -#include "slam6d/metrics.h" -#endif - -#ifdef _MSC_VER -#include "XGetopt.h" -#else -#include -#endif - -#ifdef _MSC_VER -#define strcasecmp _stricmp -#define strncasecmp _strnicmp -#else -#include -#endif - - -#ifdef _MSC_VER -#if !defined _OPENMP && defined OPENMP -#define _OPENMP -#endif -#endif - -#ifdef _OPENMP -#include -#endif - -#include "slam6d/point_type.h" -#include "slam6d/io_utils.h" -#include "show/display.h" - -/** - * This vector contains the pointer to a vertex array for - * all colors (inner vector) and all scans (outer vector) - */ -vector< vector > vvertexArrayList; - -vector< ::SDisplay*> displays; -/** - * the octrees that store the points for each scan - */ -//Show_BOctTree **octpts; -vector octpts; -/** - * Storing the base directory - */ -string scan_dir; - -/** - * Storing the ID of the main windows - */ -int window_id; - -/** - * Size of points - */ -GLfloat pointsize = 1.0; -int anim_delay = 5; - -/** - * Select Color Buffer - */ -GLenum buffermode = GL_BACK; - -/** - * Indicator whether and how the drawing window - * has to be updated. - * - * haveToUpdate == 1 redisplay - * haveToUpdate == 2 reshape - * haveToUpdate == 3 animation scan matching - * haveToUpdate == 4 stop animation scan matching - * haveToUpdate == 6 path animation - * haveToUpdate == 7 force redisplay with all points - */ -int haveToUpdate = 0; - -/** - * Flag for invert the scene - */ -bool invert = true; - -/** - * Flag for indicating brid eyes view - */ -bool showTopView = false; - -/** - * Flag for idicating camera add mode - */ -bool addCameraView = false; //Is the view in add box mode? - -/** - * Storing the apex angle of the camera - */ -GLfloat cangle = 60.0; // Current camera opening mode -GLfloat cangle_old = cangle; - -/** - * Current rotation axis of the scene as quaternion - */ -GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; -GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; - -/** - * Current translation of the scene - */ -GLdouble X = 0.0, Y = 0.0, Z = 0.0; -GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; - -/** - * parallel zoom (similar to apex angle) for parallel projection - */ -GLfloat pzoom = 2000.0; -GLfloat pzoom_old = pzoom; - - -/** - * Mode of the fog (exp, exp2, linear) - */ -GLint fogMode = GL_EXP; - -/** - * Indicates if fog should be shown - */ -int show_fog = 1; - -/** - * Indicates if the points should be shown - */ -int show_points = 1; // Show data points in the viewer? - -/** - * Indicates if camera boxes should be shown - */ -int show_cameras = 1; // Show the camera boxes in the viewer? - -/** - * Indicates if camera path or robot path should be shown - */ -int show_path = 1; // Show the camera movement path ? - -/** - * Camera navigation by mouse or by panel - */ -int cameraNavMouseMode = 1; - -int mouseNavX, mouseNavY; -int mouseNavButton = -1; - -double mouseRotX = 0.0; -double mouseRotY = 0.0; -double mouseRotZ = 0.0; - -bool keymap[256]; - -//@@@ -//int animate_both = 0; // Animate both scan matchin and path? - -int frameNr = 0; - -/** - * Storing of all transformation (frames for animation) of all scans - */ -vector < vector > MetaMatrix; - -/** - * Storing of AlgoType for all frames - */ -vector < vector > MetaAlgoType; - -/** - * Window position - */ -int START_X = 0; -int START_Y = 0; -int START_WIDTH = 960; -int START_HEIGHT = 540; -GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio -bool advanced_controls = false; - -bool fullscreen = false; -int current_width = START_WIDTH; -int current_height = START_HEIGHT; - - -// the following values are scale dependant, i.e. all values are in m -float neardistance = 0.10; -double oldneardistance = 0.10; -float maxfardistance = 400.0; -double fardistance = 400.0; -double oldfardistance = 40000.0; -double movementSpeed = 0.1; -double defaultZoom = 20.0; -GLfloat fogDensity = 0.1; -double voxelSize = 0.20; - - -float adaption_rate = 1.0; -float LevelOfDetail = 0.0001; - - -// Defines for Point Semantic -#define TYPE_UNKNOWN 0x0000 -#define TYPE_OBJECT 0x0001 -#define TYPE_GROUND 0x0002 -#define TYPE_CEILING 0x0003 - -unsigned int cam_choice = 0; - -static unsigned int path_iterator = 0; -static int oldcamNavMode = 0; - -/** - * Animation sould be saved to file - */ -int save_animation = 0; -/** - * If true, interpolation for camera path is based on distance, else always - * the same number of images between two cameras. - */ -int inter_by_dist = 1; - -/**some variables for the camera path**/ -vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; -vector cams; -vector lookats; -vector ups; - -NurbsPath cam_nurbs_path; -char *path_file_name; -char *pose_file_name; - -/** Factor for saved image size */ -int factor = 1; - -/** - * program tries to have this framerate - */ -float idealfps = 20.0; -/** - * value of the listBox fo Color Value and Colormap - */ -int listboxColorVal = 0; -int listboxColorMapVal = 0; -int colorScanVal = 0; -ScanColorManager *cm; -float mincolor_value = 0.0; -float maxcolor_value = 0.0; -//unsigned int types = Point::USE_HEIGHT; -PointType pointtype; - -/** - * Contains the selected points for each scan - */ -set *selected_points; -/** - * Select single points? - */ -int select_voxels = 0; -/** - * Select or unselect points ? - */ -int selectOrunselect = 1; -/** octree depth for selecting groups of points */ -int selection_depth = 1; -int brush_size = 0; -char *selection_file_name; - -int current_frame = 0; -#include "show_menu.cc" -#include "show_animate.cc" -#include "show_gl.cc" - -/** - * Explains the usage of this program's command line parameters - * @param prog name of the program - */ -void usage(char* prog) -{ -#ifndef _MSC_VER - const string bold("\033[1m"); - const string normal("\033[m"); -#else - const string bold(""); - const string normal(""); -#endif - - cout << endl - << bold << "USAGE " << normal << endl - << " " << prog << " [options] directory" << endl << endl; - cout << bold << "OPTIONS" << normal << endl - - << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl - << " end after scan NR" << endl - << endl - << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl - << " using shared library F for input" << endl - << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl - << endl - << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl - << " will attempt to display points with a framerate of NR" << endl - << endl - << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << - "FILE" << endl - << " load objects specified in " << endl - << endl - << endl - << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl - << " neglegt all data points with a distance larger than NR 'units'" << endl - << endl - << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl - << " neglegt all data points with a distance smaller than NR 'units'" << endl - << endl - << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl - << " use randomized octree based point reduction (pts per voxel=)" << endl - << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl - << endl - << bold << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl - << " sets the starting and reset position to: " << endl - << " 0 = the origin of the coordinate system (default)" << endl - << " 1 = the position of the first scan (default if --origin is in argument list)" << endl - << " 2 = the center of the first scan" << endl - << endl - << bold << " -S, --scanserver" << normal << endl - << " Use the scanserver as an input method and handling of scan data" << endl - << endl - << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl - << " turns on octree based point reduction (voxel size=)" << endl - << endl - << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl - << " start at scan NR (i.e., neglects the first NR scans)" << endl - << " [ATTENTION: counting naturally starts with 0]" << endl - << endl - << bold << " -C" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl - << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl - << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl - << " " << endl - << endl - - << bold << " -R, --reflectance, --reflectivity" << normal << endl - << " use reflectivity values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -a, --amplitude" << endl << normal - << " use amplitude values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -d, --deviation" << endl << normal - << " use amplitude values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -h, --height" << endl << normal - << " use y-values for coloring point clouds" << endl - << " only works when using octree display" << endl - << endl - << bold << " -T, --type" << endl << normal - << " use type values for coloring point clouds" << endl - << " only works when using octree display" << endl - << bold << " -c, --color" << endl << normal - << " use color RGB values for coloring point clouds" << endl - << bold << " --saveOct" << endl << normal - << " stores all used scans as octrees in the given directory" << endl - << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl - << " only works when using octree display" << endl - << bold << " --loadOct" << endl << normal - << " only reads octrees from the given directory" << endl - << " All reflectivity/amplitude/deviation/type settings are read from file." << endl - << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl - << " only works when using octree display" << endl - << endl << endl; - - exit(1); -} - -/** - * A function that parses the command-line arguments and sets the respective flags. - * - * @param argc the number of arguments - * @param argv the arguments - * @param dir parsing result - the directory - * @param start parsing result - starting at scan number 'start' - * @param end parsing result - stopping at scan number 'end' - * @param maxDist parsing result - maximal distance - * @param minDist parsing result - minimal distance - * @param readInitial parsing result - read a file containing a initial transformation matrix - * @param type parsing result - file format to be read - * @return 0, if the parsing was successful, 1 otherwise - */ -int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, - double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, - bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver) -{ - unsigned int types = PointType::USE_NONE; - start = 0; - end = -1; // -1 indicates no limitation - maxDist = -1; // -1 indicates no limitation - int c; - // from unistd.h - extern char *optarg; - extern int optind; - - WriteOnce w_type(type); - WriteOnce w_start(start), w_end(end); - - cout << endl; - static struct option longopts[] = { - { "origin", optional_argument, 0, 'o' }, - { "format", required_argument, 0, 'f' }, - { "fps", required_argument, 0, 'F' }, - { "scale", required_argument, 0, 'S' }, - { "start", required_argument, 0, 's' }, - { "end", required_argument, 0, 'e' }, - { "reduce", required_argument, 0, 'r' }, - { "max", required_argument, 0, 'm' }, - { "min", required_argument, 0, 'M' }, - { "octree", optional_argument, 0, 'O' }, - { "reflectance", no_argument, 0, 'R' }, - { "reflectivity", no_argument, 0, 'R' }, - { "amplitude", no_argument, 0, 'a' }, - { "deviation", no_argument, 0, 'd' }, - { "height", no_argument, 0, 'h' }, - { "type", no_argument, 0, 'T' }, - { "color", no_argument, 0, 'c' }, - { "loadObj", required_argument, 0, 'l' }, - { "saveOct", no_argument, 0, '0' }, - { "loadOct", no_argument, 0, '1' }, - { "advanced", no_argument, 0, '2' }, - { "scanserver", no_argument, 0, 'S' }, - { 0, 0, 0, 0} // needed, cf. getopt.h - }; - - while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRadhTc", longopts, NULL)) != -1) { - switch (c) { - case 's': - w_start = atoi(optarg); - if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } - break; - case 'e': - w_end = atoi(optarg); - if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } - if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } - break; - case 'm': - maxDist = atoi(optarg); - break; - case 'M': - minDist = atoi(optarg); - break; - case 'r': - red = atof(optarg); - break; - case 't': - readInitial = true; - break; - case 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } - break; - case 'f': - try { - w_type = formatname_to_io_type(optarg); - } catch (...) { // runtime_error - cerr << "Format " << optarg << " unknown." << endl; - abort(); - } - break; - case '?': - usage(argv[0]); - return 1; - case 'R': - types |= PointType::USE_REFLECTANCE; - break; - case 'a': - types |= PointType::USE_AMPLITUDE; - break; - case 'd': - types |= PointType::USE_DEVIATION; - break; - case 'h': - types |= PointType::USE_HEIGHT; - break; - case 'T': - types |= PointType::USE_TYPE; - break; - case 'c': - types |= PointType::USE_COLOR; - break; - case 'F': - fps = atof(optarg); - break; - case 'C': - scale = atof(optarg); - break; - case 'S': - scanserver = true; - break; - case 'o': - if (optarg) { - origin = atoi(optarg); - } else { - origin = 1; - } - break; - case '0': - saveOct = true; - break; - case '1': - loadOct = true; - break; - case 'l': - loadObj = optarg; - break; - case '2': - advanced_controls = true; - break; - default: - abort (); - } - } - - if (optind != argc-1) { - cerr << "\n*** Directory missing ***" << endl; - usage(argv[0]); - } - dir = argv[optind]; - -#ifndef _MSC_VER - if (dir[dir.length()-1] != '/') dir = dir + "/"; -#else - if (dir[dir.length()-1] != '\\') dir = dir + "\\"; -#endif - - parseFormatFile(dir, w_type, w_start, w_end); - - ptype = PointType(types); - return 0; -} - -void setResetView(int origin) { - if (origin == 1) { - double *transmat = MetaMatrix[0].back(); - cout << transmat << endl; - - RVX = -transmat[12]; - RVY = -transmat[13]; - RVZ = -transmat[14]; - Matrix4ToQuat(transmat, Rquat); - X = RVX; - Y = RVY; - Z = RVZ; - quat[0] = Rquat[0]; - quat[1] = Rquat[1]; - quat[2] = Rquat[2]; - quat[3] = Rquat[3]; - } else if (origin == 2) { - double center[3]; -#ifdef USE_COMPACT_TREE - ((compactTree*)octpts[0])->getCenter(center); -#else - ((Show_BOctTree*)octpts[0])->getCenter(center); -#endif - RVX = -center[0]; - RVY = -center[1]; - RVZ = -center[2]; - X = RVX; - Y = RVY; - Z = RVZ; - } -} - -/* - * A function that read the .frame files created by slam6D - * - * @param dir the directory - * @param start starting at scan number 'start' - * @param end stopping at scan number 'end' - * @param read a file containing a initial transformation matrix and apply it - */ -int readFrames(string dir, int start, int end, bool readInitial, IOType &type) -{ - - // convert to OpenGL coordinate system - double mirror[16]; - M4identity(mirror); - mirror[10] = -1.0; - - double initialTransform[16]; - if (readInitial) { - cout << "Initial Transform:" << endl; - string initialTransformFileName = dir + "initital.frame"; - ifstream initial_in(initialTransformFileName.c_str()); - if (!initial_in.good()) { - cout << "Error opening " << initialTransformFileName << endl; - exit(-1); - } - initial_in >> initialTransform; - cout << initialTransform << endl; - - // update the mirror to apply the initial frame for all frames - double tempxf[16]; - MMult(mirror, initialTransform, tempxf); - memcpy(mirror, tempxf, sizeof(tempxf)); - } - - for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { - const double* transformation; - Scan::AlgoType algoType; - vector Matrices; - vector algoTypes; - - // iterate over frames (stop if none were created) and pull/convert the frames into local containers - unsigned int frame_count; - try { - frame_count = (*it)->readFrames(); - } catch(std::ios_base::failure& e) { - break; - } - for(unsigned int i = 0; i < frame_count; ++i) { - (*it)->getFrame(i, transformation, algoType); - double* transMatOpenGL = new double[16]; - - // apply mirror to convert (and initial frame if requested) the frame and save in opengl - MMult(mirror, transformation, transMatOpenGL); - - Matrices.push_back(transMatOpenGL); - algoTypes.push_back(algoType); - } - - MetaAlgoType.push_back(algoTypes); - MetaMatrix.push_back(Matrices); - - if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && it == Scan::allScans.begin()) { - MetaAlgoType.push_back(algoTypes); - MetaMatrix.push_back(Matrices); - } - current_frame = MetaMatrix.back().size() - 1; - } - - if (MetaMatrix.size() == 0) { - cerr << "*****************************************" << endl; - cerr << "** ERROR: No .frames could be found! **" << endl; - cerr << "*****************************************" << endl; - cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; - return -1; - } - return 0; -} - -void generateFrames(int start, int end, bool identity) { - if (identity) { - cout << "using Identity for frames " << endl; - } else { - cout << "using pose information for frames " << endl; - } - int fileCounter = start; - int index = 0; - for (;;) { - if (fileCounter > end) break; // 'nuf read - fileCounter++; - - vector Matrices; - vector algoTypes; - - for (int i = 0; i < 3; i++) { - double *transMat = new double[16]; - - if (identity) { - M4identity(transMat); - transMat[10] = -1.0; - } else { - EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); - } - - Matrices.push_back(transMat); - algoTypes.push_back(Scan::ICP); - - } - index++; - MetaAlgoType.push_back(algoTypes); - MetaMatrix.push_back(Matrices); - current_frame = MetaMatrix.back().size() - 1; - } -} - - - -/* - * create display lists - * @to do general framework for color & type definitions - */ -void createDisplayLists(bool reduced) -{ - for(unsigned int s = 0; s < Scan::allScans.size() ; s++) { - Scan* scan = Scan::allScans[s]; - - vertexArray* myvertexArray1; - vertexArray* myvertexArray2; - - // count points - unsigned int color1 = 0, color2 = 0; - if(!reduced) { - scan->get(DATA_XYZ | DATA_TYPE); - DataType type(scan->get("type")); - - if(type.valid()) { - for(unsigned int i = 0; i < type.size(); ++i) { - if(type[i] & TYPE_GROUND) { - color1 += 3; - } else { - color2 += 3; - } - } - } else { - color2 = 3 * scan->size("xyz"); - } - - myvertexArray1 = new vertexArray(color1); - myvertexArray2 = new vertexArray(color2); - - color1 = 0; color2 = 0; - - DataXYZ xyz(scan->get("xyz")); - for(unsigned int i = 0; i < xyz.size(); ++i) { - if(type[i] & TYPE_GROUND) { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray1->array[color1++] = xyz[i][j]; - } - } else { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray2->array[color2++] = xyz[i][j]; - } - } - } - } else { - color2 = 3 * scan->size("xyz reduced"); - - myvertexArray1 = new vertexArray(0); - myvertexArray2 = new vertexArray(color2); - - color2 = 0; - - DataXYZ xyz_r(scan->get("xyz reduced")); - for(unsigned int i = 0; i < xyz_r.size(); ++i) { - for(unsigned int j = 0; j < 3; ++j) { - myvertexArray2->array[color2++] = xyz_r[i][j]; - } - } - } - - - - glNewList(myvertexArray1->name, GL_COMPILE); - //@ - //glColor4d(0.44, 0.44, 0.44, 1.0); - //glColor4d(0.66, 0.66, 0.66, 1.0); - glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array); - glEnableClientState(GL_VERTEX_ARRAY); - glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender); - glDisableClientState(GL_VERTEX_ARRAY); - glEndList(); - - glNewList(myvertexArray2->name, GL_COMPILE); - //glColor4d(1.0, 1.0, 1.0, 1.0); - //glColor4d(0.0, 0.0, 0.0, 1.0); - glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array); - glEnableClientState(GL_VERTEX_ARRAY); - glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender); - glDisableClientState(GL_VERTEX_ARRAY); - glEndList(); - - // append to vector - vector vvertexArray; - vvertexArray.push_back(myvertexArray1); - vvertexArray.push_back(myvertexArray2); - vvertexArrayList.push_back(vvertexArray); - } -} - -void cycleLOD() { - LevelOfDetail = 0.00001; - for (unsigned int i = 0; i < octpts.size(); i++) - octpts[i]->cycleLOD(); -} - - -void initShow(int argc, char **argv){ - - /***************/ - /* init OpenGL */ - /***************/ - glutInit(&argc,argv); - - cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl - << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl - << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; - - if(argc <= 1){ - usage(argv[0]); - } - - double red = -1.0; - int start = 0, end = -1, maxDist = -1, minDist = -1; - string dir; - bool readInitial = false; - IOType type = UOS; - int octree = 0; - bool loadOct = false; - bool saveOct = false; - string loadObj; - int origin = 0; - double scale = 0.01; // in m - bool scanserver = false; - - pose_file_name = new char[1024]; - path_file_name = new char[1024]; - selection_file_name = new char[1024]; - - strncpy(pose_file_name, "pose.dat", 1024); - strncpy(path_file_name, "path.dat", 1024); - strncpy(selection_file_name, "selected.3d", 1024); - - parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, - octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, type, scanserver); - - // modify all scale dependant variables - scale = 1.0 / scale; - movementSpeed *= scale; - neardistance *= scale; - oldneardistance *= scale; - maxfardistance *= scale; - fardistance *= scale; - fogDensity /= scale; - defaultZoom *= scale; - voxelSize *= scale; -// oldfardistance *= scale; - - //////////////////////// - SDisplay::readDisplays(loadObj, displays); - //////////////////// - - if (type == OCT) { - loadOct = true; - } - - // if we want to load display file get pointtypes from the files first - if(loadOct) { - string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; - cout << "Getting point information from " << scanFileName << endl; - cout << "Attention! All subsequent oct-files must be of the same type!" << endl; - - pointtype = BOctTree::readType(scanFileName); - } - scan_dir = dir; - - // init and create display - M4identity(view_rotate_button); - obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; - - // Loading scans, reducing, loading frames and generation if neccessary - - // load all available scans - Scan::openDirectory(scanserver, dir, type, start, end); - - if(Scan::allScans.size() == 0) { - cerr << "No scans found. Did you use the correct format?" << endl; - exit(-1); - } - - for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { - Scan* scan = *it; - scan->setRangeFilter(maxDist, minDist); - if(red > 0) { - // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time - if(scanserver) { - dynamic_cast(scan)->setShowReductionParameter(red, octree); - } else { - scan->setReductionParameter(red, octree); - } - } - } - cm = new ScanColorManager(4096, pointtype); - -#ifdef USE_GL_POINTS // use octtrees -#ifdef USE_COMPACT_TREE - cout << "Creating compact display octrees.." << endl; -#else - cout << "Creating display octrees.." << endl; -#endif - - if(loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; - - // for managed scans the input phase needs to know how much it can handle - std::size_t free_mem = 0; - if(scanserver) - free_mem = ManagedScan::getMemorySize(); - - for(unsigned int i = 0; i < Scan::allScans.size(); ++i) { - Scan* scan = Scan::allScans[i]; - - // create data structures -#ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed - compactTree* tree; - try { - if (red > 0) { // with reduction, only xyz points - DataXYZ xyz_r(scan->get("xyz reduced show")); - tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); - } else { // without reduction, xyz + attribute points - sfloat** pts = pointtype.createPointArray(scan); - unsigned int nrpts = scan->size("xyz"); - tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); - for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; - } - } catch(...) { - cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; - break; - } -#else // FIXME: remove the case above - scan->setOcttreeParameter(red, voxelSize, pointtype, loadOct, saveOct); - - DataOcttree* data_oct; - try { - data_oct = new DataOcttree(scan->get("octtree")); - } catch(runtime_error& e) { - cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; - break; - } - BOctTree* btree = &(data_oct->get()); - unsigned int tree_size = btree->getMemorySize(); - - if(scanserver) { - // check if the octtree would actually fit with all the others - if(tree_size > free_mem) { - delete data_oct; - cout << "Stopping at scan " << i << ", no more octtrees could fit in memory." << endl; - break; - } else { - // subtract available memory - free_mem -= tree_size; - } - } -#endif //FIXME: COMPACT_TREE - -#if !defined USE_COMPACT_TREE - // show structures - // associate show octtree with the scan and hand over octtree pointer ownership - Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); - - // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points - tree->unlockCachedTree(); -#endif - - // octtrees have been created successfully - octpts.push_back(tree); - - // print something -#ifdef USE_COMPACT_TREE // TODO: change compact tree for memory footprint output, remove this case - cout << "Scan " << i << " octree finished." << endl; -#else - cout << "Scan " << i << " octree finished ("; - bool space = false; - if(tree_size/1024/1024 > 0) { - cout << tree_size/1024/1024 << "M"; - space = true; - } - if((tree_size/1024)%1024 > 0) { - if(space) cout << " "; - cout << (tree_size/1024)%1024 << "K"; - space = true; - } - if(tree_size%1024 > 0) { - if(space) cout << " "; - cout << tree_size%1024 << "B"; - } - cout << ")." << endl; -#endif - } - -/* -TODO: to maximize space for octtrees, implement a heuristic to remove all -CacheObjects sequentially from the start and pack octtrees one after another -until it reaches the maximum size allowed, resetting the index, but causing the -first to be locked again and stopping by catching the exception -set heuristic, do locking, catch exception, reset heuristic to default or old -*/ -#if !defined USE_COMPACT_TREE - if(scanserver) { - // activate all octtrees until they don't fit anymore - cout << "Locking managed octtrees in memory " << flush; - bool stop = false; - unsigned int loaded = 0; - unsigned int dots = (octpts.size() / 20); - if(dots == 0) dots = 1; - vector::iterator it_remove_first = octpts.end(); - for(vector::iterator it = octpts.begin(); it != octpts.end(); ++it) { - if(!stop) { - // try to lock the octtree in cache - try { - Show_BOctTree* stree = dynamic_cast*>(*it); - stree->lockCachedTree(); - loaded++; - if(loaded % dots == 0) cout << '.' << flush; - } catch(runtime_error& e) { - stop = true; - it_remove_first = it; - } - } - if(stop) { - // delete the octtree, resize after iteration - delete *it; - } - } - // now remove iterators for deleted octtrees - if(stop) octpts.erase(it_remove_first, octpts.end()); - cout << ' ' << loaded << " octtrees loaded." << endl; - } - -#endif // !COMPACT_TREE - -#else // not using octtrees - createDisplayLists(red > 0); -#endif // USE_GL_POINTS - - // load frames now that we know how many scans we actually loaded - unsigned int real_end = min((unsigned int)(end), - (unsigned int)(start + octpts.size() - 1)); - if(readFrames(dir, start, real_end, readInitial, type)) - generateFrames(start, real_end, true); - - cm->setCurrentType(PointType::USE_HEIGHT); - //ColorMap cmap; - //cm->setColorMap(cmap); - resetMinMax(0); - - selected_points = new set[octpts.size()]; - - // sets (and computes if necessary) the pose that is used for the reset button - setResetView(origin); - - for (unsigned int i = 0; i < 256; i++) { - keymap[i] = false; - } -} - -void deinitShow() -{ - static volatile bool done = false; - if(done) return; - done = true; - - cout << "Cleaning up octtrees and scans." << endl; - if(octpts.size()) { - // delete octtrees to release the cache locks within - for(vector::iterator it = octpts.begin(); it!= octpts.end(); ++it) { - delete *it; - } - } - - Scan::closeDirectory(); -} - -/** - * Global program scope destructor workaround to clean up data regardless of - * the way of program exit. - */ -struct Deinit { ~Deinit() { deinitShow(); } } deinit; diff --git a/.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base b/.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base deleted file mode 100644 index 462b788..0000000 --- a/.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base +++ /dev/null @@ -1,29 +0,0 @@ -/** - * @file scan_io_uosr.h - * @brief IO of a 3D scan in uos file format plus a - * reflectance/intensity/temperature value - * @author Billy Okal - */ - -#ifndef __SCAN_IO_UOSR_H__ -#define __SCAN_IO_UOSR_H__ - -#include "scan_io.h" - - - -/** - * @brief IO of a 3D scan in uos file format plus a - * reflectance/intensity/temperature value - * - * The compiled class is available as shared object file - */ -class ScanIO_uosr : public ScanIO { -public: - virtual std::list readDirectory(const char* dir_path, unsigned int start, unsigned int end); - virtual void readPose(const char* dir_path, const char* identifier, double* pose); - virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/65/659ab1f367762ef26220738c7bdee95d3e31096e.svn-base b/.svn/pristine/65/659ab1f367762ef26220738c7bdee95d3e31096e.svn-base deleted file mode 100644 index 5d7dd30..0000000 --- a/.svn/pristine/65/659ab1f367762ef26220738c7bdee95d3e31096e.svn-base +++ /dev/null @@ -1,53 +0,0 @@ -# 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) - # boost::interprocess uses pthread, requiring librt - set(CLIENT_LIBS ${CLIENT_LIBS} rt) -endif(UNIX) - -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/6d/6d1e252e410290ff63a7ab6a12dd8df6a2334f6a.svn-base b/.svn/pristine/6d/6d1e252e410290ff63a7ab6a12dd8df6a2334f6a.svn-base deleted file mode 100644 index fcabf17..0000000 --- a/.svn/pristine/6d/6d1e252e410290ff63a7ab6a12dd8df6a2334f6a.svn-base +++ /dev/null @@ -1,147 +0,0 @@ -/* - * io_types implementation - * - * Copyright (C) Thomas Escher, Kai Lingemann - * - * Released under the GPL version 3. - * - */ - -#include "scanserver/io_types.h" - -#ifdef _MSC_VER -//#include // TODO: TEST -#define strcasecmp _stricmp -#define strncasecmp _strnicmp -#else -#include -#endif - -#include -#include - -#include "slam6d/globals.icc" - -IOType formatname_to_io_type(const char * string) -{ - if (strcasecmp(string, "uos") == 0) return UOS; - else if (strcasecmp(string, "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, "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 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 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 "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/70/700c1d451bb3afcf18070c2a847db45943f91ecf.svn-base b/.svn/pristine/70/700c1d451bb3afcf18070c2a847db45943f91ecf.svn-base deleted file mode 100644 index 38b47d9..0000000 --- a/.svn/pristine/70/700c1d451bb3afcf18070c2a847db45943f91ecf.svn-base +++ /dev/null @@ -1,786 +0,0 @@ -/* - * scan implementation - * - * Copyright (C) Andreas Nuechter, Kai Lingemann, Dorit Borrmann, Jan Elseberg, Thomas Escher - * - * Released under the GPL version 3. - * - */ - -#include "slam6d/scan.h" - -#include "slam6d/basicScan.h" -#include "slam6d/managedScan.h" -#include "slam6d/metaScan.h" -#include "slam6d/searchTree.h" -#include "slam6d/kd.h" -#include "slam6d/Boctree.h" -#include "slam6d/globals.icc" - -#ifdef WITH_METRICS -#include "slam6d/metrics.h" -#endif - -#ifdef _MSC_VER -#define _NO_PARALLEL_READ -#endif - -#ifdef __APPLE__ -#define _NO_PARALLEL_READ -#endif - -using std::vector; - - -vector 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_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/73/73f4b34ee616fbdb36fe3cc2d7dcec5f665d89c3.svn-base b/.svn/pristine/73/73f4b34ee616fbdb36fe3cc2d7dcec5f665d89c3.svn-base deleted file mode 100644 index 47847d4..0000000 --- a/.svn/pristine/73/73f4b34ee616fbdb36fe3cc2d7dcec5f665d89c3.svn-base +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file - * @brief - * - * @author Thomas Escher - */ - -#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_amplitude; - static std::map* > m_prefetch_type; - static std::map* > m_prefetch_deviation; -}; - -#endif //SCAN_HANDLER_H diff --git a/.svn/pristine/74/7483d87ea44f6915500ff9a9769a1515e60abd03.svn-base b/.svn/pristine/74/7483d87ea44f6915500ff9a9769a1515e60abd03.svn-base deleted file mode 100644 index 64fbefa..0000000 --- a/.svn/pristine/74/7483d87ea44f6915500ff9a9769a1515e60abd03.svn-base +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef MANAGED_SCAN_H -#define MANAGED_SCAN_H - -#include "scan.h" -#include "scanserver/sharedScan.h" - - - -class ManagedScan : public Scan { -public: - static void openDirectory(const std::string& path, IOType type, - int start, int end = -1); - static void closeDirectory(); - - static std::size_t getMemorySize(); - - virtual ~ManagedScan(); - - virtual void setRangeFilter(double max, double min); - virtual void setHeightFilter(double top, double bottom); - virtual void setReductionParameter(double voxelSize, int nrpts = 0, - PointType pointtype = PointType()); - void setShowReductionParameter(double voxelSize, int nrpts = 0, - PointType pointtype = PointType()); - virtual void setOcttreeParameter(double reduction_voxelSize, - double octtree_voxelSize, PointType pointtype, - bool loadOct, bool saveOct); - - virtual const char* getIdentifier() const { return m_shared_scan->getIdentifier(); } - - virtual DataPointer get(const std::string& identifier); - virtual void get(unsigned int types); - virtual DataPointer create(const std::string& identifier, unsigned int size); - virtual void clear(const std::string& identifier); - - virtual unsigned int readFrames(); - virtual void saveFrames(); - virtual unsigned int getFrameCount(); - virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type); - -protected: - virtual void createSearchTreePrivate(); - virtual void calcReducedOnDemandPrivate(); - virtual void addFrame(AlgoType type); - -private: - //! Reference to the shared scan - SharedScan* m_shared_scan; - - //! SharedScan vector to be deleted on closeDirectory - static SharedScanVector* shared_scans; - - //! Flag to keep track of whether we have to update/create reduced points at the start - bool m_reduced_ready; - - //! Flag to reset the persistent frames on write actions in slam to avoid a clear call - bool m_reset_frames_on_write; - - //! Voxelsize of the octtree used for reduction - double show_reduction_voxelSize; - - //! Which point to take out of the reduction octtree, 0 for center - int show_reduction_nrpts; - - //! Pointtype used for the reduction octtree - PointType show_reduction_pointtype; - - - ManagedScan(SharedScan* shared_scan); - - //! Create reduced points for show - void calcReducedShow(); - - //! Create Octtree for show - void createOcttree(); -}; - -#endif //MANAGED_SCAN_H diff --git a/.svn/pristine/7a/7a45e11ec1b989b997aff8206dfc0eecc5ed896d.svn-base b/.svn/pristine/7a/7a45e11ec1b989b997aff8206dfc0eecc5ed896d.svn-base deleted file mode 100644 index 6fbe67e..0000000 --- a/.svn/pristine/7a/7a45e11ec1b989b997aff8206dfc0eecc5ed896d.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @file - * @brief IO of a 3D scan in uos 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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/7e/7e38fcad981dbe5497326fc210013caee144cd78.svn-base b/.svn/pristine/7e/7e38fcad981dbe5497326fc210013caee144cd78.svn-base deleted file mode 100644 index a76008f..0000000 --- a/.svn/pristine/7e/7e38fcad981dbe5497326fc210013caee144cd78.svn-base +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef BASIC_SCAN_H -#define BASIC_SCAN_H - -#include "scan.h" -#include "frame.h" - -#include -#include - - -class BasicScan : public Scan { -public: - static void openDirectory(const std::string& path, IOType type, int start, int end); - static void closeDirectory(); -/* - Scan(const double *euler, int maxDist = -1); - Scan(const double rPos[3], const double rPosTheta[3], int maxDist = -1); - Scan(const double _rPos[3], const double _rPosTheta[3], vector &pts); -*/ - virtual ~BasicScan(); - - virtual void setRangeFilter(double max, double min); - virtual void setHeightFilter(double top, double bottom); - - virtual const char* getIdentifier() const { return m_identifier.c_str(); } - - virtual DataPointer get(const std::string& identifier); - virtual void get(unsigned int types); - virtual DataPointer create(const std::string& identifier, unsigned int size); - virtual void clear(const std::string& identifier); - - virtual unsigned int readFrames(); - virtual void saveFrames(); - virtual unsigned int getFrameCount(); - virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type); - -protected: - virtual void createSearchTreePrivate(); - virtual void calcReducedOnDemandPrivate(); - virtual void addFrame(AlgoType type); - -private: - //! Path and identifier of where the scan is located - std::string m_path, m_identifier; - - IOType m_type; - - double m_filter_max, m_filter_min, m_filter_top, m_filter_bottom; - bool m_filter_range_set, m_filter_height_set; - - std::map> m_data; - - std::vector m_frames; - - - //! Constructor for openDirectory - BasicScan(const std::string& path, const std::string& identifier, IOType type); - - //! Initialization function - void init(); - - void createANNTree(); - - void createOcttree(); -}; - -#endif //BASIC_SCAN_H diff --git a/.svn/pristine/83/83e02dd194de2159c56fa35afd43cd6c2258ef99.svn-base b/.svn/pristine/83/83e02dd194de2159c56fa35afd43cd6c2258ef99.svn-base deleted file mode 100644 index 049aefb..0000000 --- a/.svn/pristine/83/83e02dd194de2159c56fa35afd43cd6c2258ef99.svn-base +++ /dev/null @@ -1,178 +0,0 @@ -/* - * 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* 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/85/8558b059f349f14fc583f6af2ef10c56bd455dee.svn-base b/.svn/pristine/85/8558b059f349f14fc583f6af2ef10c56bd455dee.svn-base deleted file mode 100644 index cd0b3fa..0000000 --- a/.svn/pristine/85/8558b059f349f14fc583f6af2ef10c56bd455dee.svn-base +++ /dev/null @@ -1,234 +0,0 @@ -/* - * feature_mathcer implementation - * - * Copyright (C) HamidReza Houshiar - * - * Released under the GPL version 3. - * - */ - -#include "slam6d/fbr/feature_matcher.h" - -using namespace std; - -namespace fbr{ - - void feature_matcher::init(matcher_method method, int k, double r){ - mMethod = method; - knn = k; - radius = r; - nOfMatches = 0; - nOfFilteredMatches = 0; - } - - feature_matcher::feature_matcher(){ - init(RATIO, 0, 0); - } - - feature_matcher::feature_matcher(matcher_method method){ - if(method == KNN) - init(method, 3, 0); - else if(method == RADIUS) - init(method, 0, 1); - else - init(method, 0, 0); - } - - feature_matcher::feature_matcher(matcher_method method, double p){ - if(method == KNN) - init(method, p, 0); - else if(method == RADIUS) - init(method, 0, p); - else - init(method, 0, 0); - } - - void feature_matcher::match(feature qFeature, feature tFeature){ - vector< cv::DMatch > qtInitialMatches, tqInitialMatches, gMatches; - vector > qtInitialMatchesVector, tqInitialMatchesVector; - //Matching descriptors using one of the mMethods for SURF and SIFT feature descriptors - if(qFeature.getDescriptorMethod() != tFeature.getDescriptorMethod()){ - cout<<"inputs features don't have the same descriptors!"< readDirectory(const char* dir_path, unsigned int start, unsigned int end); - virtual void readPose(const char* dir_path, const char* identifier, double* pose); - virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/88/88c651fb430d20a21ea462e35c81221ed7b479f6.svn-base b/.svn/pristine/88/88c651fb430d20a21ea462e35c81221ed7b479f6.svn-base deleted file mode 100644 index fbb5424..0000000 --- a/.svn/pristine/88/88c651fb430d20a21ea462e35c81221ed7b479f6.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/8a/8aa0fd587375c1ce4a10896c482f7f46e412009b.svn-base b/.svn/pristine/8a/8aa0fd587375c1ce4a10896c482f7f46e412009b.svn-base deleted file mode 100644 index 156461f..0000000 --- a/.svn/pristine/8a/8aa0fd587375c1ce4a10896c482f7f46e412009b.svn-base +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @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* 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/8e/8e8044a660eb823e80d445550a523dfb4f4ad5be.svn-base b/.svn/pristine/8e/8e8044a660eb823e80d445550a523dfb4f4ad5be.svn-base deleted file mode 100644 index 44ecf6b..0000000 --- a/.svn/pristine/8e/8e8044a660eb823e80d445550a523dfb4f4ad5be.svn-base +++ /dev/null @@ -1,544 +0,0 @@ -/* - * 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); - } - } - - //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); - } - } - } - - 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: "< readDirectory(const char* dir_path, unsigned int start, unsigned int end); - virtual void readPose(const char* dir_path, const char* identifier, double* pose); - virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/94/94aad30f03099c1bb55ea4a9d35ba4b1d7ebbf43.svn-base b/.svn/pristine/94/94aad30f03099c1bb55ea4a9d35ba4b1d7ebbf43.svn-base deleted file mode 100644 index 4119b36..0000000 --- a/.svn/pristine/94/94aad30f03099c1bb55ea4a9d35ba4b1d7ebbf43.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base b/.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base deleted file mode 100644 index 35aed9d..0000000 --- a/.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base +++ /dev/null @@ -1,410 +0,0 @@ -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/98/981260cea4afb6b9d6083b246f02e4ab7cc78183.svn-base b/.svn/pristine/98/981260cea4afb6b9d6083b246f02e4ab7cc78183.svn-base deleted file mode 100644 index 4b96439..0000000 --- a/.svn/pristine/98/981260cea4afb6b9d6083b246f02e4ab7cc78183.svn-base +++ /dev/null @@ -1,210 +0,0 @@ -/* - * 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* 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/99/99042653e5414e8d0ef85202b942814bbe7741fa.svn-base b/.svn/pristine/99/99042653e5414e8d0ef85202b942814bbe7741fa.svn-base deleted file mode 100644 index 6165357..0000000 --- a/.svn/pristine/99/99042653e5414e8d0ef85202b942814bbe7741fa.svn-base +++ /dev/null @@ -1,191 +0,0 @@ -/** - * @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); - - //! 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(); - 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 geHeightTop() 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; - bool 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_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/26/2638410539b1ae55fb27e0ce177aeb5f4ff37aec.svn-base b/.svn/pristine/99/993014ebe3bd111ee6476ca3768d71d8dffcaa3c.svn-base similarity index 89% rename from .svn/pristine/26/2638410539b1ae55fb27e0ce177aeb5f4ff37aec.svn-base rename to .svn/pristine/99/993014ebe3bd111ee6476ca3768d71d8dffcaa3c.svn-base index 112eed9..bffe383 100644 --- a/.svn/pristine/26/2638410539b1ae55fb27e0ce177aeb5f4ff37aec.svn-base +++ b/.svn/pristine/99/993014ebe3bd111ee6476ca3768d71d8dffcaa3c.svn-base @@ -13,6 +13,15 @@ using namespace std; namespace fbr{ + scan_cv::scan_cv(string dir, unsigned int number, IOType format, bool scanServer){ + sDir = dir; + sNumber = number; + sFormat = format; + zMax = numeric_limits::min(); + zMin = numeric_limits::max(); + nPoints = 0; + scanserver = scanServer; + } scan_cv::scan_cv(string dir, unsigned int number, IOType format){ sDir = dir; sNumber = number; @@ -20,10 +29,10 @@ namespace fbr{ zMax = numeric_limits::min(); zMin = numeric_limits::max(); nPoints = 0; - } - + scanserver = false; + } + void scan_cv::convertScanToMat(){ - bool scanserver = false; Scan::openDirectory(scanserver, sDir, sFormat, sNumber, sNumber); if(Scan::allScans.size() == 0){ cerr << "No scans found. Did you use the correct format?" < -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* 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/a1/a14e8af6728cf25d35e5998e359a86e9a97c0f16.svn-base b/.svn/pristine/a1/a14e8af6728cf25d35e5998e359a86e9a97c0f16.svn-base deleted file mode 100644 index e7d1d27..0000000 --- a/.svn/pristine/a1/a14e8af6728cf25d35e5998e359a86e9a97c0f16.svn-base +++ /dev/null @@ -1,54 +0,0 @@ -if(WIN32) - add_library(pointfilter STATIC ../slam6d/pointfilter.cc) -else(WIN32) - add_library(pointfilter SHARED ../slam6d/pointfilter.cc) -endif(WIN32) - -set(SCANIO_LIBNAMES - uos uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne -) - -if(WITH_RIVLIB) - set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} rxp) - if(LIBXML2_FOUND) - include_directories(${LIBXML2_INCLUDE_DIR}) -# set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} riegl_project) -# target_link_libraries(scan_io_riegl_project ${RIVLIB} scan_io_rxp ${LIBXML2_LIBRARIES}) - endif(LIBXML2_FOUND) -endif(WITH_RIVLIB) - -#IF (WITH_CAD) -# IF(NOT WIN32) -# add_library(scan_io_cad SHARED scan_io_cad.cc) -# target_link_libraries(scan_io_cad ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) -# ENDIF(NOT WIN32) -#ENDIF (WITH_CAD) - -foreach(libname ${SCANIO_LIBNAMES}) -if(WIN32) - #add_library(scan_io_${libname} STATIC scan_io_${libname}.cc) - add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) -else(WIN32) - add_library(scan_io_${libname} SHARED scan_io_${libname}.cc) -endif(WIN32) - target_link_libraries(scan_io_${libname} pointfilter ${Boost_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) -endforeach(libname) - -if(WITH_RIVLIB) - target_link_libraries(scan_io_rxp ${RIVLIB}) - if(LIBXML2_FOUND) - target_link_libraries(scan_io_rxp ${LIBXML2_LIBRARIES}) #scan_io_riegl_project ${RIVLIB}) - endif(LIBXML2_FOUND) -endif(WITH_RIVLIB) - -if(WIN32) - add_library(scanio STATIC scan_io.cc ../slam6d/io_types.cc) -else(WIN32) - add_library(scanio SHARED scan_io.cc ../slam6d/io_types.cc) -endif(WIN32) - -if(UNIX) - target_link_libraries(scanio dl) -endif(UNIX) - - diff --git a/.svn/pristine/a3/a339abce00e5fad291d9254e33d2d31d2f896586.svn-base b/.svn/pristine/a3/a339abce00e5fad291d9254e33d2d31d2f896586.svn-base deleted file mode 100644 index b209cb5..0000000 --- a/.svn/pristine/a3/a339abce00e5fad291d9254e33d2d31d2f896586.svn-base +++ /dev/null @@ -1,184 +0,0 @@ -/* - * 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* 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/a9/a9b4d93ac03e9cc4801286237eb28c62b5e270e9.svn-base b/.svn/pristine/a9/a9b4d93ac03e9cc4801286237eb28c62b5e270e9.svn-base deleted file mode 100644 index 8a91344..0000000 --- a/.svn/pristine/a9/a9b4d93ac03e9cc4801286237eb28c62b5e270e9.svn-base +++ /dev/null @@ -1,413 +0,0 @@ -#ifndef SCAN_H -#define SCAN_H - -#include "io_types.h" -#include "data_types.h" -#include "point_type.h" -#include "ptpair.h" - -#include -#include - -#include -#include - -//! SearchTree types -enum nns_type { - simpleKD, ANNTree, BOCTree -}; - -class Scan; -typedef std::vector ScanVector; - -class SearchTree; -class ANNkd_tree; - -/** HOWTO scan -First: Load scans (if you want to use the scanmanager, use ManagedScan) - - BasicScan/ManagedScan::openDirectory(path, type, start, end); - -Pass it to functions (by reference to link it to the same instance) or store it in a global variable - -After loading you might want to set parameters - - for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { - Scan* scan = *it; - scan->setRangeFilter(maxDist, minDist); - scan->setHeightFilter(top, bottom); // thermo - scan->setReductionParameter(voxelSize, nrpts[, pointtype]); - scan->setSearchTreeParameter(nns_method, use_cuda); - } - -Access the contained data, will be loaded and calculated on demand - - DataXYZ xyz = scan->get("xyz"); - DataXYZ reduced = scan->get("xyz reduced"); - DataRGB rgb = scan->get("rgb"); - - xyz[i][0..2] - reflectance[i] - - unsigned int size = scan->size("xyz reduced"); - -In order to use the prefetching of all requested data field in the scanserver, mark them for use. This is relevant for efficiency, which would otherwise cause loading the files each time another data field is requested. - - scan->get(DATA_XYZ | DATA_RGB | ...); - -Under circumstances the data fields are not available (e.g. no color in uos-type scans) - - DataRGB rgb = scan->get("rgb"); - if(rgb.valid()) { ok, do something } - -If backward-compability to pointer arrays is needed, the PointerArray class can adapt - - BOctTree(PointerArray(scan->get("xyz")).get(), scan->size("xyz"), ...); - -If data isn't needed anymore, flag it for removal - - scan->clear("xyz"); - scan->clear(DATA_XYZ | DATA_RGB | ...); - -Creating data fields with the correct byte size - - scan->create("xyz somethingelse", sizeof(double)*3*N); - -Reading frames in show: - - unsigned int size = scan->readFrames(); - - const double* pose; - AlgoType type; - scan->getFrame(i, pose, type); - -Last, if program ends, clean up - - Scan::closeDirectory(scans); - -**/ - - - -/** - * This class bundles common features of different scan implementations by - * abstraction. It handles the algorithmic parts and leaves IO and other - * features to the deriving classes. - */ -class Scan { - //friend class SearchTree; // TODO: is this neccessary? -public: - enum AlgoType { - INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO, - GRAPHHOGMAN - }; - - // delete copy-ctor and assignment, scans shouldn't be copied by basic class - // Scan(const Scan& other) = delete; - // Scan& operator=(const Scan& other) = delete; - - virtual ~Scan(); - - //! Holder of all scans - also used in transform for adding frames for each scan at the same time - static std::vector allScans; - - /** - * Attempt to read a directory under \a path and return its read scans. - * No scans are loaded at this point, only checked if all exist. - * - * @param scanserver whether to use managed scans in the scanserver or not - * @param path to the directory containing the scans - * @param type determining which ScanIO to use - * @param start first scan to use - * @param end last scan to use, -1 means from start to last available - */ - static void openDirectory(bool scanserver, const std::string& path, IOType type, - int start, int end = -1); - - /** - * "Close" a directory by deleting all its scans and emptying the - * Scan::allScans vector. - */ - static void closeDirectory(); - - - /* Input filtering and parameter functions */ - - - //! Input filtering for all points based on their euclidean length - virtual void setRangeFilter(double max, double min) = 0; - - //! Input filtering for all points based on their height - virtual void setHeightFilter(double top, double bottom) = 0; - - //! Set reduction parameters, but don't reduce yet - virtual void setReductionParameter(double voxelSize, int nrpts = 0, - PointType pointtype = PointType()); - - //! Set SearchTree type, but don't create it yet - void setSearchTreeParameter(int nns_method, bool cuda_enabled); - - /** - * Set octtree parameters for show - * @param loadOct will load the serialized octtree from disk regardless - * @param saveOct serialize octtree if not loaded by loadOct after creation - */ - virtual void setOcttreeParameter(double reduction_voxelSize, - double octtree_voxelSize, PointType pointtype, - bool loadOct, bool saveOct); - - - /* Basic getter functions */ - - - inline const double* get_rPos() const; - inline const double* get_rPosTheta() const; - inline const double* get_rPosQuat() const; - //! Pose matrix after initial and match transformations (org+dalign) - inline const double* get_transMat() const; - //! Original pose matrix after initial transform - inline const double* get_transMatOrg() const; - //! Accumulated delta transformation matrix - inline const double* getDAlign() const; - - inline SearchTree* getSearchTree(); - inline ANNkd_tree* getANNTree() const; - - virtual const char* getIdentifier() const = 0; - - //! Determine the maximum number of reduced points in \a scans - static unsigned int getMaxCountReduced(ScanVector& scans); - - - /* Functions for altering data fields, implementation specific */ - - - /** - * Get the data field \a identifier, calculate it on demand if neccessary. - * - * If "xyz reduced" or "xyz reduced original" is requested, the reduction is - * started with "xyz" as input. - */ - virtual DataPointer get(const std::string& identifier) = 0; - - /** - * Load the requested IODataTypes, joined by |, from the scan file. - * - * This feature is neccessary to load multiple data fields at once, not all - * one by one with each get("...") access. - */ - virtual void get(unsigned int types) = 0; - - /** - * Creates a data field \a identifier with \a size bytes. - */ - virtual DataPointer create(const std::string& identifier, unsigned int size) = 0; - - /** - * Clear the data field \a identifier, removing its allocated memory if - * possible or marking it for prioritized removal. - */ - virtual void clear(const std::string& identifier) = 0; - - //! Extension to clear for more than one identifier, e.g. clear(DATA_XYZ | DATA_RGB); - void clear(unsigned int types); - - /** - * Get the size of \a identifier as if it were requested and size() called - * upon its type specialized DataPointer class. - * e.g size("xyz reduced") - */ - template - unsigned int size(const std::string& identifier) { - return (T(get(identifier))).size(); - } - - - /* Frame handling functions */ - - - /** - * Open the .frames-file and read its contents. If not read, the frame list - * will be empty. - * @return count of frames if file has been read, zero otherwise - */ - virtual unsigned int readFrames() = 0; - - /** - * Write the accumulated frames into a .frames-file. - */ - virtual void saveFrames() = 0; - - //! Count of frames - virtual unsigned int getFrameCount() = 0; - - //! Get contents of a frame, pass matrix pointer and type by reference - virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) = 0; - -protected: - /** - * Called from transform, this will add its current transMat pose with - * the given type as a frame into the list of frames - */ - virtual void addFrame(AlgoType type) = 0; - -public: - - - /* Direct creation of reduced points and search tree */ - - - //! Apply reduction and initial transMatOrg transformation - void toGlobal(); - - //! Copy reduced points to original and create search tree on it - void createSearchTree(); - - - /* Common transformation and matching functions */ - void mergeCoordinatesWithRoboterPosition(Scan* prevScan); - void transformAll(const double alignxf[16]); - void transformAll(const double alignQuat[4], const double alignt[3]); - - void transform(const double alignxf[16], - const AlgoType type, int islum = 0); - void transform(const double alignQuat[4], - const double alignt[3], const AlgoType type, int islum = 0); - void transformToMatrix(double alignxf[16], - const AlgoType type, int islum = 0); - void transformToEuler(double rP[3], double rPT[3], - const AlgoType type, int islum = 0); - void transformToQuat(double rP[3], double rPQ[4], - const AlgoType type, int islum = 0); - - // Scan matching functions - static void getPtPairs(std::vector *pairs, - Scan* Source, Scan* Target, - int thread_num, - int rnd, double max_dist_match2, double &sum, - double *centroid_m, double *centroid_d); - static void getNoPairsSimple(std::vector &diff, - Scan* Source, Scan* Target, - int thread_num, - double max_dist_match2); - static void getPtPairsSimple(std::vector *pairs, - Scan* Source, Scan* Target, - int thread_num, - int rnd, double max_dist_match2, - double *centroid_m, double *centroid_d); - static void getPtPairsParallel(std::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]); - -protected: - /** - * The pose of the scan - * Note: rPos/rPosTheta and transMat _should_ - * always represent the same pose!!! - */ - double rPos[3], //!< 3D position - rPosTheta[3], //!< 3D rotation in Euler representation - rQuat[4], //!< 3D rotation in Quaternion representation - transMat[16], //!< (4x4) transformation matrix - transMatOrg[16]; //!< The original pose of the scan, e.g., from odometry - - /** - * The dalignxf transformation represents the delta transformation virtually applied - * to the tree and is used to compute are actual corresponding points. - */ - double dalignxf[16]; - - //! Run ICP on GPU instead of CPU - bool cuda_enabled; - - //! Defines the method used for nearest neighbor search and which tree to use - int nns_method; - - //! SearchTree for point pair matching, works on the search points - SearchTree* kd; - - //! This KD tree is created only for the CUDA usages - ANNkd_tree* ann_kd_tree; - - //! Voxelsize of the octtree used for reduction - double reduction_voxelSize; - - //! Which point to take out of the reduction octtree, 0 for center - int reduction_nrpts; - - //! Pointtype used for the reduction octtree - PointType reduction_pointtype; - - //! Type of the searchtree to be created - int searchtree_nnstype; - - //! Use CUDA for searching - bool searchtree_cuda_enabled; - - //! Flag whether "xyz reduced" has been initialized for this Scan yet - bool m_has_reduced; - - //! Reduction value used for octtree input - double octtree_reduction_voxelSize; - - //! Voxelsize used in the octtree itself - double octtree_voxelSize; - - //! Pointtype for the Octtree - PointType octtree_pointtype; - - //! Flags to load or save the octtrees from/to storage - bool octtree_loadOct, octtree_saveOct; - - /** - * Basic initializing constructor calling the initalization function. - * Can only be called from deriving classes. - */ - Scan(); - - /** - * This function handles the reduction of points. It builds a lock for - * multithread-safety and calls caldReducedOnDemandPrivate. - * - * The intention is to reduce points, transforme them to the initial pose and - * then copy them to original for the SearchTree. - */ - void calcReducedOnDemand(); - - //! Create specific SearchTree variants matching the capability of the Scan - virtual void createSearchTreePrivate() = 0; - - //! Create reduced points in a multithread-safe environment matching the capability of the Scan - virtual void calcReducedOnDemandPrivate() = 0; - - //! Internal function of transform which alters the reduced points - void transformReduced(const double alignxf[16]); - - //! Internal function of transform which handles the matrices - void transformMatrix(const double alignxf[16]); - - //! Creating reduced points - void calcReducedPoints(); - - //! Copies reduced points to original points without any transformation. - void copyReducedToOriginal(); - - //! Inverse functionality of copyReducedToOriginal. - void copyOriginalToReduced(); - -private: - //! flag for openDirectory and closeDirectory to distinguish the scans - static bool scanserver; - - //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment - // it can not be compiled in wein32 use boost 1.48, therefore we remeove it temporarily -// boost::mutex m_mutex_reduction, m_mutex_create_tree; -}; - -#include "scan.icc" - -#endif //SCAN_H diff --git a/.svn/pristine/4b/4bf1b18c6d10bddab8417bca4a084c618d997cdd.svn-base b/.svn/pristine/aa/aad02cf22e8bc65e9505091a1025edcfa9bbde97.svn-base similarity index 59% rename from .svn/pristine/4b/4bf1b18c6d10bddab8417bca4a084c618d997cdd.svn-base rename to .svn/pristine/aa/aad02cf22e8bc65e9505091a1025edcfa9bbde97.svn-base index 79cf380..8bc956d 100644 --- a/.svn/pristine/4b/4bf1b18c6d10bddab8417bca4a084c618d997cdd.svn-base +++ b/.svn/pristine/aa/aad02cf22e8bc65e9505091a1025edcfa9bbde97.svn-base @@ -24,6 +24,7 @@ namespace fbr{ class feature{ feature_detector_method fDetectorMethod; feature_descriptor_method fDescriptorMethod; + feature_filtration_method fFiltrationMethod; vector keypoints; cv::Mat descriptors; enum description_method{ @@ -31,19 +32,40 @@ namespace fbr{ DESCRIPTOR_DESCRIPTION, ALL, }; + void featureFiltration(cv::Mat pImage, cv::Mat rImage); public: /** * constructor of feature class */ feature(); + feature(feature_detector_method detector, feature_descriptor_method descriptor, feature_filtration_method filtration); + /** + * featureDetection : creates the features + * @param pImage cv::Mat refletance panorama image + * @param method feature_detector_method + * @param rImage cv::Mat range image for filtration + * @param fMethod feature_filtration_method + **/ + void featureDetection(cv::Mat pImage, feature_detector_method method, cv::Mat rImage, feature_filtration_method fMethod); void featureDetection(cv::Mat pImage, feature_detector_method method); void featureDetection(cv::Mat pImage); + /** + * featureDescription : creates descriptor of detected features + * @param pImage cv::Mat reflectance panorama image + * @param method feature_descriptor_method + **/ void featureDescription(cv::Mat pImage, feature_descriptor_method method); void featureDescription(cv::Mat pImage); + /** + * all sets of getters for the class + */ feature_detector_method getDetectorMethod(); feature_descriptor_method getDescriptorMethod(); + feature_filtration_method getFeatureFiltrationMethod(); vector getFeatures(); cv::Mat getDescriptors(); + void setFeatures(vector keypoint); + void setDescriptors(cv::Mat descriptor); void getDescription(description_method method=ALL); unsigned int getNumberOfFeatures(); }; diff --git a/.svn/pristine/ab/ab9a0fd1b9110d10fecaef454992190248c843de.svn-base b/.svn/pristine/ab/ab9a0fd1b9110d10fecaef454992190248c843de.svn-base deleted file mode 100644 index 81f53b4..0000000 --- a/.svn/pristine/ab/ab9a0fd1b9110d10fecaef454992190248c843de.svn-base +++ /dev/null @@ -1,37 +0,0 @@ -Project admins - -Andreas Nuechter andreas@nuechti.de -Kai Lingemann kai.lingemann@gmx.de -Dorit Borrmann d.borrmann@jacobs-university.de - -List of contributors - -Andreas Nuechter andreas@nuechti.de -Kai Lingemann kai.lingemann@gmx.de -Dorit Borrmann d.borrmann@jacobs-university.de -Jan Elseberg j.elseberg@jacobs-university.de -Jochen Sprickerhof jochen@sprickerhof.de -HamidReza Houshiar h.houshiar@jacobs-university.de -Sven Albrecht sven.albrecht@uni-osnabrueck.de -Stan Serebryakov cfr.ssv@gmail.com -Thomas Escher tescher@uni-osnabrueck.de -Thomas Wiemann twiemann@uni-osnabrueck.de -Alexandru Tandrau alexandru@tandrau.com -Alexandru Eugen Ichim eugen@alexichim.com -Flavia Grosan me@flaviagrosan.com -Deyuan Qiu deyuan.qiu@googlemail.com -Darko Makreshanski d.makreshanski@jacobs-university.de -Mohammad Faisal Abdullah m.faisal@jacobs-university.de -Li Ming liming751218@whu.edu.cn -Li Wei xpaulee@gmail.com -Shams Feyzabadi sh.feyzabadi@gmail.co -Vladislav Perelmann v.perelman@jacobs-university.de -Chen Long lchen.whu@gmail.com -Remuas Dumitru r.dumitru@jaocbs-university.de - -Further contributors - -Uwe Hebbelmann, Sebastian Stock, Andre Schemschat -Hartmut Surmann -Amuz Tamrakars, Ulugbek Makhmudov -Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz \ No newline at end of file diff --git a/.svn/pristine/b0/b0899606b686fa7de519588d6e0f4b2e5114e315.svn-base b/.svn/pristine/b0/b0899606b686fa7de519588d6e0f4b2e5114e315.svn-base deleted file mode 100644 index cad527e..0000000 --- a/.svn/pristine/b0/b0899606b686fa7de519588d6e0f4b2e5114e315.svn-base +++ /dev/null @@ -1,121 +0,0 @@ -### SCAN_RED - -IF(WITH_SCAN_REDUCTION) - add_executable(scan_red scan_red.cc) - - IF(UNIX) - target_link_libraries(scan_red scan dl ANN) - ENDIF(UNIX) - - IF (WIN32) - target_link_libraries(scan_red scan ANN XGetopt) - ENDIF(WIN32) -ENDIF(WITH_SCAN_REDUCTION) - -### SCAN_DIFF -IF(WITH_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) -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) - 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/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base b/.svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base deleted file mode 100644 index dc95bb1..0000000 --- a/.svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file - * @brief Scan types and mapping functions. - * - * @author Thomas Escher, Billy Okal - */ - -#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, 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_AMPLITUDE = 1<<3, - DATA_TYPE = 1<<4, - DATA_DEVIATION = 1<<5 -}; - -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/b3/b3dc22d5dd32cd1944fd0faaaebf87a53fe5ebee.svn-base b/.svn/pristine/b3/b3dc22d5dd32cd1944fd0faaaebf87a53fe5ebee.svn-base deleted file mode 100644 index 39502ea..0000000 --- a/.svn/pristine/b3/b3dc22d5dd32cd1944fd0faaaebf87a53fe5ebee.svn-base +++ /dev/null @@ -1,90 +0,0 @@ -/** - * @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 -#include -#include -#include - -#include "scanserver/io_types.h" -#include "scanserver/pointfilter.h" - - - -/** - * @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* 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/b7/b76c973a65c7eeca48efd6354118c18ab53822db.svn-base b/.svn/pristine/b7/b76c973a65c7eeca48efd6354118c18ab53822db.svn-base deleted file mode 100644 index e64d813..0000000 --- a/.svn/pristine/b7/b76c973a65c7eeca48efd6354118c18ab53822db.svn-base +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file - * @brief IO of a 3D scan in rts file format - * @author Thomas Escher - */ - -#ifndef __SCAN_IO_UOS_H__ -#define __SCAN_IO_UOS_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* 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/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base b/.svn/pristine/b8/b8577205bc1d064eb45065d3f2c765f76e36976b.svn-base similarity index 97% rename from .svn/pristine/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base rename to .svn/pristine/b8/b8577205bc1d064eb45065d3f2c765f76e36976b.svn-base index a41cd2d..6848f12 100644 --- a/.svn/pristine/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base +++ b/.svn/pristine/b8/b8577205bc1d064eb45065d3f2c765f76e36976b.svn-base @@ -226,8 +226,11 @@ DataPointer ManagedScan::create(const std::string& identifier, unsigned int size 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."); + if(identifier == "reflectance") { + return m_shared_scan->createReflectance(size / (1*sizeof(double))); + } else + { + throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::create. Upgrade SharedScan for this data field."); } } diff --git a/.svn/pristine/bc/bc4f85bc33d567e7ebf04c3645a2e2afa7aee5ac.svn-base b/.svn/pristine/bc/bc4f85bc33d567e7ebf04c3645a2e2afa7aee5ac.svn-base new file mode 100644 index 0000000..5f25b80 --- /dev/null +++ b/.svn/pristine/bc/bc4f85bc33d567e7ebf04c3645a2e2afa7aee5ac.svn-base @@ -0,0 +1,222 @@ +/* + * feature_mathcer implementation + * + * Copyright (C) HamidReza Houshiar + * + * Released under the GPL version 3. + * + */ + +#include "slam6d/fbr/feature_matcher.h" + +using namespace std; + +namespace fbr{ + + void feature_matcher::init(matcher_method method, int k, double r, matching_filtration_method filtration){ + mMethod = method; + knn = k; + radius = r; + nOfMatches = 0; + nOfFilteredMatches = 0; + mFiltrationMethod = filtration; + } + + feature_matcher::feature_matcher(){ + init(RATIO, 0, 0, DISABLE_MATCHING_FILTER); + } + + feature_matcher::feature_matcher(matcher_method method){ + if(method == KNN) + init(method, 3, 0, DISABLE_MATCHING_FILTER); + else if(method == RADIUS) + init(method, 0, 1, DISABLE_MATCHING_FILTER); + else + init(method, 0, 0, DISABLE_MATCHING_FILTER); + } + + feature_matcher::feature_matcher(matcher_method method, double p){ + if(method == KNN) + init(method, p, 0, DISABLE_MATCHING_FILTER); + else if(method == RADIUS) + init(method, 0, p, DISABLE_MATCHING_FILTER); + else + init(method, 0, 0, DISABLE_MATCHING_FILTER); + } + + feature_matcher::feature_matcher(matcher_method method, double p, matching_filtration_method filtration){ + if(method == KNN) + init(method, p, 0, filtration); + else if(method == RADIUS) + init(method, 0, p, filtration); + else + init(method, 0, 0, filtration); + } + + void feature_matcher::findMatches(feature qFeature, feature tFeature){ + vector< cv::DMatch > qtInitialMatches; + vector > qtInitialMatchesVector; + if(qFeature.getFeatures().size() == 0 || tFeature.getFeatures().size() == 0){ + cout<<"No features has found in one or both scans!!"< -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 "slam6d/scan.h" -#ifdef WITH_SCANSERVER -#include "scanserver/clientInterface.h" -#endif //WITH_SCANSERVER - -#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 - - -/** - * 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 - << 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) -{ - int c; - // from unistd.h: - extern char *optarg; - extern int optind; - - /* 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' }, - { 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': - start = atoi(optarg); - if (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); } - 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; - 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; - } - - 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, 2010" << 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; - - parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc); - -#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); - } -#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 - 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; - 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; - 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::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; - - int thread_num = 0; - vector diff; - double transMat[16]; - - if(desc) { - Scan::getNoPairsSimple(diff, Scan::allScans[endIndex], Scan::allScans[0], 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); - 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(); - - delete Scan::allScans[0]; - Scan::allScans.clear(); - - cout << endl << endl; - cout << "Normal program end." << endl << endl; -} diff --git a/.svn/pristine/c6/c6d9b992f3fbba4d552e92e9a2be5beef93a8977.svn-base b/.svn/pristine/c6/c6d9b992f3fbba4d552e92e9a2be5beef93a8977.svn-base deleted file mode 100644 index f69b5dc..0000000 --- a/.svn/pristine/c6/c6d9b992f3fbba4d552e92e9a2be5beef93a8977.svn-base +++ /dev/null @@ -1,388 +0,0 @@ -/* - * 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_filter_range_set = false; - m_filter_height_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::get(unsigned int types) -{ - ScanIO* sio = ScanIO::getScanIO(m_type); - - vector xyz; - vector rgb; - vector reflectance; - 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); - - sio->readScan(m_path.c_str(), - m_identifier.c_str(), - filter, - &xyz, - &rgb, - &reflectance, - &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_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 == "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/f5/f5ab2bbd50dc2ab4cf98b901dc3ad06a9d485f95.svn-base b/.svn/pristine/c7/c72f40b9ef40f81592743b6b80469d0844006443.svn-base similarity index 78% rename from .svn/pristine/f5/f5ab2bbd50dc2ab4cf98b901dc3ad06a9d485f95.svn-base rename to .svn/pristine/c7/c72f40b9ef40f81592743b6b80469d0844006443.svn-base index 429bf21..ca5d467 100644 --- a/.svn/pristine/f5/f5ab2bbd50dc2ab4cf98b901dc3ad06a9d485f95.svn-base +++ b/.svn/pristine/c7/c72f40b9ef40f81592743b6b80469d0844006443.svn-base @@ -30,21 +30,29 @@ namespace fbr{ double radius; unsigned int nOfMatches; unsigned int nOfFilteredMatches; + matching_filtration_method mFiltrationMethod; - void init(matcher_method method, int k, double r); + void init(matcher_method method, int k, double r, matching_filtration_method filtration); + void findMatches(feature qFeature, feature tFeature); public: feature_matcher(); feature_matcher(matcher_method method); feature_matcher(matcher_method method, double p); + feature_matcher(matcher_method method, double p, matching_filtration_method filtration); + void match(feature qFeature, feature tFeature); + vector getMatches(); matcher_method getMatcherMethod(); + matching_filtration_method getMatchingFiltrationMethod(); unsigned int getKnn(); double getRadius(); unsigned int getNumberOfMatches(); unsigned int getNumberOfFilteredMatches(); void getDescription(); + + void setMatchingFiltrationMethod(matching_filtration_method method); }; } #endif /* FEATURE_MATCHER_H_ */ diff --git a/.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base b/.svn/pristine/c9/c94a2dbcd5aaf61d12d04948a745e39d2817debc.svn-base similarity index 99% rename from .svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base rename to .svn/pristine/c9/c94a2dbcd5aaf61d12d04948a745e39d2817debc.svn-base index 6dbf896..e488f0b 100644 --- a/.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base +++ b/.svn/pristine/c9/c94a2dbcd5aaf61d12d04948a745e39d2817debc.svn-base @@ -678,7 +678,7 @@ namespace fbr{ 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); + float phi = Long0 + (1./n) * ::atan2(x, Rho0 - y); phi *= 180.0 / M_PI; phi = 360.0 - phi; diff --git a/.svn/pristine/ca/ca80512c8fe75d732a8b699062e271ddc38b6b5d.svn-base b/.svn/pristine/ca/ca80512c8fe75d732a8b699062e271ddc38b6b5d.svn-base deleted file mode 100644 index dfd4762..0000000 --- a/.svn/pristine/ca/ca80512c8fe75d732a8b699062e271ddc38b6b5d.svn-base +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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_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/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base b/.svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base deleted file mode 100644 index 088c27a..0000000 --- a/.svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base +++ /dev/null @@ -1,171 +0,0 @@ -/* - * scan_io_uosr implementation - * - * Copyright (C) Billy Okal - * - * Released under the GPL version 3. - * - */ - - -/** - * @file scan_io_uosr.cc - * @brief IO of a 3D scan in uos file format plus a - * reflectance/intensity/temperature value - * @author Billy Okal. Jacobs University Bremen, Germany. - */ - -#include "scanio/scan_io_uosr.h" - -#include -using std::cout; -using std::cerr; -using std::endl; -#include - -#ifdef _MSC_VER -#include -#endif - -#include -#include -using namespace boost::filesystem; - -#include "slam6d/globals.icc" - - - -#define DATA_PATH_PREFIX "scan" -#define DATA_PATH_SUFFIX ".3d" -#define POSE_PATH_PREFIX "scan" -#define POSE_PATH_SUFFIX ".pose" - - - -std::list ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end) -{ - std::list identifiers; - for(unsigned int i = start; i <= end; ++i) { - // identifier is /d/d/d (000-999) - std::string identifier(to_string(i,3)); - // scan consists of data (.3d) and pose (.pose) files - path data(dir_path); - data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); - path pose(dir_path); - pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); - // stop if part of a scan is missing or end by absence is detected - if(!exists(data) || !exists(pose)) - break; - identifiers.push_back(identifier); - } - return identifiers; -} - -void ScanIO_uosr::readPose(const char* dir_path, const char* identifier, double* pose) -{ - unsigned int i; - - path pose_path(dir_path); - pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX); - if(!exists(pose_path)) - throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]"); - - // open pose file - ifstream pose_file(pose_path); - - // if the file is open, read contents - if(pose_file.good()) { - // read 6 plain doubles - for(i = 0; i < 6; ++i) pose_file >> pose[i]; - pose_file.close(); - - // convert angles from deg to rad - for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]); - } else { - throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]"); - } -} - -bool ScanIO_uosr::supports(IODataType type) -{ - return !!(type & ( DATA_REFLECTANCE | DATA_XYZ )); -} - -void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation) -{ - unsigned int i; - - // error handling - path data_path(dir_path); - data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX); - if(!exists(data_path)) - throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]"); - - if(xyz != 0) { - // open data file - ifstream data_file(data_path); - data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit); - - // overread the first line ignoring the header information - char dummy[255]; - data_file.getline(dummy, 255); - - // read points and reflectance/intensity/temperature value - double point[3]; - float reflection; - while(data_file.good()) { - try { - for(i = 0; i < 3; ++i) data_file >> point[i]; - data_file >> reflection; - } catch(std::ios_base::failure& e) { - break; - } - - // apply filter then insert point and reflectance - if(filter.check(point)) { - for(i = 0; i < 3; ++i) xyz->push_back(point[i]); - reflectance->push_back(reflection); - } - } - data_file.close(); - } -} - - - -/** - * class factory for object construction - * - * @return Pointer to new object - */ -#ifdef _MSC_VER -extern "C" __declspec(dllexport) ScanIO* create() -#else -extern "C" ScanIO* create() -#endif -{ - return new ScanIO_uosr; -} - - -/** - * class factory for object construction - * - * @return Pointer to new object - */ -#ifdef _MSC_VER -extern "C" __declspec(dllexport) void destroy(ScanIO *sio) -#else -extern "C" void destroy(ScanIO *sio) -#endif -{ - delete sio; -} - -#ifdef _MSC_VER -BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved) -{ - return TRUE; -} -#endif - diff --git a/.svn/pristine/cd/cd204dc38a13e943910a2bca09ff2e4e0a9450f9.svn-base b/.svn/pristine/cd/cd204dc38a13e943910a2bca09ff2e4e0a9450f9.svn-base deleted file mode 100644 index 878000c..0000000 --- a/.svn/pristine/cd/cd204dc38a13e943910a2bca09ff2e4e0a9450f9.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/cf/cf648c1f7b309705a9b14b550ce8879c79995ca8.svn-base b/.svn/pristine/cf/cf648c1f7b309705a9b14b550ce8879c79995ca8.svn-base deleted file mode 100644 index a1b55ec..0000000 --- a/.svn/pristine/cf/cf648c1f7b309705a9b14b550ce8879c79995ca8.svn-base +++ /dev/null @@ -1,259 +0,0 @@ -/* - * 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_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::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); - 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(); -} - -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/d1/d13ff10647a40654467dc2c9b310e3f1253e8696.svn-base b/.svn/pristine/d1/d13ff10647a40654467dc2c9b310e3f1253e8696.svn-base deleted file mode 100644 index d2c0a35..0000000 --- a/.svn/pristine/d1/d13ff10647a40654467dc2c9b310e3f1253e8696.svn-base +++ /dev/null @@ -1,146 +0,0 @@ -/* - * 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, "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, "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 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 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/d1/d1a7f039c995aa109e797d0b0a562acbfa31dcc9.svn-base b/.svn/pristine/d1/d1a7f039c995aa109e797d0b0a562acbfa31dcc9.svn-base deleted file mode 100644 index e659c19..0000000 --- a/.svn/pristine/d1/d1a7f039c995aa109e797d0b0a562acbfa31dcc9.svn-base +++ /dev/null @@ -1,209 +0,0 @@ -/* - * point_type implementation - * - * Copyright (C) Jan Elseberg - * - * 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[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[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++; -} - -bool PointType::hasReflectance() { - return hasType(USE_REFLECTANCE); -} -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::getAmplitude() { - return dimensionmap[2]; -} - -unsigned int PointType::getDeviation() { - return dimensionmap[3]; -} - -unsigned int PointType::getTime() { - return dimensionmap[6]; -} - -unsigned int PointType::getIndex() { - return dimensionmap[7]; -} - -unsigned int PointType::getType() { - return dimensionmap[4]; -} - -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_AMPLITUDE) { - return dimensionmap[2]; - } else if (type == USE_DEVIATION) { - return dimensionmap[3]; - } else if (type == USE_TYPE) { - return dimensionmap[4]; - } else if (type == USE_COLOR) { - return dimensionmap[5]; - } else if (type == USE_TIME) { - return dimensionmap[6]; - } 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_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; - - -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; - - // 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(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(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_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(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/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base b/.svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base deleted file mode 100644 index 47c23c4..0000000 --- a/.svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base +++ /dev/null @@ -1,413 +0,0 @@ -/* - * feature_based_registration implementation - * - * Copyright (C) HamidReza Houshiar - * - * Released under the GPL version 3. - * - */ - -#include -#include -#include "slam6d/fbr/fbr_global.h" -#include "slam6d/fbr/scan_cv.h" -#include "slam6d/fbr/panorama.h" -#include "slam6d/fbr/feature.h" -#include "slam6d/fbr/feature_matcher.h" -#include "slam6d/fbr/registration.h" - -using namespace std; -using namespace fbr; - -struct information{ - string local_time; - string dir, outDir; - int iWidth, iHeight, nImages, minDistance, minError, minInlier, fScanNumber, sScanNumber, verbose; - double pParam, mParam; - IOType sFormat; - projection_method pMethod; - feature_detector_method fMethod; - feature_descriptor_method dMethod; - matcher_method mMethod; - registration_method rMethod; - - int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum; - double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime; -} info; - -/** - * usage : explains how to use the program CMD - */ -void usage(int argc, char** argv){ - printf("\n"); - printf("USAGE: %s dir firstScanNumber secondScanNumber \n", argv[0]); - printf("\n"); - printf("\n"); - printf("\tOptions:\n"); - printf("\t\t-F scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n"); - printf("\t\t-W iWidth\t\t panorama image width\n"); - printf("\t\t-H iHeight\t\t panorama image height\n"); - printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n"); - printf("\t\t-N nImage\t\t number of images used for some projections\n"); - printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n"); - printf("\t\t-f fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n"); - printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n"); - printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n"); - printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n"); - printf("\t\t-E minError \t\t threshold for min error in registration process\n"); - printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n"); - printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n"); - printf("\t\t-r registration \t registration method [ALL|ransac]\n"); - printf("\t\t-V verbose \t\t level of verboseness\n"); - printf("\t\t-O outDir \t\t output directory if not stated same as input\n"); - printf("\n"); - printf("\tExamples:\n"); - printf("\tUsing Bremen City dataset:\n"); - printf("\tLoading scan000.txt and scan001.txt:\n"); - printf("\t\t %s ~/dir/to/bremen_city 0 1\n", argv[0]); - printf("\tLoading scan005.txt and scan006.txt and output panorma images and feature images and match images in ~/dir/to/bremen_city/out dir:\n"); - printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 5 6 \n", argv[0]); - printf("\tLoading scan010.txt and scan011.txt using Mercator projection and SURF feature detector and SIFT descriptor:\n"); - printf("\t\t %s -p MERCATOR -f SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 10 11 \n", argv[0]); - printf("\n"); - exit(1); -} - -void parssArgs(int argc, char** argv, information& info){ - cout<<"parssArgs"<tm_year + 1900, timeinfo->tm_mon + 1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec); - info.local_time = time; - - //default values - info.iWidth = 3600; - info.iHeight = 1000; - info.nImages = 1; - info.minDistance = 50; - info.minError = 50; - info.minInlier = 5; - info.verbose = 0; - //depend on the projection method - info.pParam = 0; - info.mParam = 0; - //=============================== - info.sFormat = RIEGL_TXT; - info.pMethod = EQUIRECTANGULAR; - info.fMethod = SIFT_DET; - info.dMethod = SIFT_DES; - info.mMethod = RATIO; - info.rMethod = RANSAC; - info.outDir = ""; - - int c; - opterr = 0; - //reade the command line and get the options - while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:")) != -1) - switch (c) - { - case 'F': - info.sFormat = stringToScanFormat(optarg); - break; - case 'W': - info.iWidth = atoi(optarg); - break; - case 'H': - info.iHeight = atoi(optarg); - break; - case 'p': - info.pMethod = stringToProjectionMethod(optarg); - break; - case 'N': - info.nImages = atoi(optarg); - break; - case 'P': - info.pParam = atoi(optarg); - break; - case 'f': - info.fMethod = stringToFeatureDetectorMethod(optarg); - break; - case 'd': - info.dMethod = stringToFeatureDescriptorMethod(optarg); - break; - case 'm': - info.mMethod = stringToMatcherMethod(optarg); - break; - case 'D': - info.minDistance = atoi(optarg); - break; - case 'E': - info.minError = atoi(optarg); - break; - case 'I': - info.minInlier = atoi(optarg); - break; - case 'M': - info.mParam = atoi(optarg); - break; - case 'r': - info.rMethod = stringToRegistrationMethod(optarg); - break; - case 'V': - cout<<"verboos"< argc - 3) - { - cout<<"Too few input arguments. At least dir and two scan numbers are required."<(i,0) = bAlign[i]; - - string yml; - yml = info.outDir+"fbr-yml.yml"; - cv::FileStorage fs(yml.c_str(), cv::FileStorage::APPEND); - fs << "feature_bas_registration" << "{"; - - fs << "pair" << "{" << "scan" << to_string(info.fScanNumber, 3); - fs << "scan" << to_string(info.sScanNumber, 3) << "}"; - - fs << "time" << "{" << "local_time" << info.local_time << "}"; - - fs << "param" << "{"; - fs << "DIR" << info.dir; - fs << "sFormat" << scanFormatToString(info.sFormat); - fs << "pMethod" << projectionMethodToString(info.pMethod); - fs << "nImage" << info.nImages; - fs << "pParam" << info.pParam; - fs << "iWidth" << info.iWidth; - fs << "iHeight" << info.iHeight; - fs << "fMethod" << featureDetectorMethodToString(info.fMethod); - fs << "dMethod" << featureDescriptorMethodToString(info.dMethod); - fs << "mMethod" << matcherMethodToString(info.mMethod); - fs << "mParam" << info.mParam; - fs << "rMethod" << registrationMethodToString(info.rMethod); - fs << "minDistance" << info.minDistance; - fs << "minInlier" << info.minInlier; - fs << "minError" << info.minError; - fs << "}"; - - fs << "input" << "{"; - fs << "first_input" << "{"; - fs << "name" << "{" << "scan" << to_string(info.fScanNumber, 3) << "}"; - fs << "point" << "{" << "amount" << info.fSPoints << "time" << info.fSTime << "}"; - fs << "projection" << "{" << "time" << info.fPTime << "}"; - fs << "feature" << "{" << "amount" << info.fFNum << "fTime" << info.fFTime << "dTime" << info.fDTime << "}"; - fs << "}"; - fs << "second_input" << "{"; - fs << "name" << "{" << "scan" << to_string(info.sScanNumber, 3) << "}"; - fs << "point" << "{" << "amount" << info.sSPoints << "time" << info.sSTime << "}"; - fs << "projection" << "{" << "time" << info.sPTime << "}"; - fs << "feature" << "{" << "amount" << info.sFNum << "fTime" << info.sFTime << "dTime" << info.sDTime << "}"; - fs << "}"; - fs << "}"; - - fs << "matches" << "{"; - fs << "amount" << info.mNum << "filteration" << info.filteredMNum << "time" << info.mTime << "}"; - - fs << "reg" << "{"; - fs << "bestError" << bError << "bestErrorIdx" << bErrorIdx << "time" << info.rTime << "bAlign" << align << "}"; - - fs << "}"; -} - -int main(int argc, char** argv){ - string out; - cv::Mat outImage; - parssArgs(argc, argv, info); - cout<<"out of parssArgs"<= 1) informationDescription(info); - - scan_cv fScan (info.dir, info.fScanNumber, info.sFormat); - if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount(); - fScan.convertScanToMat(); - if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency(); - if(info.verbose >= 2) fScan.getDescription(); - panorama fPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam); - if(info.verbose >= 4) info.fPTime = (double)cv::getTickCount(); - fPanorama.createPanorama(fScan.getMatScan()); - if(info.verbose >= 4) info.fPTime = ((double)cv::getTickCount() - info.fPTime)/cv::getTickFrequency(); - if(info.verbose >= 2) fPanorama.getDescription(); - //write panorama to image - if(info.verbose >= 1){ - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg"; - imwrite(out, fPanorama.getReflectanceImage()); - } - feature fFeature; - if(info.verbose >= 4) info.fFTime = (double)cv::getTickCount(); - fFeature.featureDetection(fPanorama.getReflectanceImage(), info.fMethod); - if(info.verbose >= 4) info.fFTime = ((double)cv::getTickCount() - info.fFTime)/cv::getTickFrequency(); - //write panorama with keypoints to image - if(info.verbose >= 1){ - cv::drawKeypoints(fPanorama.getReflectanceImage(), fFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg"; - imwrite(out, outImage); - outImage.release(); - } - if(info.verbose >= 4) info.fDTime = (double)cv::getTickCount(); - fFeature.featureDescription(fPanorama.getReflectanceImage(), info.dMethod); - if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency(); - if(info.verbose >= 2) fFeature.getDescription(); - - scan_cv sScan (info.dir, info.sScanNumber, info.sFormat); - if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount(); - sScan.convertScanToMat(); - if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency(); - if(info.verbose >= 2) sScan.getDescription(); - panorama sPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam); - if(info.verbose >= 4) info.sPTime = (double)cv::getTickCount(); - sPanorama.createPanorama(sScan.getMatScan()); - if(info.verbose >= 4) info.sPTime = ((double)cv::getTickCount() - info.sPTime)/cv::getTickFrequency(); - if(info.verbose >= 2) sPanorama.getDescription(); - //write panorama to image - if(info.verbose >= 1){ - out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg"; - imwrite(out, sPanorama.getReflectanceImage()); - } - feature sFeature; - if(info.verbose >= 4) info.sFTime = (double)cv::getTickCount(); - sFeature.featureDetection(sPanorama.getReflectanceImage(), info.fMethod); - if(info.verbose >= 4) info.sFTime = ((double)cv::getTickCount() - info.sFTime)/cv::getTickFrequency(); - //write panorama with keypoints to image - if(info.verbose >= 1){ - cv::drawKeypoints(sPanorama.getReflectanceImage(), sFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); - out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg"; - imwrite(out, outImage); - outImage.release(); - } - if(info.verbose >= 4) info.sDTime = (double)cv::getTickCount(); - sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod); - if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency(); - if(info.verbose >= 2) sFeature.getDescription(); - - feature_matcher matcher (info.mMethod, info.mParam); - if(info.verbose >= 4) info.mTime = (double)cv::getTickCount(); - matcher.match(fFeature, sFeature); - if(info.verbose >= 4) info.mTime = ((double)cv::getTickCount() - info.mTime)/cv::getTickFrequency(); - if(info.verbose >= 2) matcher.getDescription(); - //write matcheed feature to image - if(info.verbose >= 1){ - cv::drawMatches(fPanorama.getReflectanceImage(), fFeature.getFeatures(), sPanorama.getReflectanceImage(), sFeature.getFeatures(), matcher.getMatches(), outImage, cv::Scalar::all(-1), cv::Scalar::all(-1), vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+".jpg"; - imwrite(out, outImage); - outImage.release(); - } - - registration reg (info.minDistance, info.minError, info.minInlier, info.rMethod); - if(info.verbose >= 4) info.rTime = (double)cv::getTickCount(); - reg.findRegistration(fPanorama.getMap(), fFeature.getFeatures(), sPanorama.getMap(), sFeature.getFeatures(), matcher.getMatches()); - if(info.verbose >= 4) info.rTime = ((double)cv::getTickCount() - info.rTime)/cv::getTickFrequency(); - if(info.verbose >= 2) reg.getDescription(); - - //write .dat and .frames files - if(info.verbose >= 0){ - double *bAlign = reg.getBestAlign(); - - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".dat"; - ofstream dat(out.c_str()); - dat << bAlign[0] << " " << bAlign[4] << " " << bAlign[8] << " " << bAlign[12] <= 3){ - info.fSPoints = fScan.getNumberOfPoints(); - info.sSPoints = sScan.getNumberOfPoints(); - info.fFNum = fFeature.getNumberOfFeatures(); - info.sFNum = sFeature.getNumberOfFeatures(); - info.mNum = matcher.getNumberOfMatches(); - info.filteredMNum = matcher.getNumberOfFilteredMatches(); - - info_yml(info, reg.getBestError(), reg.getBestErrorIndex(), reg.getBestAlign()); - } - - return 0; -} diff --git a/.svn/pristine/d5/d593695c94d1fb12549aa3ea48556304f91dc6a4.svn-base b/.svn/pristine/d5/d593695c94d1fb12549aa3ea48556304f91dc6a4.svn-base deleted file mode 100644 index 2077dc1..0000000 --- a/.svn/pristine/d5/d593695c94d1fb12549aa3ea48556304f91dc6a4.svn-base +++ /dev/null @@ -1,401 +0,0 @@ -/* - * feature_based_registration implementation - * - * Copyright (C) HamidReza Houshiar - * - * Released under the GPL version 3. - * - */ - -#include -#include -#include "slam6d/fbr/fbr_global.h" -#include "slam6d/fbr/scan_cv.h" -#include "slam6d/fbr/panorama.h" -#include "slam6d/fbr/feature.h" -#include "slam6d/fbr/feature_matcher.h" -#include "slam6d/fbr/registration.h" - -using namespace std; -using namespace fbr; - -struct information{ - string local_time; - string dir, outDir; - int iWidth, iHeight, nImages, minDistance, minError, minInlier, fScanNumber, sScanNumber, verbose; - double pParam, mParam; - IOType sFormat; - projection_method pMethod; - feature_detector_method fMethod; - feature_descriptor_method dMethod; - matcher_method mMethod; - registration_method rMethod; - - int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum; - double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime; -} info; - -/** - * usage : explains how to use the program CMD - */ -void usage(int argc, char** argv){ - printf("\n"); - printf("USAGE: %s dir firstScanNumber secondScanNumber \n", argv[0]); - printf("\n"); - printf("\n"); - printf("\tOptions:\n"); - printf("\t\t-F scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n"); - printf("\t\t-W iWidth\t\t panorama image width\n"); - printf("\t\t-H iHeight\t\t panorama image height\n"); - printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n"); - printf("\t\t-N nImage\t\t number of images used for some projections\n"); - printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n"); - printf("\t\t-f fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n"); - printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n"); - printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n"); - printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n"); - printf("\t\t-E minError \t\t threshold for min error in registration process\n"); - printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n"); - printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n"); - printf("\t\t-r registration \t registration method [ALL|ransac]\n"); - printf("\t\t-V verbose \t\t level verboseness\n"); - printf("\t\t-O outDir \t\t level output directory if not stated same as input\n"); - printf("\n"); - exit(1); -} - -void parssArgs(int argc, char** argv, information& info){ - time_t rawtime; - struct tm *timeinfo; - time(&rawtime); - char time[50]; - timeinfo = localtime (&rawtime); - sprintf(time, "%d-%d-%d-%d:%d:%d", timeinfo->tm_year + 1900, timeinfo->tm_mon + 1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec); - info.local_time = time; - - //default values - info.iWidth = 3600; - info.iHeight = 1000; - info.nImages = 1; - info.minDistance = 50; - info.minError = 50; - info.minInlier = 5; - info.verbose = 0; - //depend on the projection method - info.pParam = 0; - info.mParam = 0; - //=============================== - info.sFormat = RIEGL_TXT; - info.pMethod = EQUIRECTANGULAR; - info.fMethod = SIFT_DET; - info.dMethod = SIFT_DES; - info.mMethod = RATIO; - info.rMethod = RANSAC; - info.outDir = ""; - - int c; - opterr = 0; - //reade the command line and get the options - while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:")) != -1) - switch (c) - { - case 'F': - info.sFormat = stringToScanFormat(optarg); - break; - case 'W': - info.iWidth = atoi(optarg); - break; - case 'H': - info.iHeight = atoi(optarg); - break; - case 'p': - info.pMethod = stringToProjectionMethod(optarg); - break; - case 'N': - info.nImages = atoi(optarg); - break; - case 'P': - info.pParam = atoi(optarg); - break; - case 'f': - info.fMethod = stringToFeatureDetectorMethod(optarg); - break; - case 'd': - info.dMethod = stringToFeatureDescriptorMethod(optarg); - break; - case 'm': - info.mMethod = stringToMatcherMethod(optarg); - break; - case 'D': - info.minDistance = atoi(optarg); - break; - case 'E': - info.minError = atoi(optarg); - break; - case 'I': - info.minInlier = atoi(optarg); - break; - case 'M': - info.mParam = atoi(optarg); - break; - case 'r': - info.rMethod = stringToRegistrationMethod(optarg); - break; - case 'V': - info.verbose = atoi(optarg); - break; - case 'O': - info.outDir = optarg; - break; - case '?': - cout<<"Unknown option character "< argc - 3) - { - cout<<"Too few input arguments. At least dir and two scan numbers are required."<(i,0) = bAlign[i]; - - string yml; - yml = info.outDir+"fbr-yml.yml"; - cv::FileStorage fs(yml.c_str(), cv::FileStorage::APPEND); - fs << "feature_bas_registration" << "{"; - - fs << "pair" << "{" << "scan" << to_string(info.fScanNumber, 3); - fs << "scan" << to_string(info.sScanNumber, 3) << "}"; - - fs << "time" << "{" << "local_time" << info.local_time << "}"; - - fs << "param" << "{"; - fs << "DIR" << info.dir; - fs << "sFormat" << scanFormatToString(info.sFormat); - fs << "pMethod" << projectionMethodToString(info.pMethod); - fs << "nImage" << info.nImages; - fs << "pParam" << info.pParam; - fs << "iWidth" << info.iWidth; - fs << "iHeight" << info.iHeight; - fs << "fMethod" << featureDetectorMethodToString(info.fMethod); - fs << "dMethod" << featureDescriptorMethodToString(info.dMethod); - fs << "mMethod" << matcherMethodToString(info.mMethod); - fs << "mParam" << info.mParam; - fs << "rMethod" << registrationMethodToString(info.rMethod); - fs << "minDistance" << info.minDistance; - fs << "minInlier" << info.minInlier; - fs << "minError" << info.minError; - fs << "}"; - - fs << "input" << "{"; - fs << "first_input" << "{"; - fs << "name" << "{" << "scan" << to_string(info.fScanNumber, 3) << "}"; - fs << "point" << "{" << "amount" << info.fSPoints << "time" << info.fSTime << "}"; - fs << "projection" << "{" << "time" << info.fPTime << "}"; - fs << "feature" << "{" << "amount" << info.fFNum << "fTime" << info.fFTime << "dTime" << info.fDTime << "}"; - fs << "}"; - fs << "second_input" << "{"; - fs << "name" << "{" << "scan" << to_string(info.sScanNumber, 3) << "}"; - fs << "point" << "{" << "amount" << info.sSPoints << "time" << info.sSTime << "}"; - fs << "projection" << "{" << "time" << info.sPTime << "}"; - fs << "feature" << "{" << "amount" << info.sFNum << "fTime" << info.sFTime << "dTime" << info.sDTime << "}"; - fs << "}"; - fs << "}"; - - fs << "matches" << "{"; - fs << "amount" << info.mNum << "filteration" << info.filteredMNum << "time" << info.mTime << "}"; - - fs << "reg" << "{"; - fs << "bestError" << bError << "bestErrorIdx" << bErrorIdx << "time" << info.rTime << "bAlign" << align << "}"; - - fs << "}"; -} - -int main(int argc, char** argv){ - string out; - cv::Mat outImage; - parssArgs(argc, argv, info); - if(info.verbose >= 1) informationDescription(info); - - scan_cv fScan (info.dir, info.fScanNumber, info.sFormat); - if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount(); - fScan.convertScanToMat(); - if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency(); - if(info.verbose >= 2) fScan.getDescription(); - panorama fPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam); - if(info.verbose >= 4) info.fPTime = (double)cv::getTickCount(); - fPanorama.createPanorama(fScan.getMatScan()); - if(info.verbose >= 4) info.fPTime = ((double)cv::getTickCount() - info.fPTime)/cv::getTickFrequency(); - if(info.verbose >= 2) fPanorama.getDescription(); - //write panorama to image - if(info.verbose >= 1){ - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg"; - imwrite(out, fPanorama.getReflectanceImage()); - } - feature fFeature; - if(info.verbose >= 4) info.fFTime = (double)cv::getTickCount(); - fFeature.featureDetection(fPanorama.getReflectanceImage(), info.fMethod); - if(info.verbose >= 4) info.fFTime = ((double)cv::getTickCount() - info.fFTime)/cv::getTickFrequency(); - //write panorama with keypoints to image - if(info.verbose >= 1){ - cv::drawKeypoints(fPanorama.getReflectanceImage(), fFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg"; - imwrite(out, outImage); - outImage.release(); - } - if(info.verbose >= 4) info.fDTime = (double)cv::getTickCount(); - fFeature.featureDescription(fPanorama.getReflectanceImage(), info.dMethod); - if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency(); - if(info.verbose >= 2) fFeature.getDescription(); - - scan_cv sScan (info.dir, info.sScanNumber, info.sFormat); - if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount(); - sScan.convertScanToMat(); - if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency(); - if(info.verbose >= 2) sScan.getDescription(); - panorama sPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam); - if(info.verbose >= 4) info.sPTime = (double)cv::getTickCount(); - sPanorama.createPanorama(sScan.getMatScan()); - if(info.verbose >= 4) info.sPTime = ((double)cv::getTickCount() - info.sPTime)/cv::getTickFrequency(); - if(info.verbose >= 2) sPanorama.getDescription(); - //write panorama to image - if(info.verbose >= 1){ - out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg"; - imwrite(out, sPanorama.getReflectanceImage()); - } - feature sFeature; - if(info.verbose >= 4) info.sFTime = (double)cv::getTickCount(); - sFeature.featureDetection(sPanorama.getReflectanceImage(), info.fMethod); - if(info.verbose >= 4) info.sFTime = ((double)cv::getTickCount() - info.sFTime)/cv::getTickFrequency(); - //write panorama with keypoints to image - if(info.verbose >= 1){ - cv::drawKeypoints(sPanorama.getReflectanceImage(), sFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); - out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg"; - imwrite(out, outImage); - outImage.release(); - } - if(info.verbose >= 4) info.sDTime = (double)cv::getTickCount(); - sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod); - if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency(); - if(info.verbose >= 2) sFeature.getDescription(); - - feature_matcher matcher (info.mMethod, info.mParam); - if(info.verbose >= 4) info.mTime = (double)cv::getTickCount(); - matcher.match(fFeature, sFeature); - if(info.verbose >= 4) info.mTime = ((double)cv::getTickCount() - info.mTime)/cv::getTickFrequency(); - if(info.verbose >= 2) matcher.getDescription(); - //write matcheed feature to image - if(info.verbose >= 1){ - cv::drawMatches(fPanorama.getReflectanceImage(), fFeature.getFeatures(), sPanorama.getReflectanceImage(), sFeature.getFeatures(), matcher.getMatches(), outImage, cv::Scalar::all(-1), cv::Scalar::all(-1), vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+".jpg"; - imwrite(out, outImage); - outImage.release(); - } - - registration reg (info.minDistance, info.minError, info.minInlier, info.rMethod); - if(info.verbose >= 4) info.rTime = (double)cv::getTickCount(); - reg.findRegistration(fPanorama.getMap(), fFeature.getFeatures(), sPanorama.getMap(), sFeature.getFeatures(), matcher.getMatches()); - if(info.verbose >= 4) info.rTime = ((double)cv::getTickCount() - info.rTime)/cv::getTickFrequency(); - if(info.verbose >= 2) reg.getDescription(); - - //write .dat and .frames files - if(info.verbose >= 0){ - double *bAlign = reg.getBestAlign(); - - out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".dat"; - ofstream dat(out.c_str()); - dat << bAlign[0] << " " << bAlign[4] << " " << bAlign[8] << " " << bAlign[12] <= 3){ - info.fSPoints = fScan.getNumberOfPoints(); - info.sSPoints = sScan.getNumberOfPoints(); - info.fFNum = fFeature.getNumberOfFeatures(); - info.sFNum = sFeature.getNumberOfFeatures(); - info.mNum = matcher.getNumberOfMatches(); - info.filteredMNum = matcher.getNumberOfFilteredMatches(); - - info_yml(info, reg.getBestError(), reg.getBestErrorIndex(), reg.getBestAlign()); - } - - return 0; -} diff --git a/.svn/pristine/d6/d6a5a93ffea0d6a4ad66c8e0a5793f94c07f21ac.svn-base b/.svn/pristine/d6/d6a5a93ffea0d6a4ad66c8e0a5793f94c07f21ac.svn-base deleted file mode 100644 index 1e5f744..0000000 --- a/.svn/pristine/d6/d6a5a93ffea0d6a4ad66c8e0a5793f94c07f21ac.svn-base +++ /dev/null @@ -1,196 +0,0 @@ -/** - * @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(); - 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_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/d9/d9f1dfdf01d6a88d739c6ab9df42a592592c6788.svn-base b/.svn/pristine/d9/d9f1dfdf01d6a88d739c6ab9df42a592592c6788.svn-base deleted file mode 100644 index a4c0107..0000000 --- a/.svn/pristine/d9/d9f1dfdf01d6a88d739c6ab9df42a592592c6788.svn-base +++ /dev/null @@ -1,113 +0,0 @@ -/** - * @file - * @brief IO filtering class for ScanIO to discard unwanted points. - * - * @author Thomas Escher - */ - -#ifndef POINT_FILTER_H -#define POINT_FILTER_H - -#include -#include - -class Checker; - - -/** - * Flexible filtering class for parsing a set of points. - * - * This class is configurable with parameters for range and height and can be transferred via the use of a parameter string. - * Use on a point set via repeated use of the check function, which creates the internal Checker structures once for each change to the parameters. The amount of tests is held as minimal as possible. - */ -class PointFilter -{ -public: - // Default empty constructor - PointFilter(); - //! Deserialization constructor, forms parameters back from a string given by getParams - PointFilter(const std::string& params); - ~PointFilter(); - - PointFilter& setRange(double maxDist, double minDist); - PointFilter& setHeight(double top, double bottom); - - //! Serialization function to convert it into a string, usable in the constructor - std::string getParams(); - - //! Check a point, returning success if all contained Checker functions accept that point (implemented in .icc) - inline bool check(double* point); -private: - //! Storage for parameter keys and values - std::map m_params; - - //! If either not created yet or parameters get changed, this flag will cause check to create a new Checker chain - bool m_changed; - - //! created in the first check call with the changed flag set - Checker* m_checker; - - //! Allocation of the checkers - void createCheckers(); - - // factory magic - template friend struct CheckerFactory; - static std::map* factory; -}; - -class Checker { -public: - Checker(); - ~Checker(); - - //! Testing function - virtual bool test(double* point) = 0; - - //! Next test in chain - Checker* m_next; -}; - -//! Factory integrating the create-function and adding it to the lookup map in an easy template -template -struct CheckerFactory { - //! Instanciate in the source code with the to be created class as template argument and associated key string as constructor argument - CheckerFactory(const std::string& key) { (*PointFilter::factory)[key] = CheckerFactory::create; } - //! Automated create function, safely returning a nullpointer if the constructor throws for unwanted values - static Checker* create(const std::string& value) { try { return new T(value); } catch(...) { return 0; } } -}; - -class CheckerRangeMax : public Checker { -public: - CheckerRangeMax(const std::string& value); - virtual bool test(double* point); -private: - double m_max; -}; - -class CheckerRangeMin : public Checker { -public: - CheckerRangeMin(const std::string& value); - virtual bool test(double* point); -private: - double m_min; -}; - -class CheckerHeightTop : public Checker { -public: - CheckerHeightTop(const std::string& value); - virtual bool test(double* point); -private: - double m_top; -}; - -class CheckerHeightBottom : public Checker { -public: - CheckerHeightBottom(const std::string& value); - virtual bool test(double* point); -private: - double m_bottom; -}; - -#include "pointfilter.icc" - -#endif //POINT_FILTER_H diff --git a/.svn/pristine/da/da1dfe175e8ce7ce4167fcbd0517ab1469769b59.svn-base b/.svn/pristine/da/da1dfe175e8ce7ce4167fcbd0517ab1469769b59.svn-base deleted file mode 100644 index 52b158f..0000000 --- a/.svn/pristine/da/da1dfe175e8ce7ce4167fcbd0517ab1469769b59.svn-base +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @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* 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/da/da95a45e11ae5b4fbc6862200656e37691eca812.svn-base b/.svn/pristine/da/da95a45e11ae5b4fbc6862200656e37691eca812.svn-base deleted file mode 100644 index ee0dc5c..0000000 --- a/.svn/pristine/da/da95a45e11ae5b4fbc6862200656e37691eca812.svn-base +++ /dev/null @@ -1,825 +0,0 @@ -/* - * 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.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.hasAmplitude()) { - cm->setCurrentType(PointType::USE_AMPLITUDE); - break; - } - case 3: - if (pointtype.hasDeviation()) { - cm->setCurrentType(PointType::USE_DEVIATION); - break; - } - case 4: - 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/e4/e466bb88a5d6f8a0b9f133b26f6a3e0635dca668.svn-base b/.svn/pristine/e4/e466bb88a5d6f8a0b9f133b26f6a3e0635dca668.svn-base deleted file mode 100644 index 5316afa..0000000 --- a/.svn/pristine/e4/e466bb88a5d6f8a0b9f133b26f6a3e0635dca668.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/e4/e4d8522facb6f6415298f42185b70da086fb4a32.svn-base b/.svn/pristine/e4/e4d8522facb6f6415298f42185b70da086fb4a32.svn-base deleted file mode 100644 index 62c70a6..0000000 --- a/.svn/pristine/e4/e4d8522facb6f6415298f42185b70da086fb4a32.svn-base +++ /dev/null @@ -1,451 +0,0 @@ -/* - * 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 *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.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"); - - 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/e7/e709a93746f1fc8832f7457d39117f3c2b0f9f28.svn-base b/.svn/pristine/e7/e709a93746f1fc8832f7457d39117f3c2b0f9f28.svn-base deleted file mode 100644 index c7d13de..0000000 --- a/.svn/pristine/e7/e709a93746f1fc8832f7457d39117f3c2b0f9f28.svn-base +++ /dev/null @@ -1,99 +0,0 @@ -/* - * scan_cv implementation - * - * Copyright (C) HamidReza Houshiar - * - * Released under the GPL version 3. - * - */ - -#include "slam6d/fbr/scan_cv.h" - -using namespace std; - -namespace fbr{ - - scan_cv::scan_cv(string dir, unsigned int number, IOType format){ - sDir = dir; - sNumber = number; - sFormat = format; - zMax = numeric_limits::min(); - zMin = numeric_limits::max(); - nPoints = 0; - } - - void scan_cv::convertScanToMat(){ - bool scanserver = false; - Scan::openDirectory(scanserver, sDir, sFormat, sNumber, sNumber); - cout<<"loading "<get("xyz"); - DataReflectance xyz_reflectance = source->get("reflectance"); - nPoints = xyz.size(); - scan.create(nPoints,1,CV_32FC(4)); - scan = cv::Scalar::all(0); - cv::MatIterator_ it; - it = scan.begin(); - for(unsigned int i = 0; i < nPoints; i++){ - float x, y, z, reflectance; - x = xyz[i][0]; - y = xyz[i][1]; - z = xyz[i][2]; - reflectance = xyz_reflectance[i]; - - //normalize the reflectance - reflectance += 32; - reflectance /= 64; - reflectance -= 0.2; - reflectance /= 0.3; - if (reflectance < 0) reflectance = 0; - if (reflectance > 1) reflectance = 1; - - (*it)[0] = x; - (*it)[1] = y; - (*it)[2] = z; - (*it)[3] = reflectance; - - //finding min and max of z - if (z > zMax) zMax = z; - if (z < zMin) zMin = z; - - ++it; - } - Scan::closeDirectory(); - } - - string scan_cv::getScanDir(){ - return sDir; - } - - unsigned int scan_cv::getScanNumber(){ - return sNumber; - } - - unsigned int scan_cv::getNumberOfPoints(){ - return nPoints; - } - - double scan_cv::getZMin(){ - return zMin; - } - - double scan_cv::getZMax(){ - return zMax; - } - - IOType scan_cv::getScanFormat(){ - return sFormat; - } - - cv::Mat scan_cv::getMatScan() - { - return scan; - } - - void scan_cv::getDescription(){ - cout<<"load "< -#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* 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< readDirectory(const char* dir_path, unsigned int start, unsigned int end); - virtual void readPose(const char* dir_path, const char* identifier, double* pose); - virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector* xyz, std::vector* rgb, std::vector* reflectance, std::vector* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/ee/eeea8978e0533145f4df4470755dbed661ddc62d.svn-base b/.svn/pristine/ee/eeea8978e0533145f4df4470755dbed661ddc62d.svn-base deleted file mode 100644 index 5871add..0000000 --- a/.svn/pristine/ee/eeea8978e0533145f4df4470755dbed661ddc62d.svn-base +++ /dev/null @@ -1,192 +0,0 @@ -/* - * 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* 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/f1/f147888ae4e11507aadc8b421f9f471411fd0488.svn-base b/.svn/pristine/f1/f147888ae4e11507aadc8b421f9f471411fd0488.svn-base deleted file mode 100644 index a02c4a4..0000000 --- a/.svn/pristine/f1/f147888ae4e11507aadc8b421f9f471411fd0488.svn-base +++ /dev/null @@ -1,1291 +0,0 @@ -/** - * @file - * @brief Globally used functions - * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. - * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. - */ - -#ifndef __GLOBALS_ICC__ -#define __GLOBALS_ICC__ - -#ifdef _MSC_VER -#include -#define _USE_MATH_DEFINES -#include - -inline int gettimeofday (struct timeval* tp, void* tzp) -{ - unsigned long t; - t = timeGetTime(); - tp->tv_sec = t / 1000; - tp->tv_usec = t % 1000; - return 0; /* 0 indicates success. */ -} -#else -#include -#endif - -#include -using std::min; -using std::max; -#include -#include -using std::stringstream; -#include -using std::ostream; -using std::istream; -#include -using std::cout; -using std::endl; -#include -#include -using std::runtime_error; - -/** - * Set bits count - * - * @param unsigned char x - * - * @return char - * - */ -inline unsigned char _my_popcount_3(unsigned char x) { - x -= (x >> 1) & 0x55; //put count of each 2 bits into those 2 bits - x = (x & 0x33) + ((x >> 2) & 0x33); //put count of each 4 bits into those 4 bits - x = (x + (x >> 4)) & 0x0f; //put count of each 8 bits into those 8 bits - return x; -} - -/** - * Converts a class T to a string of width with padding 0 - * - * @param t output - * @param width length - * - * @return string of t - * - */ -template -inline std::string to_string(const T& t, int width) -{ - stringstream ss; - ss << std::setfill('0') << std::setw(width) << t; - return ss.str(); -} - -/** - * Converts a class T to a string of width with padding 0 - * - * @param t output - * @return string of t - * - */ -template -inline std::string to_string(const T& t) -{ - stringstream ss; - ss << t; - return ss.str(); -} - - -/** - * Overridden "<<" operator for sending a (4x4)-matrix to a stream - * - * @param os stream - * @param matrix 4x4 matrix sent to stream - * @return stream - */ -inline ostream& operator<<(ostream& os, const double matrix[16]) -{ - for (int i = 0; i < 16; os << matrix[i++] << " "); - return os; -} - -/** - * Overridden ">>" operator for reading a (4x4)-matrix from a stream.
- * Throws a runtime error if not enough data in the stream. - * - * @param is stream - * @param matrix 4x4 matrix sent to stream - * @return stream -*/ -inline istream& operator>>(istream& is, double matrix[16]) -{ - for (int i = 0; i < 16; i++) { - if (!is.good()) throw runtime_error("Not enough elements to read for >>(istream&, double[16])"); - is >> matrix[i]; - } - return is; -} - - -/** - * Converts an angle (given in deg) to rad - * - * @param deg integer indicating, whether the figure to be drawn to show - * the clusters should be circles (0) or rectangles(1) - * - * @return the clustered image, with the clusters marked by colored figures - * - */ -template -inline T rad(const T deg) -{ - return ( (2 * M_PI * deg) / 360 ); -} - -/** - * Converts an angle (given in rad) to deg - * - * @param rad angle in rad - * @return angle in deg - */ -template -inline T deg(const T rad) -{ - return ( (rad * 360) / (2 * M_PI) ); -} - - -/** - * Calculates x^2 - * - * @param x input scalar value - * @return squared value - */ -template -static inline T sqr(const T &x) -{ - return x*x; -} - - -/** - * Computes the squared length of a 3-vector - * - * @param x input 3-vector - * @return length^2 of vector - */ -template -inline T Len2(const T *x) -{ - return sqr(x[0]) + sqr(x[1]) + sqr(x[2]); -} - - -/** - * Computes the length of a 3-vector - * - * @param x input 3-vector - * @return length of vector - */ -template -inline T Len(const T *x) -{ - return sqrt(Len2(x)); -} - - -/** - * Computes the squared Eucledian distance between two points - * in 3-space - * - * @param x1 first input vector - * @param x2 decond input vecotr - * @return Eucledian distance^2 between the two locations - */ -template -inline T Dist2(const T *x1, const F *x2) -{ - T dx = x2[0] - x1[0]; - T dy = x2[1] - x1[1]; - T dz = x2[2] - x1[2]; - - return sqr(dx) + sqr(dy) + sqr(dz); -} - -/* - * Normalization of the input 3-vector - * - * @param x input/output 3-vector - */ -template -static inline void Normalize3(T *x) -{ - T norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]); - x[0] /= norm; - x[1] /= norm; - x[2] /= norm; -} - -/* - * Normalization of the input 4-vector - * - * @param x input/output 4-vector - */ -template -static inline void Normalize4(T *x) -{ - T norm = sqrt((x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3])); - - x[0] /= norm; - x[1] /= norm; - x[2] /= norm; - x[3] /= norm; -} - -/** - * Sets a 4x4 matrix to identity - * - * @param M 4x4 matrix - */ -template -inline void M4identity( T *M ) -{ - M[0] = M[5] = M[10] = M[15] = 1.0; - M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0; -} - -/** - * Multiplies a 4x4 matrices in OpenGL - * (column-major) order - * - * @param M1 first input matrix - * @param M2 second input matrix - * @param Mout output matrix - * - */ -template -inline void MMult(const T *M1, - const T *M2, - T *Mout) -{ - Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; - Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; - Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; - Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; - Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; - Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; - Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; - Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; - Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; - Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; - Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; - Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; - Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; - Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; - Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; - Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; -} - -template -inline void MMult(const T *M1, - const T *M2, - float *Mout) -{ - Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; - Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; - Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; - Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; - Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; - Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; - Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; - Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; - Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; - Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; - Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; - Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; - Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; - Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; - Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; - Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; -} - -/** - * Transforms a vactor with a matrix (P = M * V) - * @param M the transformation matrix - * @param v the initial vector - * @param p the transformt vector - */ -template -inline void VTrans(const T *M, const T *V, T *P) -{ - P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12]; - P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13]; - P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14]; -} - -/** - * Converts an Euler angle to a 3x3 matrix - * - * @param rPosTheta vector of Euler angles - * @param alignxf 3x3 matrix corresponding to the Euler angles - */ -inline void EulerToMatrix3(const double *rPosTheta, double *alignxf) -{ - double sx = sin(rPosTheta[0]); - double cx = cos(rPosTheta[0]); - double sy = sin(rPosTheta[1]); - double cy = cos(rPosTheta[1]); - double sz = sin(rPosTheta[2]); - double cz = cos(rPosTheta[2]); - - alignxf[0] = cy*cz; - alignxf[1] = sx*sy*cz + cx*sz; - alignxf[2] = -cx*sy*cz + sx*sz; - alignxf[3] = -cy*sz; - alignxf[4] = -sx*sy*sz + cx*cz; - alignxf[5] = cx*sy*sz + sx*cz; - alignxf[6] = sy; - alignxf[7] = -sx*cy; - alignxf[8] = cx*cy; -} - -/** - * Calculates the determinant of a 3x3 matrix - * - * @param M input 3x3 matrix - * @return determinant of input matrix - */ -template -inline double M3det( const T *M ) -{ - double det; - det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] ) - - M[1] * ( M[3]*M[8] - M[6]*M[5] ) - + M[2] * ( M[3]*M[7] - M[6]*M[4] )); - return ( det ); -} - -/** - * Inverts a 3x3 matrix - * - * @param Min input 3x3 matrix - * @param Mout output 3x3 matrix - */ -template -inline void M3inv( const T *Min, T *Mout ) -{ - double det = M3det( Min ); - - if ( fabs( det ) < 0.0005 ) { - M3identity( Mout ); - return; - } - - Mout[0] = (double)( Min[4]*Min[8] - Min[5]*Min[7] ) / det; - Mout[1] = (double)(-( Min[1]*Min[8] - Min[7]*Min[2] )) / det; - Mout[2] = (double)( Min[1]*Min[5] - Min[4]*Min[2] ) / det; - - Mout[3] = (double)(-( Min[3]*Min[8] - Min[5]*Min[6] )) / det; - Mout[4] = (double)( Min[0]*Min[8] - Min[6]*Min[2] ) / det; - Mout[5] = (double)(-( Min[0]*Min[5] - Min[3]*Min[2] )) / det; - - Mout[6] = (double) ( Min[3]*Min[7] - Min[6]*Min[4] ) / det; - Mout[7] = (double)(-( Min[0]*Min[7] - Min[6]*Min[1] )) / det; - Mout[8] = (double) ( Min[0]*Min[4] - Min[1]*Min[3] ) / det; -} - - -/** - * Converts a pose into a RT matrix - * @param *rPos Pointer to the position (double[3]) - * @param *rPosTheta Pointer to the angles (double[3]) - * @param *alignxf The calculated matrix - */ -inline void EulerToMatrix4(const double *rPos, const double *rPosTheta, double *alignxf) -{ - double sx = sin(rPosTheta[0]); - double cx = cos(rPosTheta[0]); - double sy = sin(rPosTheta[1]); - double cy = cos(rPosTheta[1]); - double sz = sin(rPosTheta[2]); - double cz = cos(rPosTheta[2]); - - alignxf[0] = cy*cz; - alignxf[1] = sx*sy*cz + cx*sz; - alignxf[2] = -cx*sy*cz + sx*sz; - alignxf[3] = 0.0; - alignxf[4] = -cy*sz; - alignxf[5] = -sx*sy*sz + cx*cz; - alignxf[6] = cx*sy*sz + sx*cz; - alignxf[7] = 0.0; - alignxf[8] = sy; - alignxf[9] = -sx*cy; - alignxf[10] = cx*cy; - - alignxf[11] = 0.0; - - alignxf[12] = rPos[0]; - alignxf[13] = rPos[1]; - alignxf[14] = rPos[2]; - alignxf[15] = 1; -} - -/** - * Converts a 4x4 matrix to Euler angles. - * - * @param alignxf input 4x4 matrix - * @param rPosTheta output 3-vector of Euler angles - * @param rPos output vector of trnaslation (position) if set - * - */ -static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, double *rPos = 0) -{ - - double _trX, _trY; - - if(alignxf[0] > 0.0) { - rPosTheta[1] = asin(alignxf[8]); - } else { - rPosTheta[1] = M_PI - asin(alignxf[8]); - } - // rPosTheta[1] = asin( alignxf[8]); // Calculate Y-axis angle - - double C = cos( rPosTheta[1] ); - if ( fabs( C ) > 0.005 ) { // Gimball lock? - _trX = alignxf[10] / C; // No, so get X-axis angle - _trY = -alignxf[9] / C; - rPosTheta[0] = atan2( _trY, _trX ); - _trX = alignxf[0] / C; // Get Z-axis angle - _trY = -alignxf[4] / C; - rPosTheta[2] = atan2( _trY, _trX ); - } else { // Gimball lock has occurred - rPosTheta[0] = 0.0; // Set X-axis angle to zero - _trX = alignxf[5]; //1 // And calculate Z-axis angle - _trY = alignxf[1]; //2 - rPosTheta[2] = atan2( _trY, _trX ); - } - - rPosTheta[0] = rPosTheta[0]; - rPosTheta[1] = rPosTheta[1]; - rPosTheta[2] = rPosTheta[2]; - - if (rPos != 0) { - rPos[0] = alignxf[12]; - rPos[1] = alignxf[13]; - rPos[2] = alignxf[14]; - } -} - - -/** - * Sets a 3x3 matrix to the identity matrix - * - * @param M input 3x3 matrix - */ -template -static inline void M3identity( T *M ) -{ - M[0] = M[4] = M[8] = 1.0; - M[1] = M[2] = M[3] = M[5] = M[6] = M[7] = 0.0; -} - -/** - * Gets the current time (in ms) - * - * @return current time (in ms) - */ -static inline unsigned long GetCurrentTimeInMilliSec() -{ - static unsigned long milliseconds; -#ifdef _MSC_VER - SYSTEMTIME stime; - GetSystemTime(&stime); - milliseconds = ((stime.wHour * 60 + stime.wMinute) * 60 + stime.wSecond) * 1000 + stime.wMilliseconds; -#else - static struct timeval tv; - gettimeofday(&tv, NULL); - milliseconds = tv.tv_sec * 1000 + tv.tv_usec / 1000; -#endif - return milliseconds; -} - -/** - * generates random numbers in [0..rnd] - * - * @param rnd maximum number - * @return random number between 0 and rnd - */ -inline int rand(int rnd) -{ - return (int) ((double)rnd * (double)std::rand() / (RAND_MAX + 1.0)); -} - -/** - * generates unsigned character random numbers in [0..rnd] - * - * @param rnd maximum number - * @return random number between 0 and rnd - */ -inline unsigned char randUC(unsigned char rnd) -{ - return (unsigned char) ((float)rnd * std::rand() / (RAND_MAX + 1.0)); -} - -/** - * Computes the angle between 2 points in polar coordinates - */ -inline double polardist(double* p, double *p2) { - double stheta = sin(p[0]) * sin(p2[0]); - double myd2 = acos( stheta * cos(p[1]) * cos(p2[1]) - + stheta * sin(p[1]) * sin(p2[1]) - + cos(p[0]) * cos(p2[0])); - return myd2; -} - - - -inline void toKartesian(double *polar, double *kart) { - kart[0] = polar[2] * cos( polar[1] ) * sin( polar[0] ); - kart[1] = polar[2] * sin( polar[1] ) * sin( polar[0] ); - kart[2] = polar[2] * cos( polar[0] ); - -} - -/** - * Transforms a point in cartesian coordinates into polar - * coordinates - */ - -inline void toPolar(double *n, double *polar) { - double phi, theta, rho; - rho = Len(n); - Normalize3(n); - - // if(fabs(1 - fabs(n[1])) < 0.001) { - // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl; - - phi = acos(n[2]); - //if ( fabs(phi) < 0.0001) phi = 0.0001; - //if ( fabs(M_PI - phi) < 0.0001) phi = 0.0001; - - double theta0; - - if(fabs(phi) < 0.0001) { - theta = 0.0; - } else if(fabs(M_PI - phi) < 0.0001) { - theta = 0.0; - } else { - if(fabs(n[0]/sin(phi)) > 1.0) { - if(n[0]/sin(phi) < 0) { - theta0 = M_PI; - } else { - theta0 = 0.0; - } - } else { - theta0 = acos(n[0]/sin(phi)); - - } - - - double sintheta = n[1]/sin(phi); - - double EPS = 0.0001; - - if(fabs(sin(theta0) - sintheta) < EPS) { - theta = theta0; - } else if(fabs( sin( 2*M_PI - theta0 ) - sintheta ) < EPS) { - theta = 2*M_PI - theta0; - } else { - theta = 0; - cout << "Fehler" << endl; - } - - } - /* } else { - theta = 0.0; - phi = 0.0; - }*/ - polar[0] = phi; - polar[1] = theta; - polar[2] = rho; - -} - -/* - * Computes the submatrix without - * row i and column j - * - * @param Min input 4x4 matrix - * @param Mout output 3x3 matrix - * @param i row index i - * @param j column index j - */ -template -static inline void M4_submat(const T *Min, T *Mout, int i, int j ) { - int di, dj, si, sj; - // loop through 3x3 submatrix - for( di = 0; di < 3; di ++ ) { - for( dj = 0; dj < 3; dj ++ ) { - // map 3x3 element (destination) to 4x4 element (source) - si = di + ( ( di >= i ) ? 1 : 0 ); - sj = dj + ( ( dj >= j ) ? 1 : 0 ); - // copy element - Mout[di * 3 + dj] = Min[si * 4 + sj]; - } - } -} - -/* - * Computes the determinant of a 4x4 matrix - * - * @param 4x4 matrix - * @return determinant - */ -template -static inline double M4det(const T *M ) -{ - T det, result = 0, i = 1.0; - T Msub3[9]; - int n; - for ( n = 0; n < 4; n++, i *= -1.0 ) { - M4_submat( M, Msub3, 0, n ); - det = M3det( Msub3 ); - result += M[n] * det * i; - } - return( result ); -} - - -/* - * invert a 4x4 Matrix - * - * @param Min input 4x4 matrix - * @param Mout output matrix - * @return 1 if successful - */ -template -static inline int M4inv(const T *Min, T *Mout ) -{ - T mdet = M4det( Min ); - if ( fabs( mdet ) < 0.0005 ) { - cout << "Error matrix inverting!" << endl; - M4identity( Mout ); - return( 0 ); - } - T mtemp[9]; - int i, j, sign; - for ( i = 0; i < 4; i++ ) { - for ( j = 0; j < 4; j++ ) { - sign = 1 - ( (i +j) % 2 ) * 2; - M4_submat( Min, mtemp, i, j ); - Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet; - } - } - return( 1 ); -} - -/* - * transposes a 4x4 matrix - * - * @param Min input 4x4 matrix - * @param Mout output 4x4 matrix - */ -template -static inline int M4transpose(const T *Min, T *Mout ) -{ - Mout[0] = Min[0]; - Mout[4] = Min[1]; - Mout[8] = Min[2]; - Mout[12] = Min[3]; - Mout[1] = Min[4]; - Mout[5] = Min[5]; - Mout[9] = Min[6]; - Mout[13] = Min[7]; - Mout[2] = Min[8]; - Mout[6] = Min[9]; - Mout[10] = Min[10]; - Mout[14] = Min[11]; - Mout[3] = Min[12]; - Mout[7] = Min[13]; - Mout[11] = Min[14]; - Mout[15] = Min[15]; - return( 1 ); -} - -/* +++++++++-------------++++++++++++ - * NAME - * choldc - * DESCRIPTION - * Cholesky Decomposition of a symmetric - * positive definite matrix - * Overwrites lower triangle of matrix - * Numerical Recipes, but has a bit of - * the fancy C++ template thing happening - * +++++++++-------------++++++++++++ */ -static inline bool choldc(double A[3][3], double diag[3]) -{ - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = i; j < 3; j++) { - double sum = A[i][j]; - for (int k=i-1; k >= 0; k--) - sum -= A[i][k] * A[j][k]; - if (i == j) { - if (sum < 1.0e-7) - return false; - diag[i] = sqrt(sum); - } else { - A[j][i] = sum / diag[i]; - } - } - } - return true; -} - - -/* +++++++++-------------++++++++++++ - * NAME - * choldc - * DESCRIPTION - * Cholesky Decomposition of a symmetric - * positive definite matrix - * Overwrites lower triangle of matrix - * Numerical Recipes, but has a bit of - * the fancy C++ template thing happening - * +++++++++-------------++++++++++++ */ -static inline bool choldc(unsigned int n, double **A, double *diag) -{ - for (unsigned int i = 0; i < n; i++) { - for (unsigned int j = i; j < n; j++) { - double sum = A[i][j]; - for (int k=i-1; k >= 0; k--) - sum -= A[i][k] * A[j][k]; - if (i == j) { - if (sum < 1.0e-7) - return false; - diag[i] = sqrt(sum); - } else { - A[j][i] = sum / diag[i]; - } - } - } - return true; -} - - -/* +++++++++-------------++++++++++++ - * NAME - * cholsl - * DESCRIPTION - * Solve Ax=B after choldc - * +++++++++-------------++++++++++++ */ -static inline void cholsl(double A[3][3], - double diag[3], - double B[3], - double x[3]) -{ - for (int i=0; i < 3; i++) { - double sum = B[i]; - for (int k=i-1; k >= 0; k--) - sum -= A[i][k] * x[k]; - x[i] = sum / diag[i]; - } - for (int i=2; i >= 0; i--) { - double sum = x[i]; - for (int k=i+1; k < 3; k++) - sum -= A[k][i] * x[k]; - x[i] = sum / diag[i]; - } -} - - -/* +++++++++-------------++++++++++++ - * NAME - * cholsl - * DESCRIPTION - * Solve Ax=B after choldc - * +++++++++-------------++++++++++++ */ -static inline void cholsl(unsigned int n, - double **A, - double *diag, - double *B, - double *x) -{ - for (unsigned int i=0; i < n; i++) { - double sum = B[i]; - for (int k=(int)i-1; k >= 0; k--) - sum -= A[i][k] * x[k]; - x[i] = sum / diag[i]; - } - for (int i=(int)n-1; i >= 0; i--) { - double sum = x[i]; - for (unsigned int k=i+1; k < n; k++) - sum -= A[k][i] * x[k]; - x[i] = sum / diag[i]; - } -} - - -/** - * Transforms a a quaternion and a translation vector into a 4x4 - * Matrix - * - * @param quat input quaternion - * @param t input translation - * @param mat output matrix - */ -static inline void QuatToMatrix4(const double *quat, const double *t, double *mat) -{ - // double q00 = quat[0]*quat[0]; - double q11 = quat[1]*quat[1]; - double q22 = quat[2]*quat[2]; - double q33 = quat[3]*quat[3]; - double q03 = quat[0]*quat[3]; - double q13 = quat[1]*quat[3]; - double q23 = quat[2]*quat[3]; - double q02 = quat[0]*quat[2]; - double q12 = quat[1]*quat[2]; - double q01 = quat[0]*quat[1]; - mat[0] = 1 - 2 * (q22 + q33); - mat[5] = 1 - 2 * (q11 + q33); - mat[10] = 1 - 2 * (q11 + q22); - mat[4] = 2.0*(q12-q03); - mat[1] = 2.0*(q12+q03); - mat[8] = 2.0*(q13+q02); - mat[2] = 2.0*(q13-q02); - mat[9] = 2.0*(q23-q01); - mat[6] = 2.0*(q23+q01); - - mat[3] = mat[7] = mat[11] = 0.0; - - if (t == 0) { - mat[12] = mat[13] = mat[14] = 0.0; - } else { - mat[12] = t[0]; - mat[13] = t[1]; - mat[14] = t[2]; - } - mat[15] = 1.0; -} - - -/** - * Transforms a 4x4 Transformation Matrix into a quaternion - * - * @param mat matrix to be converted - * @param quat resulting quaternion - * @param t resulting translation - */ -static inline void Matrix4ToQuat(const double *mat, double *quat, double *t = 0) -{ - - double T, S, X, Y, Z, W; - T = 1 + mat[0] + mat[5] + mat[10]; - if ( T > 0.00000001 ) { // to avoid large distortions! - - S = sqrt(T) * 2; - X = ( mat[9] - mat[6] ) / S; - Y = ( mat[2] - mat[8] ) / S; - Z = ( mat[4] - mat[1] ) / S; - W = 0.25 * S; - } else if ( mat[0] > mat[5] && mat[0] > mat[10] ) { // Column 0: - S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2; - X = 0.25 * S; - Y = (mat[4] + mat[1] ) / S; - Z = (mat[2] + mat[8] ) / S; - W = (mat[9] - mat[6] ) / S; - } else if ( mat[5] > mat[10] ) { // Column 1: - S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2; - X = (mat[4] + mat[1] ) / S; - Y = 0.25 * S; - Z = (mat[9] + mat[6] ) / S; - W = (mat[2] - mat[8] ) / S; - } else { // Column 2: - S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2; - X = (mat[2] + mat[8] ) / S; - Y = (mat[9] + mat[6] ) / S; - Z = 0.25 * S; - W = (mat[4] - mat[1] ) / S; - } - quat[0] = W; - quat[1] = -X; - quat[2] = -Y; - quat[3] = -Z; - - Normalize4(quat); - if (t != 0) { - t[0] = mat[12]; - t[1] = mat[13]; - t[2] = mat[14]; - } -} - -/** - * Transforms a Quaternion to the corresponding Axis-Angle representation - * - * @param quat 4-vector of quaternion - * gets overridden by the axis/angle representation - */ -static inline void QuatToAA(double *quat){ - //double x, y, z, w; - double sum = 0.0; - - double cos_a, angle, x, y, z, sin_a; - - for(int i = 0; i < 4; i++){ - sum += quat[i]*quat[i]; - } - sum = sqrt(sum); - - //quaternion_normalise( |W,X,Y,Z| ); - cos_a = quat[0]/sum; - angle = acos( cos_a ) * 2; - sin_a = sqrt( 1.0 - cos_a * cos_a ); - if ( fabs( sin_a ) < 0.0005 ) sin_a = 1; - x = quat[1] / sin_a; - y = quat[2] / sin_a; - z = quat[3] / sin_a; - - quat[0] = angle; - quat[1] = x; - quat[2] = y; - quat[3] = z; -} - -/** - * Quaternion Multiplication q1 * q2 = q3 - */ -static inline void QMult(const double *q1, const double *q2, double *q3) { - q3[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; - q3[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; - q3[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; - q3[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; -} - -/** - * Quaternion SLERP - * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ - */ -static inline void slerp(const double *qa, const double *qb, const double t, double *qm) { - // Calculate angle between them. - double cosHalfTheta = qa[0] * qb[0] + qa[1] * qb[1] + qa[2] * qb[2] + qa[3] * qb[3]; - // if qa=qb or qa=-qb then theta = 0 and we can return qa - if (fabs(cosHalfTheta) >= 1.0) { - qm[0] = qa[0]; - qm[1] = qa[1]; - qm[2] = qa[2]; - qm[3] = qa[3]; - return; - } - // Calculate temporary values. - double halfTheta = acos(cosHalfTheta); - double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); - // if theta = 180 degrees then result is not fully defined - // we could rotate around any axis normal to qa or qb - if (fabs(sinHalfTheta) < 0.001){ - qm[0] = (qa[0] * 0.5 + qb[0] * 0.5); - qm[1] = (qa[1] * 0.5 + qb[1] * 0.5); - qm[2] = (qa[2] * 0.5 + qb[2] * 0.5); - qm[3] = (qa[3] * 0.5 + qb[3] * 0.5); - Normalize4(qm); - return; - } - double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; - double ratioB = sin(t * halfTheta) / sinHalfTheta; - //calculate Quaternion. - qm[0] = (qa[0] * ratioA + qb[0] * ratioB); - qm[1] = (qa[1] * ratioA + qb[1] * ratioB); - qm[2] = (qa[2] * ratioA + qb[2] * ratioB); - qm[3] = (qa[3] * ratioA + qb[3] * ratioB); - Normalize4(qm); -} - -/* taken from ROOT (CERN) - * as well in: - * Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning - * James J. Kuffner - * - * Distance between two rotations in Quaternion form - * Note: The rotation group is isomorphic to a 3-sphere - * with diametrically opposite points identified. - * The (rotation group-invariant) is the smaller - * of the two possible angles between the images of - * the two rotations on that sphere. Thus the distance - * is never greater than pi/2. - */ -inline double quat_dist(double quat1[4], double quat2[4]) { - double chordLength = std::fabs(quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3]); - if (chordLength > 1) chordLength = 1; // in case roundoff fouls us up - return acos(chordLength) / M_PI * 180.0; -} - - - -/** - * Converts a Rotation given by Axis-Angle and a Translation into a - * 4x4 Transformation matrix - * - * @param aa axis and angle aa[0] is the angle - * @param trans vector containing the translation - * @param matrix matrix to be computed - */ -inline void AAToMatrix(double *aa, double *trans, double *matrix ){ - double rcos = cos(aa[0]); - double rsin = sin(aa[0]); - double u = aa[1]; - double v = aa[2]; - double w = aa[3]; - - matrix[0] = rcos + u*u*(1-rcos); - matrix[1] = w * rsin + v*u*(1-rcos); - matrix[2] = -v * rsin + w*u*(1-rcos); - matrix[3] = 0.0; - matrix[4] = -w * rsin + u*v*(1-rcos); - matrix[5] = rcos + v*v*(1-rcos); - matrix[6] = u * rsin + w*v*(1-rcos); - matrix[7] = 0.0; - matrix[8] = v * rsin + u*w*(1-rcos); - matrix[9] = -u * rsin + v*w*(1-rcos); - matrix[10] = rcos + w*w*(1-rcos); - matrix[11] = 0.0; - matrix[12] = trans[0]; - matrix[13] = trans[1]; - matrix[14] = trans[2]; - matrix[15] = 1.0; - -} - - -/** - * Factors matrix A into lower and upper triangular matrices - * (L and U respectively) in solving the linear equation Ax=b. - * - * @param A (input/output) Matrix(1:n, 1:n) In input, matrix to be - * factored. On output, overwritten with lower and - * upper triangular factors. - * - * @param indx (output) Vector(1:n) Pivot vector. Describes how - * the rows of A were reordered to increase - * numerical stability. - * - * @return return int(0 if successful, 1 otherwise) - */ -inline int LU_factor( double A[4][4], int indx[4]) -{ - int M = 4; - int N = 4; - - int i=0,j=0,k=0; - int jp=0; - - double t; - - int minMN = 4; - - for (j = 0; j < minMN; j++) - { - - // find pivot in column j and test for singularity. - - jp = j; - t = fabs(A[j][j]); - for (i = j+1; i < M; i++) - if ( fabs(A[i][j]) > t) - { - jp = i; - t = fabs(A[i][j]); - } - - indx[j] = jp; - - // jp now has the index of maximum element - // of column j, below the diagonal - - if ( A[jp][j] == 0 ) - return 1; // factorization failed because of zero pivot - - - if (jp != j) // swap rows j and jp - for (k = 0; k < N; k++) - { - t = A[j][k]; - A[j][k] = A[jp][k]; - A[jp][k] =t; - } - - if (j < M) // compute elements j+1:M of jth column - { - // note A(j,j), was A(jp,p) previously which was - // guarranteed not to be zero (Label #1) - // - double recp = 1.0 / A[j][j]; - - for (k = j+1; k < M; k++) - A[k][j] *= recp; - } - - if (j < minMN) - { - // rank-1 update to trailing submatrix: E = E - x*y; - // - // E is the region A(j+1:M, j+1:N) - // x is the column vector A(j+1:M,j) - // y is row vector A(j,j+1:N) - - int ii,jj; - - for (ii = j+1; ii < M; ii++) - for (jj = j+1; jj < N; jj++) - A[ii][jj] -= A[ii][j]*A[j][jj]; - } - } - - return 0; -} - -/** - * Solves a linear system via LU after LU factor - * - * @param A 4x4 matrix - * @param indx indices - * @param b 4 vectort - * - * @return 0 - * - */ -inline int LU_solve(const double A[4][4], const int indx[4], double b[4]) -{ - int i,ii=0,ip,j; - int n = 4; - double sum = 0.0; - - for (i = 0; i < n; i++) - { - ip=indx[i]; - sum=b[ip]; - b[ip]=b[i]; - if (ii) - for (j = ii;j <= i-1; j++) - sum -= A[i][j]*b[j]; - else if (sum) ii=i; - b[i]=sum; - } - for (i = n-1; i >= 0; i--) - { - sum=b[i]; - for (j = i+1; j < n; j++) - sum -= A[i][j]*b[j]; - b[i]=sum/A[i][i]; - } - - return 0; -} - -/** - * Calculates the cross product of two 4-vectors - * - * @param x input 1 - * @param y input 2 - * @param T output - * - */ -template -static inline void Cross(const T *x, const T *y, T *result) -{ - result[0] = x[1] * y[2] - x[2] * y[1]; - result[1] = x[2] * y[0] - x[0] * y[2]; - result[2] = x[0] * y[1] - x[1] * y[0]; - return; -} - -/** - * converts a quaternion to Euler angels in the roll pitch yaw system - */ -static inline void QuatRPYEuler(const double *quat, double *euler) -{ - double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]); - double s = n > 0?2./(n*n):0.; - - double m00, m10, m20, m21, m22; - - - double xs = quat[1]*s; - double ys = quat[2]*s; - double zs = quat[3]*s; - - double wx = quat[0]*xs; - double wy = quat[0]*ys; - double wz = quat[0]*zs; - - double xx = quat[1]*xs; - double xy = quat[1]*ys; - double xz = quat[1]*zs; - - double yy = quat[2]*ys; - double yz = quat[2]*zs; - - double zz = quat[3]*zs; - - m00 = 1.0 - (yy + zz); - m22 = 1.0 - (xx + yy); - - - m10 = xy + wz; - - m20 = xz - wy; - m21 = yz + wx; - - euler[0] = atan2(m21,m22); - euler[1] = atan2(-m20,sqrt(m21*m21 + m22*m22)); - euler[2] = atan2(m10,m00); -} - -/** - * converts from Euler angels in the roll pitch yaw system to a quaternion - */ -static inline void RPYEulerQuat(const double *euler, double *quat) -{ - double sphi = sin(euler[0]); - double stheta = sin(euler[1]); - double spsi = sin(euler[2]); - double cphi = cos(euler[0]); - double ctheta = cos(euler[1]); - double cpsi = cos(euler[2]); - - double _r[3][3] = { //create rotational Matrix - {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi}, - {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi}, - { -stheta, ctheta*sphi, ctheta*cphi} - }; - -#define MY_MAX(a,b) (((a)>(b))?(a):(b)) - double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0; - double _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0; - double _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0; - double _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0; - quat[0] = _w; - quat[1] = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x); - quat[2] = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y); - quat[3] = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z); -} - - -inline void transform3(const double *alignxf, double *point) -{ - double x_neu, y_neu, z_neu; - x_neu = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8]; - y_neu = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9]; - z_neu = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10]; - point[0] = x_neu + alignxf[12]; - point[1] = y_neu + alignxf[13]; - point[2] = z_neu + alignxf[14]; -} - -inline void transform3(const double *alignxf, const double *point, double *tpoint) -{ - tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12]; - tpoint[1] = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9] + alignxf[13]; - tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14]; -} - -inline std::string trim(const std::string& source) -{ - unsigned int start = 0, end = source.size() - 1; - while(source[start] == ' ') start++; - while(source[end] == ' ') end--; - return source.substr(start, end - start + 1); -} - -#endif diff --git a/.svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base b/.svn/pristine/f1/f1eb746cd290f5e154ae3b5170eca7f9967bc498.svn-base similarity index 96% rename from .svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base rename to .svn/pristine/f1/f1eb746cd290f5e154ae3b5170eca7f9967bc498.svn-base index 9a15b60..fc59256 100644 --- a/.svn/pristine/e9/e9ae1b6630a796c01e97af1666e1308120e13844.svn-base +++ b/.svn/pristine/f1/f1eb746cd290f5e154ae3b5170eca7f9967bc498.svn-base @@ -40,10 +40,10 @@ add_executable(scanserver ${SERVER_SRCS}) # scanio for ScanHandler input set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio) -if(UNIX) +if(UNIX AND NOT APPLE) # boost::interprocess uses pthread, requiring librt set(SERVER_LIBS ${SERVER_LIBS} rt) -endif(UNIX) +endif(UNIX AND NOT APPLE) if(WIN32) # 3rd party getopt library diff --git a/.svn/pristine/f1/f1f087cd06b43e80bf804fc22363696417b31a32.svn-base b/.svn/pristine/f1/f1f087cd06b43e80bf804fc22363696417b31a32.svn-base deleted file mode 100644 index 271e9f9..0000000 --- a/.svn/pristine/f1/f1f087cd06b43e80bf804fc22363696417b31a32.svn-base +++ /dev/null @@ -1,1156 +0,0 @@ -/* - * 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 << " -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' }, - { "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:SwtRadhTcb", longopts, NULL)) != -1) { - switch (c) { - case 's': - w_start = atoi(optarg); - if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } - break; - case 'e': - w_end = atoi(optarg); - if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } - if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } - break; - case 'm': - maxDist = atoi(optarg); - break; - case 'M': - minDist = atoi(optarg); - break; - case 'r': - red = atof(optarg); - break; - case 't': - readInitial = true; - break; - case 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } - break; - case 'f': - try { - w_type = formatname_to_io_type(optarg); - } catch (...) { // runtime_error - cerr << "Format " << optarg << " unknown." << endl; - abort(); - } - break; - case '?': - usage(argv[0]); - return 1; - case 'R': - types |= PointType::USE_REFLECTANCE; - break; - case 'a': - types |= PointType::USE_AMPLITUDE; - break; - case 'd': - types |= PointType::USE_DEVIATION; - break; - case 'h': - types |= PointType::USE_HEIGHT; - break; - case 'T': - types |= PointType::USE_TYPE; - break; - case 'c': - types |= PointType::USE_COLOR; - break; - case 'F': - fps = atof(optarg); - break; - case 'C': - scale = atof(optarg); - break; - case 'S': - scanserver = true; - break; - case 'o': - if (optarg) { - origin = atoi(optarg); - } else { - origin = 1; - } - break; - case '0': - saveOct = true; - break; - case '1': - loadOct = true; - break; - case 'l': - loadObj = optarg; - break; - case '2': - advanced_controls = true; - break; - 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/f3/f33703cbb6053f625e652daf442f083252e3f393.svn-base b/.svn/pristine/f3/f33703cbb6053f625e652daf442f083252e3f393.svn-base deleted file mode 100644 index 036c110..0000000 --- a/.svn/pristine/f3/f33703cbb6053f625e652daf442f083252e3f393.svn-base +++ /dev/null @@ -1,243 +0,0 @@ -/** - * @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_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 hasAmplitude(); - bool hasDeviation(); - bool hasType(); - bool hasColor(); - bool hasTime(); - bool hasIndex(); - - unsigned int getReflectance(); - 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[8]; - - bool hasType(unsigned int type); - - unsigned int getScanSize(Scan* scan); - - DataXYZ* m_xyz; - DataRGB* m_rgb; - DataReflectance* m_reflectance; - 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_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_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_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/f3/f3d07d6fd1844fb609b1520fee0191e1b31cc4d0.svn-base b/.svn/pristine/f3/f3d07d6fd1844fb609b1520fee0191e1b31cc4d0.svn-base deleted file mode 100644 index 8fe69ef..0000000 --- a/.svn/pristine/f3/f3d07d6fd1844fb609b1520fee0191e1b31cc4d0.svn-base +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); -}; - -#endif diff --git a/.svn/pristine/f4/f4804ce45241c3da4b45f1b58b006c047ee7667a.svn-base b/.svn/pristine/f4/f4804ce45241c3da4b45f1b58b006c047ee7667a.svn-base deleted file mode 100644 index 3b7e6a0..0000000 --- a/.svn/pristine/f4/f4804ce45241c3da4b45f1b58b006c047ee7667a.svn-base +++ /dev/null @@ -1,100 +0,0 @@ -/** - * @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, - }; - /** - * @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/f5/f53abef50cf6236de7d3ec8c2f832a1e7c35845a.svn-base b/.svn/pristine/f5/f53abef50cf6236de7d3ec8c2f832a1e7c35845a.svn-base deleted file mode 100644 index 3b41b0c..0000000 --- a/.svn/pristine/f5/f53abef50cf6236de7d3ec8c2f832a1e7c35845a.svn-base +++ /dev/null @@ -1,32 +0,0 @@ -/** - * @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* amplitude, std::vector* type, std::vector* deviation); - virtual bool supports(IODataType type); - - int fileCounter; -}; - -#endif diff --git a/.svn/pristine/fd/fd0a5f70fcf62b6831815e327f3e80f8c069a691.svn-base b/.svn/pristine/fd/fd0a5f70fcf62b6831815e327f3e80f8c069a691.svn-base deleted file mode 100644 index 1c72af8..0000000 --- a/.svn/pristine/fd/fd0a5f70fcf62b6831815e327f3e80f8c069a691.svn-base +++ /dev/null @@ -1,365 +0,0 @@ -/* - * scan_red implementation - * - * Copyright (C) Dorit Borrmann - * - * 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/scan.h" -#ifdef WITH_SCANSERVER -#include "scanserver/clientInterface.h" -#endif //WITH_SCANSERVER - -#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 - - -/** - * 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 << " -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 - << 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 - << " 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 - << 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; - 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 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) -{ - bool reduced = false; - int c; - // from unistd.h: - extern char *optarg; - extern int optind; - - /* 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' }, - { 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) - switch (c) - { - case 'r': - red = atof(optarg); - reduced = true; - break; - case 's': - start = atoi(optarg); - if (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); } - 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 'O': - if (optarg) { - octree = atoi(optarg); - } else { - octree = 1; - } - break; - case 'M': - minDist = atoi(optarg); - 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 - - 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, 2010" << 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; - int octree = 0; - IOType type = RIEGL_TXT; - - parseArgs(argc, argv, dir, red, start, end, maxDist, minDist, octree, type); - -#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); - } -#endif //WITH_SCANSERVER - - // Get Scans -#ifndef WITH_SCANSERVER - Scan::dir = dir; - int fileNr = start; -#endif //WITH_SCANSERVER - 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); - } - -#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->calcReducedPoints(red, octree); - - const char* id = scan->getIdentifier(); - cout << "Writing Scan No. " << id; - cout << " with " << scan->getCountReduced() << " points" << endl; - string scanFileName; - string poseFileName; - - scanFileName = dir + "reduced/scan" + id + ".3d"; - poseFileName = dir + "reduced/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; - } - 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(); - -#ifndef WITH_SCANSERVER - delete Scan::allScans[0]; - Scan::allScans.clear(); - fileNr++; -#endif //WITH_SCANSERVER - } - - cout << endl << endl; - cout << "Normal program end." << endl << endl; - -#ifdef WITH_SCANSERVER - Scan::clearScans(); - ClientInterface::destroy(); -#endif //WITH_SCANSERVER -} - diff --git a/.svn/wc.db b/.svn/wc.db index ec0bfde..2ba34b2 100644 Binary files a/.svn/wc.db and b/.svn/wc.db differ diff --git a/include/slam6d/fbr/fbr_global.h b/include/slam6d/fbr/fbr_global.h index 8aebb0a..749e9d2 100644 --- a/include/slam6d/fbr/fbr_global.h +++ b/include/slam6d/fbr/fbr_global.h @@ -82,6 +82,21 @@ namespace fbr{ RANSAC, DISABLE, }; + /** + * @enum feature_filtration_method + */ + enum feature_filtration_method{ + OCCLUSION, + STANDARD_DEVIATION, + DISABLE_FILTER, + }; + /** + * @enum matching_filtration_method + */ + enum matching_filtration_method{ + FUNDEMENTAL_MATRIX, + DISABLE_MATCHING_FILTER, + }; //RANSAC iteration #define RANSACITR 20000 //Inlier influence @@ -101,5 +116,9 @@ namespace fbr{ matcher_method stringToMatcherMethod(string method); string registrationMethodToString(registration_method method); registration_method stringToRegistrationMethod(string method); + string featureFiltrationMethodToString(feature_filtration_method method); + feature_filtration_method stringToFeatureFiltrationMethod(string method); + string matchingFiltrationMethodToString(matching_filtration_method method); + matching_filtration_method stringToMatchingFiltrationMethod(string method); } #endif /* FBR_GLOBAL_H_ */ diff --git a/include/slam6d/fbr/feature.h b/include/slam6d/fbr/feature.h index 79cf380..8bc956d 100644 --- a/include/slam6d/fbr/feature.h +++ b/include/slam6d/fbr/feature.h @@ -24,6 +24,7 @@ namespace fbr{ class feature{ feature_detector_method fDetectorMethod; feature_descriptor_method fDescriptorMethod; + feature_filtration_method fFiltrationMethod; vector keypoints; cv::Mat descriptors; enum description_method{ @@ -31,19 +32,40 @@ namespace fbr{ DESCRIPTOR_DESCRIPTION, ALL, }; + void featureFiltration(cv::Mat pImage, cv::Mat rImage); public: /** * constructor of feature class */ feature(); + feature(feature_detector_method detector, feature_descriptor_method descriptor, feature_filtration_method filtration); + /** + * featureDetection : creates the features + * @param pImage cv::Mat refletance panorama image + * @param method feature_detector_method + * @param rImage cv::Mat range image for filtration + * @param fMethod feature_filtration_method + **/ + void featureDetection(cv::Mat pImage, feature_detector_method method, cv::Mat rImage, feature_filtration_method fMethod); void featureDetection(cv::Mat pImage, feature_detector_method method); void featureDetection(cv::Mat pImage); + /** + * featureDescription : creates descriptor of detected features + * @param pImage cv::Mat reflectance panorama image + * @param method feature_descriptor_method + **/ void featureDescription(cv::Mat pImage, feature_descriptor_method method); void featureDescription(cv::Mat pImage); + /** + * all sets of getters for the class + */ feature_detector_method getDetectorMethod(); feature_descriptor_method getDescriptorMethod(); + feature_filtration_method getFeatureFiltrationMethod(); vector getFeatures(); cv::Mat getDescriptors(); + void setFeatures(vector keypoint); + void setDescriptors(cv::Mat descriptor); void getDescription(description_method method=ALL); unsigned int getNumberOfFeatures(); }; diff --git a/include/slam6d/fbr/feature_matcher.h b/include/slam6d/fbr/feature_matcher.h index 429bf21..ca5d467 100644 --- a/include/slam6d/fbr/feature_matcher.h +++ b/include/slam6d/fbr/feature_matcher.h @@ -30,21 +30,29 @@ namespace fbr{ double radius; unsigned int nOfMatches; unsigned int nOfFilteredMatches; + matching_filtration_method mFiltrationMethod; - void init(matcher_method method, int k, double r); + void init(matcher_method method, int k, double r, matching_filtration_method filtration); + void findMatches(feature qFeature, feature tFeature); public: feature_matcher(); feature_matcher(matcher_method method); feature_matcher(matcher_method method, double p); + feature_matcher(matcher_method method, double p, matching_filtration_method filtration); + void match(feature qFeature, feature tFeature); + vector getMatches(); matcher_method getMatcherMethod(); + matching_filtration_method getMatchingFiltrationMethod(); unsigned int getKnn(); double getRadius(); unsigned int getNumberOfMatches(); unsigned int getNumberOfFilteredMatches(); void getDescription(); + + void setMatchingFiltrationMethod(matching_filtration_method method); }; } #endif /* FEATURE_MATCHER_H_ */ diff --git a/include/slam6d/fbr/scan_cv.h b/include/slam6d/fbr/scan_cv.h index bb9b88f..f247c52 100644 --- a/include/slam6d/fbr/scan_cv.h +++ b/include/slam6d/fbr/scan_cv.h @@ -45,6 +45,7 @@ namespace fbr{ * @param scanServer */ scan_cv (string dir, unsigned int number, IOType format, bool scanServer); + scan_cv (string dir, unsigned int number, IOType format); /** * @brief read scan file and convert it to open cv Mat */ diff --git a/src/slam6d/fbr/fbr_global.cc b/src/slam6d/fbr/fbr_global.cc index 20773fc..50ee4df 100644 --- a/src/slam6d/fbr/fbr_global.cc +++ b/src/slam6d/fbr/fbr_global.cc @@ -11,14 +11,14 @@ #include namespace fbr{ + string scanFormatToString(IOType format){ return io_type_to_libname(format); } - + IOType stringToScanFormat(string format){ return formatname_to_io_type(format.c_str()); - } - + } string projectionMethodToString(projection_method method){ string sMethod; switch(method){ @@ -200,4 +200,50 @@ namespace fbr{ else if(strcasecmp(method.c_str(), "DISABLE") == 0) return DISABLE; else throw std::runtime_error(std::string("registration method ") + method + std::string(" is unknown")); } + + string featureFiltrationMethodToString(feature_filtration_method method){ + string fFiltrationMethod; + switch(method){ + case OCCLUSION: + fFiltrationMethod = "OCCLUSION"; + break; + case STANDARD_DEVIATION: + fFiltrationMethod = "STANDARD_DEVIATION"; + break; + case DISABLE_FILTER: + fFiltrationMethod = "DISABLE_FILTER"; + break; + default: + throw std::runtime_error(std::string("feature filtration method ") + to_string(method) + std::string(" could not be matched to a feature filtration method")); + } + return fFiltrationMethod; + } + + feature_filtration_method stringToFeatureFiltrationMethod(string method){ + if(strcasecmp(method.c_str(), "OCCLUSION") == 0) return OCCLUSION; + else if(strcasecmp(method.c_str(), "STANDARD_DEVIATION") == 0) return STANDARD_DEVIATION; + else if(strcasecmp(method.c_str(), "DISABLE_FILTER") == 0) return DISABLE_FILTER; + else throw std::runtime_error(std::string("feature filtration method ") + method + std::string(" is unknown")); + } + + string matchingFiltrationMethodToString(matching_filtration_method method){ + string mFiltrationMethod; + switch(method){ + case FUNDEMENTAL_MATRIX: + mFiltrationMethod = "FUNDEMENTAL_MATRIX"; + break; + case DISABLE_MATCHING_FILTER: + mFiltrationMethod = "DISABLE_MATCHING_FILTER"; + break; + default: + throw std::runtime_error(std::string("matching filtration method ") + to_string(method) + std::string(" could not be matched to a matching filtration method")); + } + return mFiltrationMethod; + } + + matching_filtration_method stringToMatchingFiltrationMethod(string method){ + if(strcasecmp(method.c_str(), "FUNDEMENTAL_MATRIX") == 0) return FUNDEMENTAL_MATRIX; + else if(strcasecmp(method.c_str(), "DISABLE_MATCHING_FILTER") == 0) return DISABLE_MATCHING_FILTER; + else throw std::runtime_error(std::string("matching filtration method ") + method + std::string(" is unknown")); + } } diff --git a/src/slam6d/fbr/feature.cc b/src/slam6d/fbr/feature.cc index d7cdcca..af41bf1 100644 --- a/src/slam6d/fbr/feature.cc +++ b/src/slam6d/fbr/feature.cc @@ -16,39 +16,55 @@ namespace fbr{ feature::feature(){ fDetectorMethod = SIFT_DET; fDescriptorMethod = SIFT_DES; + fFiltrationMethod = DISABLE_FILTER; } - - void feature::featureDetection(cv::Mat pImage, feature_detector_method method){ - //Detect the keypoints using SURF Detector - if(fDetectorMethod == SURF_DET){ + feature::feature(feature_detector_method detector, feature_descriptor_method descriptor, feature_filtration_method filtration){ + fDetectorMethod = detector; + fDescriptorMethod = descriptor; + fFiltrationMethod = filtration; + } + void feature::featureDetection(cv::Mat pImage, feature_detector_method method, cv::Mat rImage, feature_filtration_method fMethod){ + fDetectorMethod = method; + fFiltrationMethod = fMethod; + switch(fDetectorMethod){ + //Detect the keypoints using SURF Detector + case SURF_DET:{ double minHessian = 400; cv::SurfFeatureDetector detector(minHessian); detector.detect(pImage, keypoints); + break; } - - //Detect the keypoints using SIFT Detector - if(fDetectorMethod == SIFT_DET){ + //Detect the keypoints using SIFT Detector + case SIFT_DET:{ cv::SiftFeatureDetector detector; detector.detect(pImage, keypoints); + break; } - - //Detect the keypoints using ORB Detector - if(fDetectorMethod == ORB_DET){ + //Detect the keypoints using ORB Detector + case ORB_DET:{ cv::OrbFeatureDetector detector; detector.detect(pImage, keypoints); + break; } - - //Detect the keypoints using FAST Detector - if(fDetectorMethod == FAST_DET){ + //Detect the keypoints using FAST Detector + case FAST_DET:{ cv::FastFeatureDetector detector; detector.detect(pImage, keypoints); + break; } - - //Detect the keypoints using STAR Detector - if(fDetectorMethod == STAR_DET){ + //Detect the keypoints using STAR Detector + case STAR_DET:{ cv::StarFeatureDetector detector; detector.detect(pImage, keypoints); + break; } + } + featureFiltration(pImage, rImage); + } + + void feature::featureDetection(cv::Mat pImage, feature_detector_method method){ + cv::Mat rImage; + featureDetection(pImage, method, rImage, fFiltrationMethod); } void feature::featureDetection(cv::Mat pImage){ @@ -57,27 +73,30 @@ namespace fbr{ void feature::featureDescription(cv::Mat pImage, feature_descriptor_method method){ + fDescriptorMethod = method; if(keypoints.size() == 0) featureDetection(pImage); - - //Create descriptor using SURF - if(fDescriptorMethod == SURF_DES){ + switch(fDescriptorMethod){ + case SURF_DES:{ + //Create descriptor using SURF cv::SurfDescriptorExtractor extractor; extractor.compute(pImage, keypoints, descriptors); + break; } - - //Create descriptor using SIFT - if(fDescriptorMethod == SIFT_DES){ + case SIFT_DES:{ + //Create descriptor using SIFT cv::SiftDescriptorExtractor extractor; extractor.compute(pImage, keypoints, descriptors); + break; } - - //Create descriptor using ORB - if(fDescriptorMethod == ORB_DES){ + case ORB_DES:{ + //Create descriptor using ORB cv::OrbDescriptorExtractor extractor; extractor.compute(pImage, keypoints, descriptors); + break; } - } + } + } void feature::featureDescription(cv::Mat pImage){ featureDescription(pImage, fDescriptorMethod); @@ -90,7 +109,11 @@ namespace fbr{ feature_descriptor_method feature::getDescriptorMethod(){ return fDescriptorMethod; } - + + feature_filtration_method feature::getFeatureFiltrationMethod(){ + return fFiltrationMethod; + } + //check for the keypoints vector not to be empty vector feature::getFeatures(){ return keypoints; @@ -100,18 +123,87 @@ namespace fbr{ cv::Mat feature::getDescriptors(){ return descriptors; } + + void feature::setFeatures(vector keypoint){ + keypoints = keypoint; + } + + void feature::setDescriptors(cv::Mat descriptor){ + descriptors = descriptor; + } void feature::getDescription(description_method method){ if(method == FEATURE_DESCRIPTION) - cout<<"fDetectorMethod: "< filteredKeypoints; + if(fFiltrationMethod == OCCLUSION){ + for(unsigned int i = 0; i < keypoints.size(); i++){ + int x, y; + x = keypoints[i].pt.x; + y = keypoints[i].pt.y; + float range[8]; + if(rImage.at(y,x) != 0){ + range[0] = rImage.at(y,x)-rImage.at(y+1,x+1); + range[1] = rImage.at(y,x)-rImage.at(y,x+1); + range[2] = rImage.at(y,x)-rImage.at(y-1,x+1); + range[3] = rImage.at(y,x)-rImage.at(y-1,x); + range[4] = rImage.at(y,x)-rImage.at(y-1,x-1); + range[5] = rImage.at(y,x)-rImage.at(y,x-1); + range[6] = rImage.at(y,x)-rImage.at(y+1,x-1); + range[7] = rImage.at(y,x)-rImage.at(y+1,x); + + int count=0; + for(unsigned int j = 0; j < 8; j++){ + if(range[j] < 20) + count++; + } + if(count == 8) + filteredKeypoints.push_back(keypoints[i]); + } + } + }else if(fFiltrationMethod == STANDARD_DEVIATION){ + for(unsigned int i = 0; i < keypoints.size(); i++){ + int x, y; + x = keypoints[i].pt.x; + y = keypoints[i].pt.y; + float range[9]; + if(rImage.at(y,x) != 0){ + range[0] = rImage.at(y,x)-rImage.at(y+1,x+1); + range[1] = rImage.at(y,x)-rImage.at(y,x+1); + range[2] = rImage.at(y,x)-rImage.at(y-1,x+1); + range[3] = rImage.at(y,x)-rImage.at(y-1,x); + range[4] = rImage.at(y,x)-rImage.at(y-1,x-1); + range[5] = rImage.at(y,x)-rImage.at(y,x-1); + range[6] = rImage.at(y,x)-rImage.at(y+1,x-1); + range[7] = rImage.at(y,x)-rImage.at(y+1,x); + range[8] = rImage.at(y,x)-rImage.at(y,x); + + double t_std, r=0, temp=0; + for(unsigned int j = 0; j < 9; j++) + r += range[j]; + r /= 9; + for(unsigned int j = 0; j < 9; j++) + temp += ((range[j] - r) * (range[j] - r)); + t_std = sqrt(temp/9); + if(t_std < 0.1) + filteredKeypoints.push_back(keypoints[i]); + } + } + } + if(fFiltrationMethod != DISABLE_FILTER) + keypoints = filteredKeypoints; + } } + diff --git a/src/slam6d/fbr/feature_matcher.cc b/src/slam6d/fbr/feature_matcher.cc index 188bc1d..5f25b80 100644 --- a/src/slam6d/fbr/feature_matcher.cc +++ b/src/slam6d/fbr/feature_matcher.cc @@ -13,39 +13,49 @@ using namespace std; namespace fbr{ - void feature_matcher::init(matcher_method method, int k, double r){ + void feature_matcher::init(matcher_method method, int k, double r, matching_filtration_method filtration){ mMethod = method; knn = k; radius = r; nOfMatches = 0; nOfFilteredMatches = 0; + mFiltrationMethod = filtration; } feature_matcher::feature_matcher(){ - init(RATIO, 0, 0); + init(RATIO, 0, 0, DISABLE_MATCHING_FILTER); } feature_matcher::feature_matcher(matcher_method method){ if(method == KNN) - init(method, 3, 0); + init(method, 3, 0, DISABLE_MATCHING_FILTER); else if(method == RADIUS) - init(method, 0, 1); + init(method, 0, 1, DISABLE_MATCHING_FILTER); else - init(method, 0, 0); + init(method, 0, 0, DISABLE_MATCHING_FILTER); } feature_matcher::feature_matcher(matcher_method method, double p){ if(method == KNN) - init(method, p, 0); + init(method, p, 0, DISABLE_MATCHING_FILTER); else if(method == RADIUS) - init(method, 0, p); + init(method, 0, p, DISABLE_MATCHING_FILTER); else - init(method, 0, 0); + init(method, 0, 0, DISABLE_MATCHING_FILTER); } - - void feature_matcher::match(feature qFeature, feature tFeature){ - vector< cv::DMatch > qtInitialMatches, tqInitialMatches, gMatches; - vector > qtInitialMatchesVector, tqInitialMatchesVector; + + feature_matcher::feature_matcher(matcher_method method, double p, matching_filtration_method filtration){ + if(method == KNN) + init(method, p, 0, filtration); + else if(method == RADIUS) + init(method, 0, p, filtration); + else + init(method, 0, 0, filtration); + } + + void feature_matcher::findMatches(feature qFeature, feature tFeature){ + vector< cv::DMatch > qtInitialMatches; + vector > qtInitialMatchesVector; if(qFeature.getFeatures().size() == 0 || tFeature.getFeatures().size() == 0){ cout<<"No features has found in one or both scans!!"<::min(); + zMin = numeric_limits::max(); + nPoints = 0; + scanserver = false; + } void scan_cv::convertScanToMat(){ Scan::openDirectory(scanserver, sDir, sFormat, sNumber, sNumber); @@ -105,5 +114,5 @@ namespace fbr{ void scan_cv::getDescription(){ cout<<"load "<