josch
/
3dpcp
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

update to svn r721

Browse Source
main
josch 12 years ago
parent efb86ac6fd
commit 9bf8e054d0
119 changed files with 897 additions and 24252 deletions
Show Stats Download Patch File Download Diff File

96
.svn/pristine/00/005448ebd9d9464d1a40fef89097904af691347a.svn-base
Unescape Escape View File

@ -1,96 +0,0 @@
/**
* @file
* @brief
*
* @author Thomas Escher
*/
#ifndef MULTI_ARRAY_H
#define MULTI_ARRAY_H
#include <iostream>
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<typename return_type>
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<return_type*>(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<typename return_type>
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<return_type*>(getData()) + i);
}
//! Count of T objects in this CacheData
inline unsigned int size() const { return CacheDataAccess::getSize() / sizeof(return_type); }
};
typedef TripleArray<double> DataXYZ;
typedef TripleArray<unsigned char> DataRGB;
typedef SingleArray<float> DataReflectance;
typedef SingleArray<float> DataAmplitude;
typedef SingleArray<int> DataType;
typedef SingleArray<float> 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<typename T>
class Array {
public:
//! Create a temporary array and fill it sequentially with pointers to each point
Array(const TripleArray<T>& 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

23
.svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base → .svn/pristine/01/01306314b7639abedb5e9adb7c97d92417714645.svn-base
Unescape Escape View File

@ -12,6 +12,10 @@
#include <vector>
#include <fstream>
#include <opencv2/opencv.hpp>
//for opencv 2.4
#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
#include <opencv2/nonfree/nonfree.hpp>
#endif
#include <math.h>
#include <string>
#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_ */

90
.svn/pristine/01/01bcde80a6566f62bb36950729e0ab538661d811.svn-base
Unescape Escape View File

@ -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 <string>
#include <list>
#include <map>
#include <vector>
/**
* @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<std::string> 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<double>* xyz = 0, std::vector<unsigned char>* rgb = 0, std::vector<float>* reflectance = 0, std::vector<float>* amplitude = 0, std::vector<int>* type = 0, std::vector<float>* 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<IOType, ScanIO *> 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

75
.svn/pristine/02/023acba8dc7f8a304cfe4c94511903d976659f48.svn-base
Unescape Escape View File

@ -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<vector<vector<cv::Vec3f> > > 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_<cv::Vec4f> 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<vector<vector<cv::Vec3f> > > getExtendedMap();
panorama_map_method getMapMethod();
void getDescription();
};
}
#endif /* PANORAMA_H_ */

38
.svn/pristine/02/026e80e057a6d9159d25c5e8e26f83bbd0193ce8.svn-base
Unescape Escape View File

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

23
.svn/pristine/03/03ccc14e340c414865346c135b0fb69f2b68f058.svn-base
Unescape Escape View File

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

30
.svn/pristine/05/056f32a80b96bc1cd572918b1f14195c26777b0e.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
private:
std::string cached_dir;
std::vector<double> cached_poses;
};
#endif

15
.svn/pristine/78/78173974d7adc42895bd5ed6c64cf3d66c8813a7.svn-base → .svn/pristine/0d/0daad64cb6c1333f17a971b251ba9dabf0e903ac.svn-base
Unescape Escape View File

@ -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 "<<optopt<<endl;
usage(argc, argv);
@ -292,12 +298,13 @@ void info_yml(information info, double bError, double bErrorIdx, double* bAlign)
}
int main(int argc, char** argv){
cout<<CV_VERSION<<endl;
string out;
cv::Mat outImage;
parssArgs(argc, argv, info);
if(info.verbose >= 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);

23
.svn/pristine/10/1030b86587f750dc525d03b394a0c558d9d62919.svn-base
Unescape Escape View File

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

150
.svn/pristine/13/134e4d73b3f6526bc7ea6fc90124dc28d38d6566.svn-base
Unescape Escape View File

@ -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 <string.h> // TODO: TEST
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
#include <stdexcept>
#include <string>
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"));
}
}

220
.svn/pristine/1b/1b506d343f0ac655ba69c79f83780b07c77f8b50.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".rgb"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".dat"
std::list<std::string> ScanIO_riegl_rgb::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0 || 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<unsigned char>(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

136
.svn/pristine/1b/1bf353f1f69296fe2826ceb5a77f200b98f37b02.svn-base
Unescape Escape View File

@ -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 <cmath>
#include <iostream>
using std::ostream;
using std::istream;
#include <stdexcept>
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

429
.svn/pristine/1c/1cd1acf52aa3601767e206e9fbee1863ce165799.svn-base
Unescape Escape View File

@ -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 <SOURCE_DIR>/configure --prefix=<INSTALL_DIR>
BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/
)
MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH")
ELSE(WITH_HOGMAN)
MESSAGE(STATUS "Without HOGMAN")
ENDIF(WITH_HOGMAN)
OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF)
IF(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "exporting additional libraries")
ELSE(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "not exporting libraries")
ENDIF(EXPORT_SHARED_LIBS)
OPTION(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF)
IF(WITH_METRICS)
MESSAGE(STATUS "With metrics in slam6d.")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS")
ELSE(WITH_METRICS)
MESSAGE(STATUS "Without metrics in slam6d.")
ENDIF(WITH_METRICS)
IF(WIN32)
SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" )
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64)
add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp)
SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib")
ELSE(WIN32)
SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" )
# Add include path for OpenGL without GL/-prefix
# to avoid the include incompatibility between MACOS
# and linux
FIND_PATH(OPENGL_INC gl.h /usr/include/GL)
include_directories(${OPENGL_INC})
ENDIF(WIN32)
# Add OpenGL includes for MACOS if needed
# The OSX OpenGL frameworks natively supports freeglut extensions
IF(APPLE)
include_directories(/System/Library/Frameworks/GLUT.framework/Headers)
include_directories(/System/Library/Frameworks/OpenGL.framework/Headers)
ENDIF(APPLE)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}")
# Hide CMake variables
SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE)
SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE)
# Set output directories for libraries and executables
SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib )
SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj )
SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin )
# Set include and link dirs ...
include_directories(${CMAKE_SOURCE_DIR}/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src)
link_directories(${CMAKE_SOURCE_DIR}/obj)
link_directories(${CMAKE_SOURCE_DIR}/lib)
add_subdirectory(3rdparty)
add_subdirectory(src/slam6d)
add_subdirectory(src/scanio)
add_subdirectory(src/scanserver)
add_subdirectory(src/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)

839
.svn/pristine/1d/1d575d9db1fdef6c354a816d14a84b1c1e453eda.svn-base
Unescape Escape View File

@ -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 <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <GL/glut.h>
#include <GL/glu.h>
#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 */

451
.svn/pristine/26/266314f603cf18978cb6caed1562f09a116a9d8c.svn-base
Unescape Escape View File

@ -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, &current_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()...."<<endl;
// cout << "show_points: " << show_points << endl;
GLUI_Master.set_glutMouseFunc( CallBackMouseFunc );
GLUI_Master.set_glutKeyboardFunc( CallBackInterfaceFunc );
GLUI_Master.set_glutIdleFunc( CallBackIdleFunc );
GLUI_Master.set_glutSpecialFunc( CallBackSpecialFunc );
}
/**
* This function is called when a user starts to animate the generated path
*/
void pathAnimate(int dummy) {
//signal that the screen needs to be repainted for animation
haveToUpdate = 6;
path_iterator = 0;
}
/**
* This function clears the selected points
*/
void clearSelection(int dummy) {
for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--)
selected_points[iterator].clear();
}

54
.svn/pristine/26/26d9673062e17505dfdc3387c1b4df76dd0e77ae.svn-base
Unescape Escape View File

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

101
.svn/pristine/28/2878eac1a186f2e71956fd631c8f6434a75ca8da.svn-base
Unescape Escape View File

@ -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 <iostream>
#include <vector>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <math.h>
#include <string>
#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_ */

199
.svn/pristine/2b/2bf57854b6f6ed6517f40c82f222db3540d9ddc8.svn-base
Unescape Escape View File

@ -1,199 +0,0 @@
/*
* fbr_global implementation
*
* Copyright (C) HamidReza Houshiar
*
* Released under the GPL version 3.
*
*/
#include "slam6d/fbr/fbr_global.h"
#include <stdexcept>
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"));
}
}

212
.svn/pristine/2e/2e1762223ed226d142512a9173b2f0f09d4ac842.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".txt"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".dat"
std::list<std::string> ScanIO_riegl_txt::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0 || 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

27
.svn/pristine/2e/2ed449ccbf7fa33992f53488365eebb1fb24647d.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

287
.svn/pristine/32/32250353bae1cc0945b11ff22f9065c0f50af294.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
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<std::string> ScanIO_rxp::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> 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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* 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

260
.svn/pristine/32/325e477fa262d358d99bade889afebec00301298.svn-base
Unescape Escape View File

@ -1,260 +0,0 @@
/*
* scanHandler implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
#include "scanserver/scanHandler.h"
#include <iostream>
#include <vector>
#include <stdexcept>
using namespace std;
#include <boost/scoped_ptr.hpp>
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<SharedScan*, std::vector<double>* > ScanHandler::m_prefetch_xyz;
std::map<SharedScan*, std::vector<unsigned char>* > ScanHandler::m_prefetch_rgb;
std::map<SharedScan*, std::vector<float>* > ScanHandler::m_prefetch_reflectance;
std::map<SharedScan*, std::vector<float>* > ScanHandler::m_prefetch_amplitude;
std::map<SharedScan*, std::vector<int>* > ScanHandler::m_prefetch_type;
std::map<SharedScan*, std::vector<float>* > 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<typename T>
class PrefetchVector : public PrefetchVectorBase {
public:
PrefetchVector(SharedScan* scan, map<SharedScan*, vector<T>*>& 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<T>* 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<SharedScan*, vector<T>*>::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<T>;
}
}
//! 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<T*>(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<SharedScan*, vector<T>*>* m_prefetches;
vector<T>* 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<double> xyz(m_scan, m_prefetch_xyz);
PrefetchVector<unsigned char> rgb(m_scan, m_prefetch_rgb);
PrefetchVector<float> reflectance(m_scan, m_prefetch_reflectance);
PrefetchVector<float> amplitude(m_scan, m_prefetch_amplitude);
PrefetchVector<int> type(m_scan, m_prefetch_type);
PrefetchVector<float> 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 = &amplitude;
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;
}

52
.svn/pristine/88/88d7ba7604baa0151912d115b5883d7847f0a39d.svn-base → .svn/pristine/33/333506af4f1aa220b56eba089abbbc9af8a8e67e.svn-base
Unescape Escape View File

@ -11,14 +11,14 @@
#include <stdexcept>
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"));
}
}

1101
.svn/pristine/3a/3aebe3063370637c79be880b9229b7a0506eec33.svn-base
View File

File diff suppressed because it is too large Load Diff

2040
.svn/pristine/3f/3febf05b1fb33b70feb950db724d9edc7b2c0924.svn-base
View File

File diff suppressed because it is too large Load Diff

113
.svn/pristine/40/40986fa5eb03ba3679675bb0c5c956a8bd67fd57.svn-base
Unescape Escape View File

@ -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 <string>
#include <map>
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<std::string, std::string> 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<typename T> friend struct CheckerFactory;
static std::map<std::string, Checker* (*)(const std::string&)> 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<typename T>
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<T>::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

373
.svn/pristine/45/45a8678ada1c9c0d4a7d5ec515a6aa4d87f50ac2.svn-base
Unescape Escape View File

@ -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 <sstream>
using std::stringstream;
#include <boost/filesystem/operations.hpp>
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<Scan*>::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<double>(PointerArray<double>(get("xyz reduced original")).get(), size<DataXYZ>("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<double>* oct = new BOctTree<double>(PointerArray<double>(xyz).get(), xyz.size(), show_reduction_voxelSize);
vector<double*> 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<float> 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<float>* btree = 0;
// if loadOct is given, load the octtree under blind assumption that parameters match
if(octtree_loadOct && exists(scanFileName)) {
btree = new BOctTree<float>(scanFileName);
} else {
if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points
TripleArray<float> xyz_r(get("xyz reduced show"));
btree = new BOctTree<float>(PointerArray<float>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
} else { // without reduction, xyz + attribute points
float** pts = octtree_pointtype.createPointArray<float>(this);
unsigned int nrpts = size<DataXYZ>("xyz");
btree = new BOctTree<float>(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<float>(*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<AlgoType>(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<unsigned int>(type));
}

7
.svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base → .svn/pristine/45/45e2e25572e2899c2bcc3e7f1bdd3c3604393dd7.svn-base
Unescape Escape View File

@ -238,6 +238,13 @@ DataXYZ SharedScan::createXYZReduced(unsigned int size) {
return m_xyz_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
}
DataReflectance SharedScan::createReflectance(unsigned int size) {
// size is in units of double[1], scale to bytes
return m_reflectance->createCacheData<SharedScan::onAllocation>(size*1*sizeof(double));
}
DataXYZ SharedScan::getXYZReducedOriginal() {
return m_xyz_reduced_original->getCacheData<SharedScan::onCacheMiss>();
}

117
.svn/pristine/4b/4b27fd327b1d20021abb2011400cffc1f9cfa752.svn-base
Unescape Escape View File

@ -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<cv::KeyPoint> 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: "<<featureDetectorMethodToString(fDetectorMethod)<<", number of detected features: "<<keypoints.size()<<"."<<endl;
else if(method == DESCRIPTOR_DESCRIPTION)
cout<<"fDescriptorMethod: "<<featureDescriptorMethodToString(fDescriptorMethod)<<"."<<endl;
else
cout<<"fDetectorMethod: "<<featureDetectorMethodToString(fDetectorMethod)<<", number of detected features: "<<keypoints.size()<<", fDescriptorMethod: "<<featureDescriptorMethodToString(fDescriptorMethod)<<"."<<endl;
cout<<endl;
}
unsigned int feature::getNumberOfFeatures(){
return keypoints.size();
}
}

27
.svn/pristine/4d/4d169008a4dcff811c6ec2f36ff18531ea655b94.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

187
.svn/pristine/4e/4e804d0103295031b87d4267c45d1fd00445ad1b.svn-base
Unescape Escape View File

@ -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 <sstream>
using std::stringstream;
#include <stdexcept>
using std::runtime_error;
#include <iostream>
using std::cout;
using std::endl;
map<string, Checker* (*)(const string&)> 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<string, string>::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<string, string>::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<CheckerRangeMax> max("rangemax");
CheckerFactory<CheckerRangeMin> min("rangemin");
CheckerFactory<CheckerHeightTop> top("heighttop");
CheckerFactory<CheckerHeightBottom> 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;
}

398
.svn/pristine/51/510ce7ed25ff56a932fc7c5485d244c0abd8dd80.svn-base
Unescape Escape View File

@ -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 <list>
#include <utility>
#include <fstream>
using std::ifstream;
using std::ofstream;
using std::flush;
using std::string;
using std::map;
using std::pair;
using std::vector;
#include <boost/filesystem/operations.hpp>
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<std::string> identifiers(sio->readDirectory(path.c_str(), start, end));
Scan::allScans.reserve(identifiers.size());
// for each identifier, create a scan
for(std::list<std::string>::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<double> xyz;
vector<unsigned char> rgb;
vector<float> reflectance;
vector<float> amplitude;
vector<int> type;
vector<float> 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,
&amplitude,
&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<double*>(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<unsigned char*>(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<float*>(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<int*>(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<float*>(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<float*>(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<string, pair<unsigned char*, unsigned int>>::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<string, pair<unsigned char*, unsigned int>>::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<string, pair<unsigned char*, unsigned int>>::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<double> 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<double>(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<double>(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<float>* btree = 0;
// try to load from file, if successful return
if(octtree_loadOct && exists(scanFileName)) {
btree = new BOctTree<float>(scanFileName);
m_data.insert(
std::make_pair(
"octtree",
std::make_pair(
reinterpret_cast<unsigned char*>(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<float>(PointerArray<double>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
} else { // without reduction, xyz + attribute points
float** pts = octtree_pointtype.createPointArray<float>(this);
unsigned int nrpts = size<DataXYZ>("xyz");
btree = new BOctTree<float>(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<unsigned char*>(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<Frame>::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<AlgoType>(frame.type);
}
void BasicScan::addFrame(AlgoType type)
{
m_frames.push_back(Frame(transMat, type));
}

27
.svn/pristine/51/51f8034e47c326f2f7d893942aeacdf616bbb23a.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

602
.svn/pristine/52/5236522fa5d1061da94980ae6be0cd497574e062.svn-base
Unescape Escape View File

@ -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_<cv::Vec4f> it, double range){
iReflectance.at<uchar>(y,x) = (*it)[3]*255;//reflectance
iRange.at<float>(y,x) = range;//range
if(mapMethod == FARTHEST){
//adding the point with max distance
if( iRange.at<float>(y,x) < range ){
iMap.at<cv::Vec3f>(y,x)[0] = (*it)[0];//x
iMap.at<cv::Vec3f>(y,x)[1] = (*it)[1];//y
iMap.at<cv::Vec3f>(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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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: "<<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;
//go through all points
cv::MatIterator_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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<vector<vector<cv::Vec3f> > > panorama::getExtendedMap(){
return extendedIMap;
}
panorama_map_method panorama::getMapMethod(){
return mapMethod;
}
void panorama::getDescription(){
cout<<"panorama created with width: "<<iWidth<<", and height: "<<iHeight<<", and projection method: "<<projectionMethodToString(pMethod)<<", number of images: "<<nImages<<", projection param: "<<pParam<<"."<<endl;
cout<<endl;
}
}

278
.svn/pristine/53/535f20e64cd69a844e713f686931353c70b324a4.svn-base
Unescape Escape View File

@ -1,278 +0,0 @@
/*
* sharedScan implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
#include "scanserver/sharedScan.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <string>
#include "scanserver/clientInterface.h"
SharedScan::SharedScan(const ip::allocator<void, SegmentManager> & 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<SharedScan::onInvalidation>();
}
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<SharedScan::onInvalidation>();
m_rgb->invalidate<SharedScan::onInvalidation>();
}
void SharedScan::invalidateReduced()
{
m_xyz_reduced->invalidate<SharedScan::onInvalidation>();
m_xyz_reduced_original->invalidate<SharedScan::onInvalidation>();
}
void SharedScan::invalidateShow()
{
m_show_reduced->invalidate<SharedScan::onInvalidation>();
m_octtree->invalidate<SharedScan::onInvalidation>();
}
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<SharedScan::onCacheMiss>();
}
DataRGB SharedScan::getRGB() {
return m_rgb->getCacheData<SharedScan::onCacheMiss>();
}
DataReflectance SharedScan::getReflectance() {
return m_reflectance->getCacheData<SharedScan::onCacheMiss>();
}
DataAmplitude SharedScan::getAmplitude() {
return m_amplitude->getCacheData<SharedScan::onCacheMiss>();
}
DataType SharedScan::getType() {
return m_type->getCacheData<SharedScan::onCacheMiss>();
}
DataDeviation SharedScan::getDeviation() {
return m_deviation->getCacheData<SharedScan::onCacheMiss>();
}
DataXYZ SharedScan::getXYZReduced() {
return m_xyz_reduced->getCacheData<SharedScan::onCacheMiss>();
}
DataXYZ SharedScan::createXYZReduced(unsigned int size) {
// size is in units of double[3], scale to bytes
return m_xyz_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
}
DataXYZ SharedScan::getXYZReducedOriginal() {
return m_xyz_reduced_original->getCacheData<SharedScan::onCacheMiss>();
}
DataXYZ SharedScan::createXYZReducedOriginal(unsigned int size) {
// size is in units of double[3], scale to bytes
return m_xyz_reduced_original->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
}
TripleArray<float> SharedScan::getXYZReducedShow() {
return m_show_reduced->getCacheData<SharedScan::onCacheMiss>();
}
TripleArray<float> SharedScan::createXYZReducedShow(unsigned int size) {
return m_show_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(float));
}
DataPointer SharedScan::getOcttree() {
return m_octtree->getCacheData<SharedScan::onCacheMiss>();
}
DataPointer SharedScan::createOcttree(unsigned int size) {
return m_octtree->createCacheData<SharedScan::onAllocation>(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);
}

7
.svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base → .svn/pristine/54/54c5247d90fb88aedce1de8a13a10c0a576a83c0.svn-base
Unescape Escape View File

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

30
.svn/pristine/57/574f5472dde9dff620cd7a4b2b432c86dd1368fd.svn-base
Unescape Escape View File

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

293
.svn/pristine/5c/5cf3012e8582e4ae022c0642566732f071a25fae.svn-base
Unescape Escape View File

@ -1,293 +0,0 @@
#ifndef __COLORMANAGER_H__
#define __COLORMANAGER_H__
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#include <windows.h>
#endif
#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif
#include <string.h>
#include <stdio.h>
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

225
.svn/pristine/5d/5d349c54702145a5568fd48844c3df5cd57f4a63.svn-base
Unescape Escape View File

@ -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<typename T>
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<T*>(m_pointer) + i);
}
//! The number of T instances in this array
unsigned int size() {
return m_size / sizeof(T);
}
};
template<typename T>
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<T*>(m_pointer) + (i*3);
}
//! The number of T[3] instances in this array
unsigned int size() const {
return m_size / (3 * sizeof(T));
}
};
template<typename T>
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<T*>(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<typename T>
class PointerArray {
public:
//! Create a temporary array and fill it sequentially with pointers to points
PointerArray(const TripleArray<T>& 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<double> DataXYZ;
typedef TripleArray<float> DataXYZFloat;
typedef TripleArray<unsigned char> DataRGB;
typedef SingleArray<float> DataReflectance;
typedef SingleArray<float> DataAmplitude;
typedef SingleArray<int> DataType;
typedef SingleArray<float> DataDeviation;
#endif //DATA_TYPES_H

209
.svn/pristine/5d/5d5aa0784271d0773bf3ae3abec0d78473bbaf24.svn-base
Unescape Escape View File

@ -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<cv::KeyPoint> feature::getFeatures(){
return keypoints;
}
//check for the descriptor Mat not to be empty
cv::Mat feature::getDescriptors(){
return descriptors;
}
void feature::setFeatures(vector<cv::KeyPoint> keypoint){
keypoints = keypoint;
}
void feature::setDescriptors(cv::Mat descriptor){
descriptors = descriptor;
}
void feature::getDescription(description_method method){
if(method == FEATURE_DESCRIPTION)
cout<<"fDetectorMethod: "<<featureDetectorMethodToString(fDetectorMethod)<<", number of detected features: "<<keypoints.size()<<", feature filtration method: "<<featureFiltrationMethodToString(fFiltrationMethod)<<"."<<endl;
else if(method == DESCRIPTOR_DESCRIPTION)
cout<<"fDescriptorMethod: "<<featureDescriptorMethodToString(fDescriptorMethod)<<"."<<endl;
else
cout<<"fDetectorMethod: "<<featureDetectorMethodToString(fDetectorMethod)<<", number of detected features: "<<keypoints.size()<<", feature filtration method: "<<featureFiltrationMethodToString(fFiltrationMethod)<<", fDescriptorMethod: "<<featureDescriptorMethodToString(fDescriptorMethod)<<"."<<endl;
cout<<endl;
}
unsigned int feature::getNumberOfFeatures(){
return keypoints.size();
}
void feature::featureFiltration(cv::Mat pImage, cv::Mat rImage){
vector<cv::KeyPoint> 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<float>(y,x) != 0){
range[0] = rImage.at<float>(y,x)-rImage.at<float>(y+1,x+1);
range[1] = rImage.at<float>(y,x)-rImage.at<float>(y,x+1);
range[2] = rImage.at<float>(y,x)-rImage.at<float>(y-1,x+1);
range[3] = rImage.at<float>(y,x)-rImage.at<float>(y-1,x);
range[4] = rImage.at<float>(y,x)-rImage.at<float>(y-1,x-1);
range[5] = rImage.at<float>(y,x)-rImage.at<float>(y,x-1);
range[6] = rImage.at<float>(y,x)-rImage.at<float>(y+1,x-1);
range[7] = rImage.at<float>(y,x)-rImage.at<float>(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<float>(y,x) != 0){
range[0] = rImage.at<float>(y,x)-rImage.at<float>(y+1,x+1);
range[1] = rImage.at<float>(y,x)-rImage.at<float>(y,x+1);
range[2] = rImage.at<float>(y,x)-rImage.at<float>(y-1,x+1);
range[3] = rImage.at<float>(y,x)-rImage.at<float>(y-1,x);
range[4] = rImage.at<float>(y,x)-rImage.at<float>(y-1,x-1);
range[5] = rImage.at<float>(y,x)-rImage.at<float>(y,x-1);
range[6] = rImage.at<float>(y,x)-rImage.at<float>(y+1,x-1);
range[7] = rImage.at<float>(y,x)-rImage.at<float>(y+1,x);
range[8] = rImage.at<float>(y,x)-rImage.at<float>(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;
}
}

367
.svn/pristine/5e/5ed8495e8d30d5132dc3c320ee00fb7b4724c43c.svn-base
Unescape Escape View File

@ -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 <sstream>
using std::stringstream;
#include <boost/filesystem/operations.hpp>
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<Scan*>::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<double>(PointerArray<double>(get("xyz reduced original")).get(), size<DataXYZ>("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<double>* oct = new BOctTree<double>(PointerArray<double>(xyz).get(), xyz.size(), show_reduction_voxelSize);
vector<double*> 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<float> 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<float>* btree = 0;
// if loadOct is given, load the octtree under blind assumption that parameters match
if(octtree_loadOct && exists(scanFileName)) {
btree = new BOctTree<float>(scanFileName);
} else {
if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points
TripleArray<float> xyz_r(get("xyz reduced show"));
btree = new BOctTree<float>(PointerArray<float>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
} else { // without reduction, xyz + attribute points
float** pts = octtree_pointtype.createPointArray<float>(this);
unsigned int nrpts = size<DataXYZ>("xyz");
btree = new BOctTree<float>(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<float>(*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<AlgoType>(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<unsigned int>(type));
}

3
.svn/pristine/b2/b2657f2e0912b3ab79b3d8363eaca625fc23a19c.svn-base → .svn/pristine/5f/5fd7a2a341a6a1042dca7aae8607621b59b18eb4.svn-base
Unescape Escape View File

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

187
.svn/pristine/5f/5febe2ca6937f6f48cd72ce0c45940e082763ba5.svn-base
Unescape Escape View File

@ -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 <sstream>
using std::stringstream;
#include <stdexcept>
using std::runtime_error;
#include <iostream>
using std::cout;
using std::endl;
map<string, Checker* (*)(const string&)>* PointFilter::factory = new map<string, Checker* (*)(const string&)>;
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<string, string>::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<string, string>::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<CheckerRangeMax> max("rangemax");
CheckerFactory<CheckerRangeMin> min("rangemin");
CheckerFactory<CheckerHeightTop> top("heighttop");
CheckerFactory<CheckerHeightBottom> 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;
}

5
.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base → .svn/pristine/61/61771213756ec66e2eeacafd08cd99af19bf4be7.svn-base
Unescape Escape View File

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

132
.svn/pristine/61/61dba2dfb5388a94f80f4d66b22b747da8f6f9fa.svn-base
Unescape Escape View File

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

1101
.svn/pristine/63/639b068d1a6b6d98176edab38d89aecc832953a9.svn-base
View File

File diff suppressed because it is too large Load Diff

29
.svn/pristine/65/6509e097a9fa22fdab710646b8a518c83adb8055.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

53
.svn/pristine/65/659ab1f367762ef26220738c7bdee95d3e31096e.svn-base
Unescape Escape View File

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

147
.svn/pristine/6d/6d1e252e410290ff63a7ab6a12dd8df6a2334f6a.svn-base
Unescape Escape View File

@ -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 <string.h> // TODO: TEST
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
#include <stdexcept>
#include <string>
#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"));
}
}

786
.svn/pristine/70/700c1d451bb3afcf18070c2a847db45943f91ecf.svn-base
Unescape Escape View File

@ -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*> 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<boost::mutex> 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<boost::mutex> 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<double> *oct = new BOctTree<double>(PointerArray<double>(xyz).get(),
xyz.size(), reduction_voxelSize, reduction_pointtype);
vector<double*> 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<MetaScan*>(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<MetaScan*>(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 <double*> &diff,
Scan* Source, Scan* Target,
int thread_num,
double max_dist_match2)
{
DataXYZ xyz_r(Source->get("xyz reduced"));
KDtree* kd = new KDtree(PointerArray<double>(Target->get("xyz reduced")).get(), Target->size<DataXYZ>("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 <PtPair> *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<double>(Source->get("xyz reduced")).get(), Source->size<DataXYZ>("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 <PtPair> *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 <PtPair> *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<MetaScan*>(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<Scan*>::iterator it = scans.begin(); it != scans.end(); ++it) {
unsigned int count = (*it)->size<DataXYZ>("xyz reduced");
if(count > max)
max = count;
}
return max;
}

56
.svn/pristine/73/73f4b34ee616fbdb36fe3cc2d7dcec5f665d89c3.svn-base
Unescape Escape View File

@ -1,56 +0,0 @@
/**
* @file
* @brief
*
* @author Thomas Escher
*/
#ifndef SCAN_HANDLER_H
#define SCAN_HANDLER_H
#include "scanserver/temporaryHandler.h"
#include <map>
#include <vector>
/**
* @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<SharedScan*, std::vector<double>* > m_prefetch_xyz;
static std::map<SharedScan*, std::vector<unsigned char>* > m_prefetch_rgb;
static std::map<SharedScan*, std::vector<float>* > m_prefetch_reflectance;
static std::map<SharedScan*, std::vector<float>* > m_prefetch_amplitude;
static std::map<SharedScan*, std::vector<int>* > m_prefetch_type;
static std::map<SharedScan*, std::vector<float>* > m_prefetch_deviation;
};
#endif //SCAN_HANDLER_H

78
.svn/pristine/74/7483d87ea44f6915500ff9a9769a1515e60abd03.svn-base
Unescape Escape View File

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

27
.svn/pristine/7a/7a45e11ec1b989b997aff8206dfc0eecc5ed896d.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

67
.svn/pristine/7e/7e38fcad981dbe5497326fc210013caee144cd78.svn-base
Unescape Escape View File

@ -1,67 +0,0 @@
#ifndef BASIC_SCAN_H
#define BASIC_SCAN_H
#include "scan.h"
#include "frame.h"
#include <vector>
#include <map>
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<double *> &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<std::string, std::pair<unsigned char*, unsigned int>> m_data;
std::vector<Frame> 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

178
.svn/pristine/83/83e02dd194de2159c56fa35afd43cd6c2258ef99.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".3d"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
std::list<std::string> ScanIO_uos::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0) {
// open data file
ifstream data_file(data_path);
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
// 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

234
.svn/pristine/85/8558b059f349f14fc583f6af2ef10c56bd455dee.svn-base
Unescape Escape View File

@ -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<vector<cv::DMatch> > 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!"<<endl;
cout<<"qFeature DescriptorMethod="<<qFeature.getDescriptorMethod()<<endl;
cout<<"tFeature DescriptorMethod="<<tFeature.getDescriptorMethod()<<endl;
} else if( qFeature.getDescriptorMethod() == SURF_DES || qFeature.getDescriptorMethod() == SIFT_DES){
if(mMethod == KNN){
cv::FlannBasedMatcher matcher;
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, knn);
matcher.knnMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, knn);
}
if(mMethod == RADIUS){
cv::FlannBasedMatcher matcher;
matcher.radiusMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, radius);
matcher.radiusMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, radius);
}
if(mMethod == KNN || mMethod == RADIUS){
//find the matches that has been found in both way
for(unsigned int i = 0; i < qtInitialMatchesVector.size(); i++){
for(unsigned int j = 0; j < qtInitialMatchesVector[i].size(); j++){
cv::DMatch forward = qtInitialMatchesVector[i][j];
for(unsigned int k = 0; k < tqInitialMatchesVector[forward.trainIdx].size(); k++){
cv::DMatch backward = tqInitialMatchesVector[forward.trainIdx][k];
if(backward.trainIdx == forward.queryIdx)
matches.push_back(forward);
}
//matches.push_back(qtInitialMatchesVector[i][j]);
}
}
}
if(mMethod == RATIO){
cv::FlannBasedMatcher matcher;
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, 2);
for(unsigned int i = 0; i < qtInitialMatchesVector.size(); i++){
float ratio = qtInitialMatchesVector[i][0].distance/qtInitialMatchesVector[i][1].distance;
if(ratio < 0.8)
matches.push_back(qtInitialMatchesVector[i][0]);
}
matcher.knnMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, 2);
for(unsigned int i = 0; i < tqInitialMatchesVector.size(); i++){
float ratio = tqInitialMatchesVector[i][0].distance/tqInitialMatchesVector[i][1].distance;
if(ratio < 0.8){
cv::DMatch tq_qt;
tq_qt.queryIdx = tqInitialMatchesVector[i][0].trainIdx;
tq_qt.trainIdx = tqInitialMatchesVector[i][0].queryIdx;
tq_qt.imgIdx = tqInitialMatchesVector[i][0].imgIdx;
tq_qt.distance = tqInitialMatchesVector[i][0].distance;
matches.push_back(tq_qt);
}
}
}
if(mMethod == BRUTEFORCE){
cv::BruteForceMatcher< cv::L2<float> > matcher;
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
matcher.match(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatches);
}
if(mMethod == FLANN){
cv::FlannBasedMatcher matcher;
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
matcher.match(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatches);
}
if(mMethod == FLANN || mMethod == BRUTEFORCE)
{
//add the intersection of both way matches
for(unsigned int i = 0; i < qtInitialMatches.size(); i++){
for(unsigned int j =0 ; j<tqInitialMatches.size(); j++){
if(qtInitialMatches[i].queryIdx == tqInitialMatches[j].trainIdx && qtInitialMatches[i].trainIdx == tqInitialMatches[j].queryIdx){
matches.push_back(qtInitialMatches[i]);
}
}
}
}
}
//Matching descriptors using BruteFore with Hamming distance for ORB descriptor
else if(qFeature.getDescriptorMethod() == ORB_DES){
if(mMethod == KNN){
cv::BruteForceMatcher< cv::Hamming > matcher;
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, knn);
matcher.knnMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, knn);
}
if(mMethod == RADIUS){
cv::BruteForceMatcher< cv::Hamming > matcher;
matcher.radiusMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, radius);
matcher.radiusMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, radius);
}
if(mMethod == KNN || mMethod == RADIUS){
for(unsigned int i = 0; i < qtInitialMatchesVector.size(); i++){
for(unsigned int j = 0; j < qtInitialMatchesVector[i].size(); j++){
cv::DMatch forward = qtInitialMatchesVector[i][j];
for(unsigned int k = 0; k < tqInitialMatchesVector[forward.trainIdx].size(); k++){
cv::DMatch backward = tqInitialMatchesVector[forward.trainIdx][k];
if(backward.trainIdx == forward.queryIdx)
matches.push_back(forward);
}
//matches.push_back(qtInitialMatchesVector[i][j]);
}
}
}
if(mMethod == RATIO){
cv::BruteForceMatcher< cv::Hamming > matcher;
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, 2);
for(unsigned int i = 0; i < qtInitialMatchesVector.size(); i++){
float ratio = qtInitialMatchesVector[i][0].distance/qtInitialMatchesVector[i][1].distance;
if(ratio < 0.8)
matches.push_back(qtInitialMatchesVector[i][0]);
}
matcher.knnMatch(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatchesVector, 2);
for(unsigned int i = 0; i < tqInitialMatchesVector.size(); i++){
float ratio = tqInitialMatchesVector[i][0].distance/tqInitialMatchesVector[i][1].distance;
if(ratio < 0.8){
cv::DMatch tq_qt;
tq_qt.queryIdx = tqInitialMatchesVector[i][0].trainIdx;
tq_qt.trainIdx = tqInitialMatchesVector[i][0].queryIdx;
tq_qt.imgIdx = tqInitialMatchesVector[i][0].imgIdx;
tq_qt.distance = tqInitialMatchesVector[i][0].distance;
matches.push_back(tq_qt);
}
}
}
if(mMethod == BRUTEFORCE){
cv::BruteForceMatcher< cv::Hamming > matcher;
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
matcher.match(tFeature.getDescriptors(), qFeature.getDescriptors(), tqInitialMatches);
for(unsigned int i = 0; i < qtInitialMatches.size(); i++){
for(unsigned int j =0 ; j<tqInitialMatches.size(); j++){
if(qtInitialMatches[i].queryIdx == tqInitialMatches[j].trainIdx && qtInitialMatches[i].trainIdx == tqInitialMatches[j].queryIdx){
matches.push_back(qtInitialMatches[i]);
}
}
}
}
}
//filter the matches with RANSAC and FundementalMatrix
vector<cv::Point2f> points_1, points_2;
for( unsigned int i = 0; i < matches.size(); i++ ){
//Get the keypoints from the intersection of both matches
points_1.push_back( qFeature.getFeatures()[ matches[i].queryIdx ].pt );
points_2.push_back( tFeature.getFeatures()[ matches[i].trainIdx ].pt );
}
//calculating the fundemental matrix
cv::Mat fStatus;
cv::Mat fundementalMatrix = findFundamentalMat( points_1, points_2, cv::FM_RANSAC, 3, 0.99, fStatus);
cv::MatIterator_<uchar> it, end;
int counter = 0;
//get the inliers from fundemental matrix
for( it = fStatus.begin<uchar>(), end = fStatus.end<uchar>(); it != end; ++it){
if(*it == 1)
gMatches.push_back(matches[counter]);
counter++;
}
nOfMatches = matches.size();
matches = gMatches;
nOfFilteredMatches = matches.size();
}
vector<cv::DMatch> feature_matcher::getMatches(){
return matches;
}
matcher_method feature_matcher::getMatcherMethod(){
return mMethod;
}
unsigned int feature_matcher::getKnn(){
return knn;
}
double feature_matcher::getRadius(){
return radius;
}
unsigned int feature_matcher::getNumberOfMatches(){
return nOfMatches;
}
unsigned int feature_matcher::getNumberOfFilteredMatches(){
return nOfFilteredMatches;
}
void feature_matcher::getDescription(){
cout<<"number of Matches: "<<nOfMatches<<", number of Matches after filteration: "<<nOfFilteredMatches<<", matching method: "<<matcherMethodToString(mMethod)<<", knn: "<<knn<<", radius: "<<radius<<"."<<endl;
cout<<endl;
}
}

27
.svn/pristine/85/85b258b4b246c5c8685a1cf81f28382ada0c5ae6.svn-base
Unescape Escape View File

@ -1,27 +0,0 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_RIEGL_RGB_H__
#define __SCAN_IO_RIEGL_RGB_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for Riegl scans
*
* The compiled class is available as shared object file
*/
class ScanIO_riegl_rgb : public ScanIO {
public:
virtual std::list<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

27
.svn/pristine/88/88c651fb430d20a21ea462e35c81221ed7b479f6.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

83
.svn/pristine/8a/8aa0fd587375c1ce4a10896c482f7f46e412009b.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
ScanIO_rxp() : dec(0), imp(0) {}
private:
std::tr1::shared_ptr<basic_rconnection> 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<double>* xyz, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* 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<double>* xyz, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
this->filter = &filter;
this->xyz = xyz;
this->reflectance = reflectance;
this->amplitude = amplitude;
this->type = type;
this->deviation = deviation;
}
protected:
PointFilter* filter;
std::vector<double> *xyz;
std::vector<float> *reflectance;
std::vector<float> *amplitude;
std::vector<int> *type;
std::vector<float> *deviation;
int start;
int currentscan;
// overridden from pointcloud class
void on_echo_transformed(echo_type echo);
void on_frame_stop(const scanlib::frame_stop<iterator_type>& arg) {
scanlib::basic_packets::on_frame_stop(arg);
currentscan++;
}
};
#endif

544
.svn/pristine/8e/8e8044a660eb823e80d445550a523dfb4f4ad5be.svn-base
Unescape Escape View File

@ -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_<cv::Vec4f> it, double range){
iReflectance.at<uchar>(y,x) = (*it)[3]*255;//reflectance
iRange.at<float>(y,x) = range;//range
if(mapMethod == FARTHEST){
//adding the point with max distance
if( iRange.at<float>(y,x) < range ){
iMap.at<cv::Vec3f>(y,x)[0] = (*it)[0];//x
iMap.at<cv::Vec3f>(y,x)[1] = (*it)[1];//y
iMap.at<cv::Vec3f>(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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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: "<<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;
//go through all points
cv::MatIterator_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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_<cv::Vec4f> it, end;
for( it = scan.begin<cv::Vec4f>(), end = scan.end<cv::Vec4f>(); 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<vector<vector<cv::Vec3f> > > panorama::getExtendedMap(){
return extendedIMap;
}
panorama_map_method panorama::getMapMethod(){
return mapMethod;
}
void panorama::getDescription(){
cout<<"panorama created with width: "<<iWidth<<", and height: "<<iHeight<<", and projection method: "<<projectionMethodToString(pMethod)<<", number of images: "<<nImages<<", projection param: "<<pParam<<"."<<endl;
cout<<endl;
}
}

27
.svn/pristine/91/918c474185616fe51687a5df7fe5c5306bf064b5.svn-base
Unescape Escape View File

@ -1,27 +0,0 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_RIEGL_RGB_H__
#define __SCAN_IO_RIEGL_RGB_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for Riegl scans
*
* The compiled class is available as shared object file
*/
class ScanIO_riegl_rgb : public ScanIO {
public:
virtual std::list<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

27
.svn/pristine/94/94aad30f03099c1bb55ea4a9d35ba4b1d7ebbf43.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

410
.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base
Unescape Escape View File

@ -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 <SOURCE_DIR>/configure --prefix=<INSTALL_DIR>
BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/
)
MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH")
ELSE(WITH_HOGMAN)
MESSAGE(STATUS "Without HOGMAN")
ENDIF(WITH_HOGMAN)
OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF)
IF(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "exporting additional libraries")
ELSE(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "not exporting libraries")
ENDIF(EXPORT_SHARED_LIBS)
OPTION(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF)
IF(WITH_METRICS)
MESSAGE(STATUS "With metrics in slam6d.")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS")
ELSE(WITH_METRICS)
MESSAGE(STATUS "Without metrics in slam6d.")
ENDIF(WITH_METRICS)
IF(WIN32)
SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" )
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64)
add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp)
SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib")
ELSE(WIN32)
SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" )
# Add include path for OpenGL without GL/-prefix
# to avoid the include incompatibility between MACOS
# and linux
FIND_PATH(OPENGL_INC gl.h /usr/include/GL)
include_directories(${OPENGL_INC})
ENDIF(WIN32)
# Add OpenGL includes for MACOS if needed
# The OSX OpenGL frameworks natively supports freeglut extensions
IF(APPLE)
include_directories(/System/Library/Frameworks/GLUT.framework/Headers)
include_directories(/System/Library/Frameworks/OpenGL.framework/Headers)
ENDIF(APPLE)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}")
# Hide CMake variables
SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE)
SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE)
# Set output directories for libraries and executables
SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib )
SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj )
SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin )
# Set include and link dirs ...
include_directories(${CMAKE_SOURCE_DIR}/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src)
link_directories(${CMAKE_SOURCE_DIR}/obj)
link_directories(${CMAKE_SOURCE_DIR}/lib)
add_subdirectory(3rdparty)
add_subdirectory(src/slam6d)
add_subdirectory(src/scanio)
add_subdirectory(src/scanserver)
add_subdirectory(src/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)

210
.svn/pristine/98/981260cea4afb6b9d6083b246f02e4ab7cc78183.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "Color_ScanPos"
#define DATA_PATH_SUFFIX " - Scan001.txt"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
std::list<std::string> ScanIO_ks_rgb::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
// 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<unsigned char>(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

191
.svn/pristine/99/99042653e5414e8d0ef85202b942814bbe7741fa.svn-base
Unescape Escape View File

@ -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 <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/smart_ptr/shared_ptr.hpp>
#include <boost/interprocess/containers/string.hpp>
#include <boost/interprocess/containers/vector.hpp>
// 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<ip::offset_ptr<SharedScan>, SegmentManager> SharedScanAllocator;
typedef ip::vector<ip::offset_ptr<SharedScan>, SharedScanAllocator> SharedScanVector;
// allocator and type for a shared string
typedef ip::allocator<char, SegmentManager> CharAllocator;
typedef ip::basic_string<char, std::char_traits<char>, CharAllocator> SharedString;
// shared pointer for shared strings
typedef ip::managed_shared_ptr<SharedString, ip::managed_shared_memory>::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<void, SegmentManager> & 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<float> getXYZReducedShow();
//! Create a new set of reduced points for use in show
TripleArray<float> 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<double> m_pose;
ip::offset_ptr<CacheObject> 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

17
.svn/pristine/26/2638410539b1ae55fb27e0ce177aeb5f4ff37aec.svn-base → .svn/pristine/99/993014ebe3bd111ee6476ca3768d71d8dffcaa3c.svn-base
Unescape Escape View File

@ -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<double>::min();
zMin = numeric_limits<double>::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<double>::min();
zMin = numeric_limits<double>::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?" <<endl;
@ -105,5 +114,5 @@ namespace fbr{
void scan_cv::getDescription(){
cout<<"load "<<sDir<<", with scan number: " <<sNumber<<", with "<<nPoints<<" points, sFormat: "<<scanFormatToString(sFormat)<<"."<<endl;
cout<<endl;
}
}
}

208
.svn/pristine/a0/a0f29033774d5457a328f769e2a81a55be32c6d1.svn-base
Unescape Escape View File

@ -1,208 +0,0 @@
/*
* scan_io_rts implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_rts.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
using std::vector;
#include <sstream>
using std::stringstream;
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "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<std::string> ScanIO_rts::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> 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<double> 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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* 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

54
.svn/pristine/a1/a14e8af6728cf25d35e5998e359a86e9a97c0f16.svn-base
Unescape Escape View File

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

184
.svn/pristine/a3/a339abce00e5fad291d9254e33d2d31d2f896586.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".3d"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
std::list<std::string> ScanIO_uos_rgb::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_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<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0 || 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<unsigned char>(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

413
.svn/pristine/a9/a9b4d93ac03e9cc4801286237eb28c62b5e270e9.svn-base
Unescape Escape View File

@ -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 <string>
#include <vector>
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
//! SearchTree types
enum nns_type {
simpleKD, ANNTree, BOCTree
};
class Scan;
typedef std::vector<Scan*> 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<Scan*> 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<DataXYZ>("xyz reduced")
*/
template<typename T>
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<PtPair> *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<double*> &diff,
Scan* Source, Scan* Target,
int thread_num,
double max_dist_match2);
static void getPtPairsSimple(std::vector<PtPair> *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<PtPair> *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

22
.svn/pristine/4b/4bf1b18c6d10bddab8417bca4a084c618d997cdd.svn-base → .svn/pristine/aa/aad02cf22e8bc65e9505091a1025edcfa9bbde97.svn-base
Unescape Escape View File

@ -24,6 +24,7 @@ namespace fbr{
class feature{
feature_detector_method fDetectorMethod;
feature_descriptor_method fDescriptorMethod;
feature_filtration_method fFiltrationMethod;
vector<cv::KeyPoint> 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<cv::KeyPoint> getFeatures();
cv::Mat getDescriptors();
void setFeatures(vector<cv::KeyPoint> keypoint);
void setDescriptors(cv::Mat descriptor);
void getDescription(description_method method=ALL);
unsigned int getNumberOfFeatures();
};

37
.svn/pristine/ab/ab9a0fd1b9110d10fecaef454992190248c843de.svn-base
Unescape Escape View File

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

121
.svn/pristine/b0/b0899606b686fa7de519588d6e0f4b2e5114e315.svn-base
Unescape Escape View File

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

30
.svn/pristine/b1/b1999e9ec85e2d12c77311bdb1fb49e2417cd2c1.svn-base
Unescape Escape View File

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

90
.svn/pristine/b3/b3dc22d5dd32cd1944fd0faaaebf87a53fe5ebee.svn-base
Unescape Escape View File

@ -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 <string>
#include <list>
#include <map>
#include <vector>
#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<std::string> 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<double>* xyz = 0, std::vector<unsigned char>* rgb = 0, std::vector<float>* reflectance = 0, std::vector<float>* amplitude = 0, std::vector<int>* type = 0, std::vector<float>* 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<IOType, ScanIO *> 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

30
.svn/pristine/b7/b76c973a65c7eeca48efd6354118c18ab53822db.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
private:
std::string cached_dir;
std::vector<double> cached_poses;
};
#endif

7
.svn/pristine/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base → .svn/pristine/b8/b8577205bc1d064eb45065d3f2c765f76e36976b.svn-base
Unescape Escape View File

@ -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.");
}
}

222
.svn/pristine/bc/bc4f85bc33d567e7ebf04c3645a2e2afa7aee5ac.svn-base
Unescape Escape View File

@ -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<vector<cv::DMatch> > qtInitialMatchesVector;
if(qFeature.getFeatures().size() == 0 || tFeature.getFeatures().size() == 0){
cout<<"No features has found in one or both scans!!"<<endl;
exit(-1);
}
//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!"<<endl;
cout<<"qFeature DescriptorMethod="<<qFeature.getDescriptorMethod()<<endl;
cout<<"tFeature DescriptorMethod="<<tFeature.getDescriptorMethod()<<endl;
} else if( qFeature.getDescriptorMethod() == SURF_DES || qFeature.getDescriptorMethod() == SIFT_DES){
if(mMethod == KNN){
cv::FlannBasedMatcher matcher;
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, knn);
}
if(mMethod == RADIUS){
cv::FlannBasedMatcher matcher;
matcher.radiusMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, radius);
}
if(mMethod == RATIO){
cv::FlannBasedMatcher matcher;
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, 2);
}
if(mMethod == BRUTEFORCE){
//opencv 2.4
#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv::BFMatcher matcher (cv::NORM_L2);
#else //older version of opencv than 2.4
cv::BruteForceMatcher< cv::L2<float> > matcher;
#endif
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
qtInitialMatchesVector.push_back(qtInitialMatches);
}
if(mMethod == FLANN){
cv::FlannBasedMatcher matcher;
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
qtInitialMatchesVector.push_back(qtInitialMatches);
}
}
//Matching descriptors using Hamming distance for ORB descriptor
else if(qFeature.getDescriptorMethod() == ORB_DES){
if(mMethod == KNN){
//opencv 2.4
#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv::BFMatcher matcher (cv::NORM_HAMMING);
#else //older version of opencv than 2.4
cv::BruteForceMatcher< cv::Hamming > matcher;
#endif
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, knn);
}
if(mMethod == RADIUS){
//opencv 2.4
#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv::BFMatcher matcher (cv::NORM_HAMMING);
#else //older version of opencv than 2.4
cv::BruteForceMatcher< cv::Hamming > matcher;
#endif
matcher.radiusMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, radius);
}
if(mMethod == RATIO){
//opencv 2.4
#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv::BFMatcher matcher (cv::NORM_HAMMING);
#else //older version of opencv than 2.4
cv::BruteForceMatcher< cv::Hamming > matcher;
#endif
matcher.knnMatch(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatchesVector, 2);
}
if(mMethod == BRUTEFORCE){
//opencv 2.4
#if (CV_MAJOR_VERSION >= 2) && (CV_MINOR_VERSION >= 4)
cv::BFMatcher matcher (cv::NORM_HAMMING);
#else //older version of opencv than 2.4
cv::BruteForceMatcher< cv::Hamming > matcher;
#endif
matcher.match(qFeature.getDescriptors(), tFeature.getDescriptors(), qtInitialMatches);
qtInitialMatchesVector.push_back(qtInitialMatches);
}
}
if(mMethod == RATIO){
for(unsigned int i = 0; i < qtInitialMatchesVector.size(); i++){
float ratio = qtInitialMatchesVector[i][0].distance/qtInitialMatchesVector[i][1].distance;
if(ratio < 0.66)
matches.push_back(qtInitialMatchesVector[i][0]);
}
}else{
for(unsigned int i = 0; i < qtInitialMatchesVector.size(); i++){
for(unsigned int j = 0; j < qtInitialMatchesVector[i].size(); j++){
matches.push_back(qtInitialMatchesVector[i][j]);
}
}
}
}
void feature_matcher::match(feature qFeature, feature tFeature){
vector< cv::DMatch > gMatches;
findMatches(qFeature, tFeature);
if(mFiltrationMethod == FUNDEMENTAL_MATRIX){
//filter the matches with RANSAC and FundementalMatrix
vector<cv::Point2f> points_1, points_2;
for( unsigned int i = 0; i < matches.size(); i++ ){
//Get the keypoints from the intersection of both matches
points_1.push_back( qFeature.getFeatures()[ matches[i].queryIdx ].pt );
points_2.push_back( tFeature.getFeatures()[ matches[i].trainIdx ].pt );
}
//calculating the fundemental matrix
cv::Mat fStatus;
cv::Mat fundementalMatrix = findFundamentalMat( points_1, points_2, cv::FM_RANSAC, 3, 0.99, fStatus);
cv::MatIterator_<uchar> it, end;
int counter = 0;
//get the inliers from fundemental matrix
for( it = fStatus.begin<uchar>(), end = fStatus.end<uchar>(); it != end; ++it){
if(*it == 1)
gMatches.push_back(matches[counter]);
counter++;
}
nOfMatches = matches.size();
matches = gMatches;
nOfFilteredMatches = matches.size();
}else{
nOfMatches = matches.size();
nOfFilteredMatches = matches.size();
}
}
vector<cv::DMatch> feature_matcher::getMatches(){
return matches;
}
matcher_method feature_matcher::getMatcherMethod(){
return mMethod;
}
matching_filtration_method feature_matcher::getMatchingFiltrationMethod(){
return mFiltrationMethod;
}
unsigned int feature_matcher::getKnn(){
return knn;
}
double feature_matcher::getRadius(){
return radius;
}
unsigned int feature_matcher::getNumberOfMatches(){
return nOfMatches;
}
unsigned int feature_matcher::getNumberOfFilteredMatches(){
return nOfFilteredMatches;
}
void feature_matcher::getDescription(){
cout<<"number of Matches: "<<nOfMatches<<", number of Matches after filteration: "<<nOfFilteredMatches<<", matching method: "<<matcherMethodToString(mMethod)<<", matching filtration method: "<<matchingFiltrationMethodToString(mFiltrationMethod)<<", knn: "<<knn<<", radius: "<<radius<<"."<<endl;
cout<<endl;
}
void feature_matcher::setMatchingFiltrationMethod(matching_filtration_method method){
mFiltrationMethod = method;
}
}

53
.svn/pristine/bd/bdb4df0b64a5b2583826a9e1d6e7ce7dbbc54d3d.svn-base
Unescape Escape View File

@ -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 AND NOT APPLE)
# boost::interprocess uses pthread, requiring librt
#set(CLIENT_LIBS ${CLIENT_LIBS} rt)
endif(UNIX AND NOT APPLE)
target_link_libraries(scanclient ${CLIENT_LIBS})
# SERVER EXECUTABLE
# build by source
set(SERVER_SRCS
scanserver.cc serverInterface.cc frame_io.cc serverScan.cc
cache/cacheManager.cc cache/cacheHandler.cc scanHandler.cc
temporaryHandler.cc cacheIO.cc
)
add_executable(scanserver ${SERVER_SRCS})
# add libraries
# boost::interprocess/filesystem
# scanclient basic functionality
# scanio for ScanHandler input
set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio)
if(UNIX)
# boost::interprocess uses pthread, requiring librt
set(SERVER_LIBS ${SERVER_LIBS} rt)
endif(UNIX)
if(WIN32)
# 3rd party getopt library
set(SERVER_LIBS ${SERVER_LIBS} XGetopt)
endif(WIN32)
target_link_libraries(scanserver ${SERVER_LIBS})

368
.svn/pristine/c2/c2bdc507f968e0d7232c7f2823597d087a22af03.svn-base
Unescape Escape View File

@ -1,368 +0,0 @@
/*
* scan_diff implementation
*
* Copyright (C) Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main program calculating difference of 3D scans.
*
* Program for calculating the difference between scans after matching with slam6d
* Usage: bin/scan_diff -s <START> -e <END> -d <NR> '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 <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <fstream>
using std::ofstream;
using std::ifstream;
#include <errno.h>
#include "slam6d/scan.h"
#ifdef WITH_SCANSERVER
#include "scanserver/clientInterface.h"
#endif //WITH_SCANSERVER
#include "slam6d/globals.icc"
#ifdef _OPENMP
#include <omp.h>
#endif
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#include <windows.h>
#include <direct.h>
#else
#include <sys/stat.h>
#include <sys/types.h>
#include <strings.h>
#include <dlfcn.h>
#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 <START> -e <END> [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 <NR> -s <NR> -e <NR> '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<double*> 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;
}

388
.svn/pristine/c6/c6d9b992f3fbba4d552e92e9a2be5beef93a8977.svn-base
Unescape Escape View File

@ -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 <list>
#include <utility>
#include <fstream>
using std::ifstream;
using std::ofstream;
using std::flush;
using std::string;
using std::map;
using std::pair;
using std::vector;
#include <boost/filesystem/operations.hpp>
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<std::string> identifiers(sio->readDirectory(path.c_str(), start, end));
Scan::allScans.reserve(identifiers.size());
// for each identifier, create a scan
for(std::list<std::string>::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<double> xyz;
vector<unsigned char> rgb;
vector<float> reflectance;
vector<float> amplitude;
vector<int> type;
vector<float> 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,
&amplitude,
&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<double*>(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<unsigned char*>(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<float*>(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<int*>(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<float*>(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<float*>(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<string, pair<unsigned char*, unsigned int>>::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<string, pair<unsigned char*, unsigned int>>::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<string, pair<unsigned char*, unsigned int>>::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<double> 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<double>(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<double>(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<float>* btree = 0;
// try to load from file, if successful return
if(octtree_loadOct && exists(scanFileName)) {
btree = new BOctTree<float>(scanFileName);
m_data.insert(
std::make_pair(
"octtree",
std::make_pair(
reinterpret_cast<unsigned char*>(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<float>(PointerArray<double>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
} else { // without reduction, xyz + attribute points
float** pts = octtree_pointtype.createPointArray<float>(this);
unsigned int nrpts = size<DataXYZ>("xyz");
btree = new BOctTree<float>(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<unsigned char*>(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<Frame>::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<AlgoType>(frame.type);
}
void BasicScan::addFrame(AlgoType type)
{
m_frames.push_back(Frame(transMat, type));
}

10
.svn/pristine/f5/f5ab2bbd50dc2ab4cf98b901dc3ad06a9d485f95.svn-base → .svn/pristine/c7/c72f40b9ef40f81592743b6b80469d0844006443.svn-base
Unescape Escape View File

@ -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<cv::DMatch> 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_ */

2
.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base → .svn/pristine/c9/c94a2dbcd5aaf61d12d04948a745e39d2817debc.svn-base
Unescape Escape View File

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

44
.svn/pristine/ca/ca80512c8fe75d732a8b699062e271ddc38b6b5d.svn-base
Unescape Escape View File

@ -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<void, SegmentManager> & 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));
}

171
.svn/pristine/cb/cb61b0ec7b6de027098b23bd3cb5250702e83988.svn-base
Unescape Escape View File

@ -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 <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".3d"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
std::list<std::string> ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_uosr::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
// read 6 plain doubles
for(i = 0; i < 6; ++i) pose_file >> pose[i];
pose_file.close();
// convert angles from deg to rad
for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]);
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_uosr::supports(IODataType type)
{
return !!(type & ( DATA_REFLECTANCE | DATA_XYZ ));
}
void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0) {
// open data file
ifstream data_file(data_path);
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
// overread the first line ignoring the header information
char dummy[255];
data_file.getline(dummy, 255);
// read points and reflectance/intensity/temperature value
double point[3];
float reflection;
while(data_file.good()) {
try {
for(i = 0; i < 3; ++i) data_file >> point[i];
data_file >> reflection;
} catch(std::ios_base::failure& e) {
break;
}
// apply filter then insert point and reflectance
if(filter.check(point)) {
for(i = 0; i < 3; ++i) xyz->push_back(point[i]);
reflectance->push_back(reflection);
}
}
data_file.close();
}
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) ScanIO* create()
#else
extern "C" ScanIO* create()
#endif
{
return new ScanIO_uosr;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
#else
extern "C" void destroy(ScanIO *sio)
#endif
{
delete sio;
}
#ifdef _MSC_VER
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
{
return TRUE;
}
#endif

27
.svn/pristine/cd/cd204dc38a13e943910a2bca09ff2e4e0a9450f9.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

259
.svn/pristine/cf/cf648c1f7b309705a9b14b550ce8879c79995ca8.svn-base
Unescape Escape View File

@ -1,259 +0,0 @@
/*
* sharedScan implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
#include "scanserver/sharedScan.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <string>
#include "scanserver/clientInterface.h"
SharedScan::SharedScan(const ip::allocator<void, SegmentManager> & 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<SharedScan::onInvalidation>();
}
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<SharedScan::onInvalidation>();
m_rgb->invalidate<SharedScan::onInvalidation>();
}
void SharedScan::invalidateReduced()
{
m_xyz_reduced->invalidate<SharedScan::onInvalidation>();
m_xyz_reduced_original->invalidate<SharedScan::onInvalidation>();
}
void SharedScan::invalidateShow()
{
m_show_reduced->invalidate<SharedScan::onInvalidation>();
m_octtree->invalidate<SharedScan::onInvalidation>();
}
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<SharedScan::onCacheMiss>();
}
DataRGB SharedScan::getRGB() {
return m_rgb->getCacheData<SharedScan::onCacheMiss>();
}
DataReflectance SharedScan::getReflectance() {
return m_reflectance->getCacheData<SharedScan::onCacheMiss>();
}
DataAmplitude SharedScan::getAmplitude() {
return m_amplitude->getCacheData<SharedScan::onCacheMiss>();
}
DataType SharedScan::getType() {
return m_type->getCacheData<SharedScan::onCacheMiss>();
}
DataDeviation SharedScan::getDeviation() {
return m_deviation->getCacheData<SharedScan::onCacheMiss>();
}
DataXYZ SharedScan::getXYZReduced() {
return m_xyz_reduced->getCacheData<SharedScan::onCacheMiss>();
}
DataXYZ SharedScan::createXYZReduced(unsigned int size) {
// size is in units of double[3], scale to bytes
return m_xyz_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
}
DataXYZ SharedScan::getXYZReducedOriginal() {
return m_xyz_reduced_original->getCacheData<SharedScan::onCacheMiss>();
}
DataXYZ SharedScan::createXYZReducedOriginal(unsigned int size) {
// size is in units of double[3], scale to bytes
return m_xyz_reduced_original->createCacheData<SharedScan::onAllocation>(size*3*sizeof(double));
}
TripleArray<float> SharedScan::getXYZReducedShow() {
return m_show_reduced->getCacheData<SharedScan::onCacheMiss>();
}
TripleArray<float> SharedScan::createXYZReducedShow(unsigned int size) {
return m_show_reduced->createCacheData<SharedScan::onAllocation>(size*3*sizeof(float));
}
DataPointer SharedScan::getOcttree() {
return m_octtree->getCacheData<SharedScan::onCacheMiss>();
}
DataPointer SharedScan::createOcttree(unsigned int size) {
return m_octtree->createCacheData<SharedScan::onAllocation>(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);
}

146
.svn/pristine/d1/d13ff10647a40654467dc2c9b310e3f1253e8696.svn-base
Unescape Escape View File

@ -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 <string.h> // TODO: TEST
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
#include <stdexcept>
#include <string>
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"));
}
}

209
.svn/pristine/d1/d1a7f039c995aa109e797d0b0a562acbfa31dcc9.svn-base
Unescape Escape View File

@ -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 <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <fstream>
#include <string.h>
#include <stdexcept>
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<char*>(&types), sizeof(unsigned int));
return PointType(types);
}
void PointType::serialize(std::ofstream &f) {
f.write(reinterpret_cast<char*>(&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<DataXYZ>("xyz");
}

413
.svn/pristine/d4/d4ef74555c6dae4125e756d976951b6c7a7e8ed7.svn-base
Unescape Escape View File

@ -1,413 +0,0 @@
/*
* feature_based_registration implementation
*
* Copyright (C) HamidReza Houshiar
*
* Released under the GPL version 3.
*
*/
#include <stdio.h>
#include <fstream>
#include "slam6d/fbr/fbr_global.h"
#include "slam6d/fbr/scan_cv.h"
#include "slam6d/fbr/panorama.h"
#include "slam6d/fbr/feature.h"
#include "slam6d/fbr/feature_matcher.h"
#include "slam6d/fbr/registration.h"
using namespace std;
using namespace fbr;
struct information{
string local_time;
string dir, outDir;
int iWidth, iHeight, nImages, minDistance, minError, minInlier, fScanNumber, sScanNumber, verbose;
double pParam, mParam;
IOType sFormat;
projection_method pMethod;
feature_detector_method fMethod;
feature_descriptor_method dMethod;
matcher_method mMethod;
registration_method rMethod;
int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum;
double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime;
} info;
/**
* usage : explains how to use the program CMD
*/
void usage(int argc, char** argv){
printf("\n");
printf("USAGE: %s dir firstScanNumber secondScanNumber \n", argv[0]);
printf("\n");
printf("\n");
printf("\tOptions:\n");
printf("\t\t-F scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n");
printf("\t\t-W iWidth\t\t panorama image width\n");
printf("\t\t-H iHeight\t\t panorama image height\n");
printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n");
printf("\t\t-N nImage\t\t number of images used for some projections\n");
printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n");
printf("\t\t-f fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n");
printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n");
printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n");
printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n");
printf("\t\t-E minError \t\t threshold for min error in registration process\n");
printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n");
printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n");
printf("\t\t-r registration \t registration method [ALL|ransac]\n");
printf("\t\t-V verbose \t\t level of verboseness\n");
printf("\t\t-O outDir \t\t output directory if not stated same as input\n");
printf("\n");
printf("\tExamples:\n");
printf("\tUsing Bremen City dataset:\n");
printf("\tLoading scan000.txt and scan001.txt:\n");
printf("\t\t %s ~/dir/to/bremen_city 0 1\n", argv[0]);
printf("\tLoading scan005.txt and scan006.txt and output panorma images and feature images and match images in ~/dir/to/bremen_city/out dir:\n");
printf("\t\t %s -V 1 -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 5 6 \n", argv[0]);
printf("\tLoading scan010.txt and scan011.txt using Mercator projection and SURF feature detector and SIFT descriptor:\n");
printf("\t\t %s -p MERCATOR -f SURF -d SIFT -O ~/dir/to/bremen_city/out/ ~/dir/to/bremen_city 10 11 \n", argv[0]);
printf("\n");
exit(1);
}
void parssArgs(int argc, char** argv, information& info){
cout<<"parssArgs"<<endl;
time_t rawtime;
struct tm *timeinfo;
time(&rawtime);
char time[50];
timeinfo = localtime (&rawtime);
sprintf(time, "%d-%d-%d-%d:%d:%d", timeinfo->tm_year + 1900, timeinfo->tm_mon + 1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
info.local_time = time;
//default values
info.iWidth = 3600;
info.iHeight = 1000;
info.nImages = 1;
info.minDistance = 50;
info.minError = 50;
info.minInlier = 5;
info.verbose = 0;
//depend on the projection method
info.pParam = 0;
info.mParam = 0;
//===============================
info.sFormat = RIEGL_TXT;
info.pMethod = EQUIRECTANGULAR;
info.fMethod = SIFT_DET;
info.dMethod = SIFT_DES;
info.mMethod = RATIO;
info.rMethod = RANSAC;
info.outDir = "";
int c;
opterr = 0;
//reade the command line and get the options
while ((c = getopt (argc, argv, "F:W:H:p:N:P:f:d:m:D:E:I:M:r:V:O:")) != -1)
switch (c)
{
case 'F':
info.sFormat = stringToScanFormat(optarg);
break;
case 'W':
info.iWidth = atoi(optarg);
break;
case 'H':
info.iHeight = atoi(optarg);
break;
case 'p':
info.pMethod = stringToProjectionMethod(optarg);
break;
case 'N':
info.nImages = atoi(optarg);
break;
case 'P':
info.pParam = atoi(optarg);
break;
case 'f':
info.fMethod = stringToFeatureDetectorMethod(optarg);
break;
case 'd':
info.dMethod = stringToFeatureDescriptorMethod(optarg);
break;
case 'm':
info.mMethod = stringToMatcherMethod(optarg);
break;
case 'D':
info.minDistance = atoi(optarg);
break;
case 'E':
info.minError = atoi(optarg);
break;
case 'I':
info.minInlier = atoi(optarg);
break;
case 'M':
info.mParam = atoi(optarg);
break;
case 'r':
info.rMethod = stringToRegistrationMethod(optarg);
break;
case 'V':
cout<<"verboos"<<endl;
info.verbose = atoi(optarg);
break;
case 'O':
info.outDir = optarg;
break;
case '?':
cout<<"Unknown option character "<<optopt<<endl;
usage(argc, argv);
break;
default:
usage(argc, argv);
}
if(info.pMethod == PANNINI && info.pParam == 0){
info.pParam = 1;
if(info.nImages < 3) info.nImages = 3;
}
if(info.pMethod == STEREOGRAPHIC && info.pParam == 0){
info.pParam = 2;
if(info.nImages < 3) info.nImages = 3;
}
if(info.pMethod == RECTILINEAR && info.nImages < 3)
info.nImages = 3;
if(info.mMethod == KNN && info.mParam == 0)
info.mParam = 3;
if(info.mMethod == RADIUS && info.mParam == 0)
info.mParam = 100;
if(info.dMethod == ORB_DES && info.fMethod == SIFT_DET){
cout<<"Error: SIFT feature doesn't work with ORB descriptor."<<endl;
usage(argc, argv);
}
if(info.mMethod == FLANN && info.dMethod == ORB_DES){
cout<<"Error: ORB descriptoronly works with BRUTEFORCE matcher."<<endl;
usage(argc, argv);
}
if (optind > argc - 3)
{
cout<<"Too few input arguments. At least dir and two scan numbers are required."<<endl;
usage(argc, argv);
}
info.dir = argv[optind];
info.fScanNumber = atoi(argv[optind+1]);
info.sScanNumber = atoi(argv[optind+2]);
if(info.outDir.empty()) info.outDir = info.dir;
else if(info.outDir.compare(info.outDir.size()-1, 1, "/") != 0) info.outDir += "/";
}
void informationDescription(information info){
cout<<"program parameters are:"<<endl;
cout<<endl;
cout<<"local time: "<<info.local_time<<endl;
cout<<"input dir: "<<info.dir<<endl;
cout<<"output dir: "<<info.outDir<<endl;
cout<<"first scan number: "<<info.fScanNumber<<endl;
cout<<"second scan number: "<<info.sScanNumber<<endl;
cout<<"scan format: "<<scanFormatToString(info.sFormat)<<endl;
cout<<endl;
cout<<"image width: "<<info.iWidth<<endl;
cout<<"image height: "<<info.iHeight<<endl;
cout<<"number of images: "<<info.nImages<<endl;
cout<<"projection parameter: "<<info.pParam<<endl;
cout<<"projection method: "<<projectionMethodToString(info.pMethod)<<endl;
cout<<endl;
cout<<"feature detector method: "<<featureDetectorMethodToString(info.fMethod)<<endl;
cout<<"feature descriptor method: "<<featureDescriptorMethodToString(info.dMethod)<<endl;
cout<<endl;
cout<<"matcher parameter: "<<info.mParam<<endl;
cout<<"matcher method: "<<matcherMethodToString(info.mMethod)<<endl;
cout<<endl;
cout<<"min distacne: "<<info.minDistance<<endl;
cout<<"min error: "<<info.minError<<endl;
cout<<"min inlier: "<<info.minInlier<<endl;
cout<<"registration method: "<<registrationMethodToString(info.rMethod)<<endl;
cout<<endl;
}
void info_yml(information info, double bError, double bErrorIdx, double* bAlign){
cv::Mat align(16, 1, CV_32FC(1), cv::Scalar::all(0));
for(int i = 0 ; i < 16 ; i++)
align.at<float>(i,0) = bAlign[i];
string yml;
yml = info.outDir+"fbr-yml.yml";
cv::FileStorage fs(yml.c_str(), cv::FileStorage::APPEND);
fs << "feature_bas_registration" << "{";
fs << "pair" << "{" << "scan" << to_string(info.fScanNumber, 3);
fs << "scan" << to_string(info.sScanNumber, 3) << "}";
fs << "time" << "{" << "local_time" << info.local_time << "}";
fs << "param" << "{";
fs << "DIR" << info.dir;
fs << "sFormat" << scanFormatToString(info.sFormat);
fs << "pMethod" << projectionMethodToString(info.pMethod);
fs << "nImage" << info.nImages;
fs << "pParam" << info.pParam;
fs << "iWidth" << info.iWidth;
fs << "iHeight" << info.iHeight;
fs << "fMethod" << featureDetectorMethodToString(info.fMethod);
fs << "dMethod" << featureDescriptorMethodToString(info.dMethod);
fs << "mMethod" << matcherMethodToString(info.mMethod);
fs << "mParam" << info.mParam;
fs << "rMethod" << registrationMethodToString(info.rMethod);
fs << "minDistance" << info.minDistance;
fs << "minInlier" << info.minInlier;
fs << "minError" << info.minError;
fs << "}";
fs << "input" << "{";
fs << "first_input" << "{";
fs << "name" << "{" << "scan" << to_string(info.fScanNumber, 3) << "}";
fs << "point" << "{" << "amount" << info.fSPoints << "time" << info.fSTime << "}";
fs << "projection" << "{" << "time" << info.fPTime << "}";
fs << "feature" << "{" << "amount" << info.fFNum << "fTime" << info.fFTime << "dTime" << info.fDTime << "}";
fs << "}";
fs << "second_input" << "{";
fs << "name" << "{" << "scan" << to_string(info.sScanNumber, 3) << "}";
fs << "point" << "{" << "amount" << info.sSPoints << "time" << info.sSTime << "}";
fs << "projection" << "{" << "time" << info.sPTime << "}";
fs << "feature" << "{" << "amount" << info.sFNum << "fTime" << info.sFTime << "dTime" << info.sDTime << "}";
fs << "}";
fs << "}";
fs << "matches" << "{";
fs << "amount" << info.mNum << "filteration" << info.filteredMNum << "time" << info.mTime << "}";
fs << "reg" << "{";
fs << "bestError" << bError << "bestErrorIdx" << bErrorIdx << "time" << info.rTime << "bAlign" << align << "}";
fs << "}";
}
int main(int argc, char** argv){
string out;
cv::Mat outImage;
parssArgs(argc, argv, info);
cout<<"out of parssArgs"<<endl;
if(info.verbose >= 1) informationDescription(info);
scan_cv fScan (info.dir, info.fScanNumber, info.sFormat);
if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount();
fScan.convertScanToMat();
if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency();
if(info.verbose >= 2) fScan.getDescription();
panorama fPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam);
if(info.verbose >= 4) info.fPTime = (double)cv::getTickCount();
fPanorama.createPanorama(fScan.getMatScan());
if(info.verbose >= 4) info.fPTime = ((double)cv::getTickCount() - info.fPTime)/cv::getTickFrequency();
if(info.verbose >= 2) fPanorama.getDescription();
//write panorama to image
if(info.verbose >= 1){
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg";
imwrite(out, fPanorama.getReflectanceImage());
}
feature fFeature;
if(info.verbose >= 4) info.fFTime = (double)cv::getTickCount();
fFeature.featureDetection(fPanorama.getReflectanceImage(), info.fMethod);
if(info.verbose >= 4) info.fFTime = ((double)cv::getTickCount() - info.fFTime)/cv::getTickFrequency();
//write panorama with keypoints to image
if(info.verbose >= 1){
cv::drawKeypoints(fPanorama.getReflectanceImage(), fFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg";
imwrite(out, outImage);
outImage.release();
}
if(info.verbose >= 4) info.fDTime = (double)cv::getTickCount();
fFeature.featureDescription(fPanorama.getReflectanceImage(), info.dMethod);
if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency();
if(info.verbose >= 2) fFeature.getDescription();
scan_cv sScan (info.dir, info.sScanNumber, info.sFormat);
if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount();
sScan.convertScanToMat();
if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency();
if(info.verbose >= 2) sScan.getDescription();
panorama sPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam);
if(info.verbose >= 4) info.sPTime = (double)cv::getTickCount();
sPanorama.createPanorama(sScan.getMatScan());
if(info.verbose >= 4) info.sPTime = ((double)cv::getTickCount() - info.sPTime)/cv::getTickFrequency();
if(info.verbose >= 2) sPanorama.getDescription();
//write panorama to image
if(info.verbose >= 1){
out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg";
imwrite(out, sPanorama.getReflectanceImage());
}
feature sFeature;
if(info.verbose >= 4) info.sFTime = (double)cv::getTickCount();
sFeature.featureDetection(sPanorama.getReflectanceImage(), info.fMethod);
if(info.verbose >= 4) info.sFTime = ((double)cv::getTickCount() - info.sFTime)/cv::getTickFrequency();
//write panorama with keypoints to image
if(info.verbose >= 1){
cv::drawKeypoints(sPanorama.getReflectanceImage(), sFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg";
imwrite(out, outImage);
outImage.release();
}
if(info.verbose >= 4) info.sDTime = (double)cv::getTickCount();
sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod);
if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency();
if(info.verbose >= 2) sFeature.getDescription();
feature_matcher matcher (info.mMethod, info.mParam);
if(info.verbose >= 4) info.mTime = (double)cv::getTickCount();
matcher.match(fFeature, sFeature);
if(info.verbose >= 4) info.mTime = ((double)cv::getTickCount() - info.mTime)/cv::getTickFrequency();
if(info.verbose >= 2) matcher.getDescription();
//write matcheed feature to image
if(info.verbose >= 1){
cv::drawMatches(fPanorama.getReflectanceImage(), fFeature.getFeatures(), sPanorama.getReflectanceImage(), sFeature.getFeatures(), matcher.getMatches(), outImage, cv::Scalar::all(-1), cv::Scalar::all(-1), vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+".jpg";
imwrite(out, outImage);
outImage.release();
}
registration reg (info.minDistance, info.minError, info.minInlier, info.rMethod);
if(info.verbose >= 4) info.rTime = (double)cv::getTickCount();
reg.findRegistration(fPanorama.getMap(), fFeature.getFeatures(), sPanorama.getMap(), sFeature.getFeatures(), matcher.getMatches());
if(info.verbose >= 4) info.rTime = ((double)cv::getTickCount() - info.rTime)/cv::getTickFrequency();
if(info.verbose >= 2) reg.getDescription();
//write .dat and .frames files
if(info.verbose >= 0){
double *bAlign = reg.getBestAlign();
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".dat";
ofstream dat(out.c_str());
dat << bAlign[0] << " " << bAlign[4] << " " << bAlign[8] << " " << bAlign[12] <<endl;
dat << bAlign[1] << " " << bAlign[5] << " " << bAlign[9] << " " << bAlign[13] <<endl;
dat << bAlign[2] << " " << bAlign[6] << " " << bAlign[10] << " " << bAlign[14] <<endl;
dat << bAlign[3] << " " << bAlign[7] << " " << bAlign[11] << " " << bAlign[15] <<endl;
dat.close();
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".frames";
ofstream frames(out.c_str());
for (int n = 0 ; n < 2 ; n++)
{
for(int i = 0; i < 16; i++)
frames << bAlign[i] <<" ";
frames << "2" << endl;
}
frames.close();
}
if(info.verbose >= 3){
info.fSPoints = fScan.getNumberOfPoints();
info.sSPoints = sScan.getNumberOfPoints();
info.fFNum = fFeature.getNumberOfFeatures();
info.sFNum = sFeature.getNumberOfFeatures();
info.mNum = matcher.getNumberOfMatches();
info.filteredMNum = matcher.getNumberOfFilteredMatches();
info_yml(info, reg.getBestError(), reg.getBestErrorIndex(), reg.getBestAlign());
}
return 0;
}

401
.svn/pristine/d5/d593695c94d1fb12549aa3ea48556304f91dc6a4.svn-base
Unescape Escape View File

@ -1,401 +0,0 @@
/*
* feature_based_registration implementation
*
* Copyright (C) HamidReza Houshiar
*
* Released under the GPL version 3.
*
*/
#include <stdio.h>
#include <fstream>
#include "slam6d/fbr/fbr_global.h"
#include "slam6d/fbr/scan_cv.h"
#include "slam6d/fbr/panorama.h"
#include "slam6d/fbr/feature.h"
#include "slam6d/fbr/feature_matcher.h"
#include "slam6d/fbr/registration.h"
using namespace std;
using namespace fbr;
struct information{
string local_time;
string dir, outDir;
int iWidth, iHeight, nImages, minDistance, minError, minInlier, fScanNumber, sScanNumber, verbose;
double pParam, mParam;
IOType sFormat;
projection_method pMethod;
feature_detector_method fMethod;
feature_descriptor_method dMethod;
matcher_method mMethod;
registration_method rMethod;
int fSPoints, sSPoints, fFNum, sFNum, mNum, filteredMNum;
double fSTime, sSTime, fPTime, sPTime, fFTime, sFTime, fDTime, sDTime, mTime, rTime;
} info;
/**
* usage : explains how to use the program CMD
*/
void usage(int argc, char** argv){
printf("\n");
printf("USAGE: %s dir firstScanNumber secondScanNumber \n", argv[0]);
printf("\n");
printf("\n");
printf("\tOptions:\n");
printf("\t\t-F scanFormat\t\t input scan file format [RIEGL_TXT|RXP|ALL SLAM6D SCAN_IO]\n");
printf("\t\t-W iWidth\t\t panorama image width\n");
printf("\t\t-H iHeight\t\t panorama image height\n");
printf("\t\t-p pMethod\t\t projection method [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|RECTILINEAR|PANNINI|STEREOGRAPHIC|ZAXIS]\n");
printf("\t\t-N nImage\t\t number of images used for some projections\n");
printf("\t\t-P pParam\t\t special projection parameter (d for Pannini and r for stereographic)\n");
printf("\t\t-f fMethod\t\t feature detection method [SURF|SIFT|ORB|FAST|STAR]\n");
printf("\t\t-d dMethod\t\t feature description method [SURF|SIFT|ORB]\n");
printf("\t\t-m mMethod\t\t feature matching method [BRUTEFORCE|FLANN|KNN|RADIUS|RATIO]\n");
printf("\t\t-D minDistance \t\t threshold for min distance in registration process\n");
printf("\t\t-E minError \t\t threshold for min error in registration process\n");
printf("\t\t-I minInlier \t\t threshold for min number of inliers in registration process\n");
printf("\t\t-M mParam \t\t special matching paameter (knn for KNN and r for radius)\n");
printf("\t\t-r registration \t registration method [ALL|ransac]\n");
printf("\t\t-V verbose \t\t level 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 "<<optopt<<endl;
usage(argc, argv);
break;
default:
usage(argc, argv);
}
if(info.pMethod == PANNINI && info.pParam == 0){
info.pParam = 1;
if(info.nImages < 3) info.nImages = 3;
}
if(info.pMethod == STEREOGRAPHIC && info.pParam == 0){
info.pParam = 2;
if(info.nImages < 3) info.nImages = 3;
}
if(info.pMethod == RECTILINEAR && info.nImages < 3)
info.nImages = 3;
if(info.mMethod == KNN && info.mParam == 0)
info.mParam = 3;
if(info.mMethod == RADIUS && info.mParam == 0)
info.mParam = 100;
if(info.dMethod == ORB_DES && info.fMethod == SIFT_DET){
cout<<"Error: SIFT feature doesn't work with ORB descriptor."<<endl;
usage(argc, argv);
}
if(info.mMethod == FLANN && info.dMethod == ORB_DES){
cout<<"Error: ORB descriptoronly works with BRUTEFORCE matcher."<<endl;
usage(argc, argv);
}
if (optind > argc - 3)
{
cout<<"Too few input arguments. At least dir and two scan numbers are required."<<endl;
usage(argc, argv);
}
info.dir = argv[optind];
info.fScanNumber = atoi(argv[optind+1]);
info.sScanNumber = atoi(argv[optind+2]);
if(info.outDir.empty()) info.outDir = info.dir;
else if(info.outDir.compare(info.outDir.size()-1, 1, "/") != 0) info.outDir += "/";
}
void informationDescription(information info){
cout<<"program parameters are:"<<endl;
cout<<endl;
cout<<"local time: "<<info.local_time<<endl;
cout<<"input dir: "<<info.dir<<endl;
cout<<"output dir: "<<info.outDir<<endl;
cout<<"first scan number: "<<info.fScanNumber<<endl;
cout<<"second scan number: "<<info.sScanNumber<<endl;
cout<<"scan format: "<<scanFormatToString(info.sFormat)<<endl;
cout<<endl;
cout<<"image width: "<<info.iWidth<<endl;
cout<<"image height: "<<info.iHeight<<endl;
cout<<"number of images: "<<info.nImages<<endl;
cout<<"projection parameter: "<<info.pParam<<endl;
cout<<"projection method: "<<projectionMethodToString(info.pMethod)<<endl;
cout<<endl;
cout<<"feature detector method: "<<featureDetectorMethodToString(info.fMethod)<<endl;
cout<<"feature descriptor method: "<<featureDescriptorMethodToString(info.dMethod)<<endl;
cout<<endl;
cout<<"matcher parameter: "<<info.mParam<<endl;
cout<<"matcher method: "<<matcherMethodToString(info.mMethod)<<endl;
cout<<endl;
cout<<"min distacne: "<<info.minDistance<<endl;
cout<<"min error: "<<info.minError<<endl;
cout<<"min inlier: "<<info.minInlier<<endl;
cout<<"registration method: "<<registrationMethodToString(info.rMethod)<<endl;
cout<<endl;
}
void info_yml(information info, double bError, double bErrorIdx, double* bAlign){
cv::Mat align(16, 1, CV_32FC(1), cv::Scalar::all(0));
for(int i = 0 ; i < 16 ; i++)
align.at<float>(i,0) = bAlign[i];
string yml;
yml = info.outDir+"fbr-yml.yml";
cv::FileStorage fs(yml.c_str(), cv::FileStorage::APPEND);
fs << "feature_bas_registration" << "{";
fs << "pair" << "{" << "scan" << to_string(info.fScanNumber, 3);
fs << "scan" << to_string(info.sScanNumber, 3) << "}";
fs << "time" << "{" << "local_time" << info.local_time << "}";
fs << "param" << "{";
fs << "DIR" << info.dir;
fs << "sFormat" << scanFormatToString(info.sFormat);
fs << "pMethod" << projectionMethodToString(info.pMethod);
fs << "nImage" << info.nImages;
fs << "pParam" << info.pParam;
fs << "iWidth" << info.iWidth;
fs << "iHeight" << info.iHeight;
fs << "fMethod" << featureDetectorMethodToString(info.fMethod);
fs << "dMethod" << featureDescriptorMethodToString(info.dMethod);
fs << "mMethod" << matcherMethodToString(info.mMethod);
fs << "mParam" << info.mParam;
fs << "rMethod" << registrationMethodToString(info.rMethod);
fs << "minDistance" << info.minDistance;
fs << "minInlier" << info.minInlier;
fs << "minError" << info.minError;
fs << "}";
fs << "input" << "{";
fs << "first_input" << "{";
fs << "name" << "{" << "scan" << to_string(info.fScanNumber, 3) << "}";
fs << "point" << "{" << "amount" << info.fSPoints << "time" << info.fSTime << "}";
fs << "projection" << "{" << "time" << info.fPTime << "}";
fs << "feature" << "{" << "amount" << info.fFNum << "fTime" << info.fFTime << "dTime" << info.fDTime << "}";
fs << "}";
fs << "second_input" << "{";
fs << "name" << "{" << "scan" << to_string(info.sScanNumber, 3) << "}";
fs << "point" << "{" << "amount" << info.sSPoints << "time" << info.sSTime << "}";
fs << "projection" << "{" << "time" << info.sPTime << "}";
fs << "feature" << "{" << "amount" << info.sFNum << "fTime" << info.sFTime << "dTime" << info.sDTime << "}";
fs << "}";
fs << "}";
fs << "matches" << "{";
fs << "amount" << info.mNum << "filteration" << info.filteredMNum << "time" << info.mTime << "}";
fs << "reg" << "{";
fs << "bestError" << bError << "bestErrorIdx" << bErrorIdx << "time" << info.rTime << "bAlign" << align << "}";
fs << "}";
}
int main(int argc, char** argv){
string out;
cv::Mat outImage;
parssArgs(argc, argv, info);
if(info.verbose >= 1) informationDescription(info);
scan_cv fScan (info.dir, info.fScanNumber, info.sFormat);
if(info.verbose >= 4) info.fSTime = (double)cv::getTickCount();
fScan.convertScanToMat();
if(info.verbose >= 4) info.fSTime = ((double)cv::getTickCount() - info.fSTime)/cv::getTickFrequency();
if(info.verbose >= 2) fScan.getDescription();
panorama fPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam);
if(info.verbose >= 4) info.fPTime = (double)cv::getTickCount();
fPanorama.createPanorama(fScan.getMatScan());
if(info.verbose >= 4) info.fPTime = ((double)cv::getTickCount() - info.fPTime)/cv::getTickFrequency();
if(info.verbose >= 2) fPanorama.getDescription();
//write panorama to image
if(info.verbose >= 1){
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg";
imwrite(out, fPanorama.getReflectanceImage());
}
feature fFeature;
if(info.verbose >= 4) info.fFTime = (double)cv::getTickCount();
fFeature.featureDetection(fPanorama.getReflectanceImage(), info.fMethod);
if(info.verbose >= 4) info.fFTime = ((double)cv::getTickCount() - info.fFTime)/cv::getTickFrequency();
//write panorama with keypoints to image
if(info.verbose >= 1){
cv::drawKeypoints(fPanorama.getReflectanceImage(), fFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg";
imwrite(out, outImage);
outImage.release();
}
if(info.verbose >= 4) info.fDTime = (double)cv::getTickCount();
fFeature.featureDescription(fPanorama.getReflectanceImage(), info.dMethod);
if(info.verbose >= 4) info.fDTime = ((double)cv::getTickCount() - info.fDTime)/cv::getTickFrequency();
if(info.verbose >= 2) fFeature.getDescription();
scan_cv sScan (info.dir, info.sScanNumber, info.sFormat);
if(info.verbose >= 4) info.sSTime = (double)cv::getTickCount();
sScan.convertScanToMat();
if(info.verbose >= 4) info.sSTime = ((double)cv::getTickCount() - info.sSTime)/cv::getTickFrequency();
if(info.verbose >= 2) sScan.getDescription();
panorama sPanorama (info.iWidth, info.iHeight, info.pMethod, info.nImages, info.pParam);
if(info.verbose >= 4) info.sPTime = (double)cv::getTickCount();
sPanorama.createPanorama(sScan.getMatScan());
if(info.verbose >= 4) info.sPTime = ((double)cv::getTickCount() - info.sPTime)/cv::getTickFrequency();
if(info.verbose >= 2) sPanorama.getDescription();
//write panorama to image
if(info.verbose >= 1){
out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+".jpg";
imwrite(out, sPanorama.getReflectanceImage());
}
feature sFeature;
if(info.verbose >= 4) info.sFTime = (double)cv::getTickCount();
sFeature.featureDetection(sPanorama.getReflectanceImage(), info.fMethod);
if(info.verbose >= 4) info.sFTime = ((double)cv::getTickCount() - info.sFTime)/cv::getTickFrequency();
//write panorama with keypoints to image
if(info.verbose >= 1){
cv::drawKeypoints(sPanorama.getReflectanceImage(), sFeature.getFeatures(), outImage, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
out = info.outDir+info.local_time+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+".jpg";
imwrite(out, outImage);
outImage.release();
}
if(info.verbose >= 4) info.sDTime = (double)cv::getTickCount();
sFeature.featureDescription(sPanorama.getReflectanceImage(), info.dMethod);
if(info.verbose >= 4) info.sDTime = ((double)cv::getTickCount() - info.sDTime)/cv::getTickFrequency();
if(info.verbose >= 2) sFeature.getDescription();
feature_matcher matcher (info.mMethod, info.mParam);
if(info.verbose >= 4) info.mTime = (double)cv::getTickCount();
matcher.match(fFeature, sFeature);
if(info.verbose >= 4) info.mTime = ((double)cv::getTickCount() - info.mTime)/cv::getTickFrequency();
if(info.verbose >= 2) matcher.getDescription();
//write matcheed feature to image
if(info.verbose >= 1){
cv::drawMatches(fPanorama.getReflectanceImage(), fFeature.getFeatures(), sPanorama.getReflectanceImage(), sFeature.getFeatures(), matcher.getMatches(), outImage, cv::Scalar::all(-1), cv::Scalar::all(-1), vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+".jpg";
imwrite(out, outImage);
outImage.release();
}
registration reg (info.minDistance, info.minError, info.minInlier, info.rMethod);
if(info.verbose >= 4) info.rTime = (double)cv::getTickCount();
reg.findRegistration(fPanorama.getMap(), fFeature.getFeatures(), sPanorama.getMap(), sFeature.getFeatures(), matcher.getMatches());
if(info.verbose >= 4) info.rTime = ((double)cv::getTickCount() - info.rTime)/cv::getTickFrequency();
if(info.verbose >= 2) reg.getDescription();
//write .dat and .frames files
if(info.verbose >= 0){
double *bAlign = reg.getBestAlign();
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".dat";
ofstream dat(out.c_str());
dat << bAlign[0] << " " << bAlign[4] << " " << bAlign[8] << " " << bAlign[12] <<endl;
dat << bAlign[1] << " " << bAlign[5] << " " << bAlign[9] << " " << bAlign[13] <<endl;
dat << bAlign[2] << " " << bAlign[6] << " " << bAlign[10] << " " << bAlign[14] <<endl;
dat << bAlign[3] << " " << bAlign[7] << " " << bAlign[11] << " " << bAlign[15] <<endl;
dat.close();
out = info.outDir+info.local_time+"_scan"+to_string(info.fScanNumber, 3)+"_scan"+to_string(info.sScanNumber, 3)+"_"+projectionMethodToString(info.pMethod)+"_"+to_string(info.iWidth)+"x"+to_string(info.iHeight)+"_"+featureDetectorMethodToString(info.fMethod)+"_"+featureDescriptorMethodToString(info.dMethod)+"_"+matcherMethodToString(info.mMethod)+"_"+registrationMethodToString(info.rMethod)+".frames";
ofstream frames(out.c_str());
for (int n = 0 ; n < 2 ; n++)
{
for(int i = 0; i < 16; i++)
frames << bAlign[i] <<" ";
frames << "2" << endl;
}
frames.close();
}
if(info.verbose >= 3){
info.fSPoints = fScan.getNumberOfPoints();
info.sSPoints = sScan.getNumberOfPoints();
info.fFNum = fFeature.getNumberOfFeatures();
info.sFNum = sFeature.getNumberOfFeatures();
info.mNum = matcher.getNumberOfMatches();
info.filteredMNum = matcher.getNumberOfFilteredMatches();
info_yml(info, reg.getBestError(), reg.getBestErrorIndex(), reg.getBestAlign());
}
return 0;
}

196
.svn/pristine/d6/d6a5a93ffea0d6a4ad66c8e0a5793f94c07f21ac.svn-base
Unescape Escape View File

@ -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 <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/smart_ptr/shared_ptr.hpp>
#include <boost/interprocess/containers/string.hpp>
#include <boost/interprocess/containers/vector.hpp>
// 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<ip::offset_ptr<SharedScan>, SegmentManager> SharedScanAllocator;
typedef ip::vector<ip::offset_ptr<SharedScan>, SharedScanAllocator> SharedScanVector;
// allocator and type for a shared string
typedef ip::allocator<char, SegmentManager> CharAllocator;
typedef ip::basic_string<char, std::char_traits<char>, CharAllocator> SharedString;
// shared pointer for shared strings
typedef ip::managed_shared_ptr<SharedString, ip::managed_shared_memory>::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<void, SegmentManager> & 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<float> getXYZReducedShow();
//! Create a new set of reduced points for use in show
TripleArray<float> 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<double> m_pose;
ip::offset_ptr<CacheObject> 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

113
.svn/pristine/d9/d9f1dfdf01d6a88d739c6ab9df42a592592c6788.svn-base
Unescape Escape View File

@ -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 <string>
#include <map>
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<std::string, std::string> 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<typename T> friend struct CheckerFactory;
static std::map<std::string, Checker* (*)(const std::string&)>* 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<typename T>
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<T>::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

83
.svn/pristine/da/da1dfe175e8ce7ce4167fcbd0517ab1469769b59.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter&& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
ScanIO_rxp() : dec(0), imp(0) {}
private:
std::tr1::shared_ptr<basic_rconnection> 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<double>* xyz, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* 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<double>* xyz, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
this->filter = &filter;
this->xyz = xyz;
this->reflectance = reflectance;
this->amplitude = amplitude;
this->type = type;
this->deviation = deviation;
}
protected:
PointFilter* filter;
std::vector<double> *xyz;
std::vector<float> *reflectance;
std::vector<float> *amplitude;
std::vector<int> *type;
std::vector<float> *deviation;
int start;
int currentscan;
// overridden from pointcloud class
void on_echo_transformed(echo_type echo);
void on_frame_stop(const scanlib::frame_stop<iterator_type>& arg) {
scanlib::basic_packets::on_frame_stop(arg);
currentscan++;
}
};
#endif

825
.svn/pristine/da/da95a45e11ae5b4fbc6862200656e37691eca812.svn-base
Unescape Escape View File

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

27
.svn/pristine/e4/e466bb88a5d6f8a0b9f133b26f6a3e0635dca668.svn-base
Unescape Escape View File

@ -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<std::string> readDirectory(const char* dir_path, unsigned int start, unsigned int end);
virtual void readPose(const char* dir_path, const char* identifier, double* pose);
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

451
.svn/pristine/e4/e4d8522facb6f6415298f42185b70da086fb4a32.svn-base
Unescape Escape View File

@ -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, &current_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()...."<<endl;
// cout << "show_points: " << show_points << endl;
GLUI_Master.set_glutMouseFunc( CallBackMouseFunc );
GLUI_Master.set_glutKeyboardFunc( CallBackInterfaceFunc );
GLUI_Master.set_glutIdleFunc( CallBackIdleFunc );
GLUI_Master.set_glutSpecialFunc( CallBackSpecialFunc );
}
/**
* This function is called when a user starts to animate the generated path
*/
void pathAnimate(int dummy) {
//signal that the screen needs to be repainted for animation
haveToUpdate = 6;
path_iterator = 0;
}
/**
* This function clears the selected points
*/
void clearSelection(int dummy) {
for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--)
selected_points[iterator].clear();
}

99
.svn/pristine/e7/e709a93746f1fc8832f7457d39117f3c2b0f9f28.svn-base
Unescape Escape View File

@ -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<double>::min();
zMin = numeric_limits<double>::max();
nPoints = 0;
}
void scan_cv::convertScanToMat(){
bool scanserver = false;
Scan::openDirectory(scanserver, sDir, sFormat, sNumber, sNumber);
cout<<"loading "<<sDir<<" with scan number " <<sNumber<<"."<<endl;
Scan * source = * Scan::allScans.begin();
DataXYZ xyz = source->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_<cv::Vec4f> it;
it = scan.begin<cv::Vec4f>();
for(unsigned int i = 0; i < nPoints; i++){
float x, y, z, reflectance;
x = xyz[i][0];
y = xyz[i][1];
z = xyz[i][2];
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 "<<sDir<<", with scan number: " <<sNumber<<", with "<<nPoints<<" points, sFormat: "<<scanFormatToString(sFormat)<<"."<<endl;
cout<<endl;
}
}

614
.svn/pristine/e8/e8ccc08850bf9ad9953e47e8d7e1da1e5a51c1bf.svn-base
Unescape Escape View File

@ -1,614 +0,0 @@
/*
* scan_io_velodyne implementation
*
* Copyright (C) Mohammad Faisal, Dorit Borrmann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Andreas Nuechter. Jacobs University Bremen gGmbH
* @author Dorit Borrmann. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
* @author Mohammad Faisal. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
*/
#include "slam6d/point.h"
#include "veloslam/velodefs.h"
#include "scanio/scan_io_velodyne.h"
#include "slam6d/globals.icc"
#include <fstream>
#include<strstream>
#include <iostream>
using std::ifstream;
using std::ostrstream;
using std::cerr;
using std::endl;
using std::ends;
#include <algorithm>
using std::swap;
#include<cstdio>
#include<cstdlib>
#include<cmath>
#include<cstring>
#include <errno.h>
using namespace std;
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
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"<<endl;
string data[6];
getline (myfile,line);
string theDelimiter = ",";
int j=0;
do
{
getline (myfile,line);
int i = 0;
size_t start = 0, end = 0;
while ( end != string::npos && i < 6)
{
end = line.find( theDelimiter, start);
data[i] = line.substr(start, (end == string::npos)?string::npos:end - start);
start = ((end>(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."<<endl;
backup();
}
return 0;
}
int read_one_packet (
FILE *fp,
PointFilter& filter,
std::vector<double>* xyz,
std::vector<unsigned char>* rgb,
std::vector<float>* reflectance,
std::vector<float>* amplitude,
std::vector<int>* type,
std::vector<float>* deviation)
{
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<BLOCK_SIZE)
return -1;
ps = ( unsigned short * ) buf;
p = buf;
physicalNO = 0;
for ( i = 0; i < 12; i++ )
{
//Each frame start with 0xEEFF || 0xDDFF
if ( *ps == 0xEEFF )
Head = 0;
else if ( *ps == 0xDDFF )
Head = 32;
pshort = ( unsigned short * ) ( p + 2 );
rot = ( ( unsigned short ) ( *pshort ) / 100.0 );
circle_col = c * 6 + i / 2;
rotational = ( ( float ) ( *pshort ) ) / 100.0;
for ( j = 0; j < 32; j++ )
{
physicalNO = j + Head;
logicalNO = velodyne_physical_to_logical ( physicalNO );
circle_col = c * 6 + i / 2;
circle_row = logicalNO;
pt = ( unsigned short * ) ( p + 4 + j * 3 );
distance = absf ( ( *pt ) * 0.002 );
if( distance < 120 && distance > 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<std::string> 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<std::string> 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<double>* xyz,
std::vector<unsigned char>* rgb,
std::vector<float>* reflectance,
std::vector<float>* amplitude,
std::vector<int>* type,
std::vector<float>* deviation)
{
unsigned int i;
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<<data_path <<endl;
cerr << "ERROR: Missing file " << data_path <<" "<<strerror(errno)<< endl;
exit(1);
return;
}
cout << "Processing Scan " << data_path;
cout.flush();
xyz->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<<endl;
fclose(scan_in);
// process next frames
}
/**
* 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_velodyne;
}
/**
* 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

53
.svn/pristine/e9/e997c5ebf8c9d5197819fb044063ad31d33d4580.svn-base
Unescape Escape View File

@ -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 AND NOT APPLE)
# boost::interprocess uses pthread, requiring librt
#set(CLIENT_LIBS ${CLIENT_LIBS} rt)
endif(UNIX AND NOT APPLE)
target_link_libraries(scanclient ${CLIENT_LIBS})
# SERVER EXECUTABLE
# build by source
set(SERVER_SRCS
scanserver.cc serverInterface.cc frame_io.cc serverScan.cc
cache/cacheManager.cc cache/cacheHandler.cc scanHandler.cc
temporaryHandler.cc cacheIO.cc
)
add_executable(scanserver ${SERVER_SRCS})
# add libraries
# boost::interprocess/filesystem
# scanclient basic functionality
# scanio for ScanHandler input
set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio)
if(UNIX)
# boost::interprocess uses pthread, requiring librt
#set(SERVER_LIBS ${SERVER_LIBS} rt)
endif(UNIX)
if(WIN32)
# 3rd party getopt library
set(SERVER_LIBS ${SERVER_LIBS} XGetopt)
endif(WIN32)
target_link_libraries(scanserver ${SERVER_LIBS})

Some files were not shown because too many files have changed in this diff Show More

Write Preview
Loading…
Cancel
Save