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

update to svn revision 715

Browse source
This commit is contained in:
josch 2012-10-05 08:55:14 +02:00
parent ebddd68faa
commit 2fa4039b88
107 changed files with 14794 additions and 292 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

171
.svn/pristine/03/03075b55ebfb6f87c17388bd580e470b377222b3.svn-base Normal file
View file

@ -0,0 +1,171 @@
/*
* scan_io_uosr implementation
*
* Copyright (C) Billy Okal
*
* Released under the GPL version 3.
*
*/
/**
* @file scan_io_uosr.cc
* @brief IO of a 3D scan in uos file format plus a
* reflectance/intensity/temperature value
* @author Billy Okal. Jacobs University Bremen, Germany.
*/
#include "scanio/scan_io_uosr.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".3d"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
std::list<std::string> ScanIO_uosr::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_uosr::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
// read 6 plain doubles
for(i = 0; i < 6; ++i) pose_file >> pose[i];
pose_file.close();
// convert angles from deg to rad
for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]);
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_uosr::supports(IODataType type)
{
return !!(type & ( DATA_REFLECTANCE | DATA_XYZ ));
}
void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* temperature, 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

322
.svn/pristine/08/088a2028a56b46e0edd257d15d96be51a35bd9a1.svn-base Normal file
View file

@ -0,0 +1,322 @@
#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,
TEMP = 6
};
virtual ~ColorMap() {};
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
d[0] = d[1] = d[2] = 1.0;
}
static ColorMap getColorMap(CM map);
/**
* hue is in [0,360], all others in [0,1]
*/
static void convert_hsv_to_rgb(float hue, float s, float v,
float &r, float &g, float &b);
};
class GreyMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
d[0] = (float)i/(float)buckets;
d[1] = (float)i/(float)buckets;
d[2] = (float)i/(float)buckets;
}
};
class HSVMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
float t = (float)i/(float)buckets;
convert_hsv_to_rgb(360.0*t, 1.0, 1.0, d[0], d[1], d[2]);
}
};
class SHSVMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
float t = (float)i/(float)buckets;
convert_hsv_to_rgb(240.0*(1.0-t), 1.0, 1.0, d[0], d[1], d[2]);
}
};
class JetMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
float t = (float)i/(float)buckets;
if (t <= 0.125) {
d[0] = d[1] = 0.0; d[2] = 0.5 + 0.5*(t/0.125);
} else if (t < 0.375) {
d[0] = 0.0; d[2] = 1.0; d[1] = (t-0.125)/0.25;
} else if (t < 0.625) {
d[1] = 1.0; d[0] = (t-0.375)/0.25;; d[2] = 1.0 - d[0];
} else if (t < 0.875) {
d[0] = 1.0; d[2] = 0.0; d[1] = 1.0 - (t-0.625)/0.25;
} else {
d[1] = d[2] = 0.0; d[0] = 1.0 - 0.5*((t - 0.875)/0.125);
}
}
};
class HotMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
float t = (float)i/(float)buckets;
if (t <= 1.0/3.0) {
d[1] = d[2] = 0.0; d[0] = t/(1.0/3.0);
} else if (t <= 2.0/3.0) {
d[0] = 1.0; d[2] = 0.0; d[1] = (t-(1.0/3.0))/(1.0/3.0);
} else {
d[0] = 1.0; d[1] = 1.0; d[2] = (t-(2.0/3.0))/(1.0/3.0);
}
}
};
class TempMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
float t = 1.0 - (float)i/(float)buckets;
if(t <= 1.0/5.0) {
d[1] = d[2] = 0.0;
d[0] = t/(1.0/5.0);
} else if(t <=2.0/5.0) {
d[0] = 1.0;
d[1] = (t-(1.0/5.0))/(1.0/5.0);
d[2] = 0.0;
} else if(t <=3.0/5.0) {
d[0] = 1.0 - ((t-(2.0/5.0))/(1.0/5.0));
d[1] = 1.0;
d[2] = (t-(2.0/5.0))/(1.0/5.0);
} else if(t <=4.0/5.0) {
d[0] = 0.0;
d[1] = 1.0 - ((t-(3.0/5.0))/(1.0/5.0));
d[2] = 1.0;
} else {
d[0] = 0.0;
d[1] = 0.0;
d[2] = 1.0 - ((t-(4.0/5.0))/(1.3/5.0));
}
}
};
class DiffMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets);
private:
static const float cmap[7][3];
};
class ColorManager {
public:
ColorManager(unsigned int _buckets, unsigned int pointdim, float *_mins, float *_maxs, const float *_color = 0) : buckets(_buckets) {
if (_color) {
color[0] = _color[0];
color[1] = _color[1];
color[2] = _color[2];
} else {
color[0] = 1;
color[1] = 1;
color[2] = 1;
}
colormap = new float*[buckets + 1]; // allow a color more for values equal to max
for (unsigned int i = 0; i < buckets; i++) {
colormap[i] = new float[3];
}
colormap[buckets] = new float[3];
mins = new float[pointdim];
maxs = new float[pointdim];
for (unsigned int i = 0; i < pointdim; i++) {
mins[i] = _mins[i];
maxs[i] = _maxs[i];
}
setCurrentDim(0);
}
virtual ~ColorManager() {
for (unsigned int i = 0; i < buckets; i++) {
delete[] colormap[i];
}
delete[] colormap[buckets];
delete[] colormap;
delete[] mins;
delete[] maxs;
}
virtual void load() {
glColor3f(color[0], color[1], color[2] );
glEnable (GL_TEXTURE_1D);
glBindTexture (GL_TEXTURE_1D, 0);
}
virtual void unload() {
glDisable (GL_TEXTURE_1D);
}
virtual void setColor(float *val) {
glTexCoord1f( (float)((val[currentdim]-min)/extent) );
}
virtual void setColor(double *val) {
glTexCoord1f( (float)((val[currentdim]-min)/extent) );
}
virtual void setColor(short int *val) {
glTexCoord1f( (float)((val[currentdim]-min)/extent) );
}
virtual void setColor(signed char *val) {
glTexCoord1f( (float)((val[currentdim]-min)/extent) );
}
virtual void setColorMap(ColorMap &cm) {
for (unsigned int i = 0; i < buckets; i++) {
cm.calcColor(colormap[i], i, buckets);
}
cm.calcColor(colormap[buckets], buckets-1, buckets);
convertToTexture1D();
}
void setCurrentDim(unsigned int cdim) {
currentdim = cdim;
makeValid();
}
void setMinMax(float _min, float _max) {
if (_min < _max) {
min = _min;
max = _max;
}
extent = max - min;
}
protected:
void convertToTexture1D() {
unsigned char *imageData = new unsigned char[(buckets+1) * 3];
for (unsigned int i = 0; i < buckets; i++) {
imageData[3*i+0] = colormap[i][0]*255;
imageData[3*i+1] = colormap[i][1]*255;
imageData[3*i+2] = colormap[i][2]*255;
}
imageData[3*buckets+0] = colormap[buckets][0]*255;
imageData[3*buckets+1] = colormap[buckets][1]*255;
imageData[3*buckets+2] = colormap[buckets][2]*255;
glBindTexture (GL_TEXTURE_1D, 0);
glPixelStorei (GL_UNPACK_ALIGNMENT, 1);
glTexParameteri (GL_TEXTURE_1D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri (GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri (GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexEnvf (GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
glTexImage1D (GL_TEXTURE_1D, 0, GL_RGB, buckets+1, 0, GL_RGB, GL_UNSIGNED_BYTE, imageData);
delete[] imageData;
}
void makeValid() {
min = mins[currentdim];
max = maxs[currentdim];
extent = max - min;
}
unsigned int buckets;
unsigned int currentdim;
/** stores minima and maxima for each point dimension */
float *mins;
float *maxs;
/** maps color to value */
float **colormap;
float min;
float max;
float extent;
float color[3];
};
class CColorManager : public ColorManager {
public:
CColorManager(unsigned int buckets, unsigned int pointdim, float *mins, float *maxs, unsigned int _colordim) : ColorManager(buckets, pointdim, mins, maxs) {
colordim = _colordim;
}
virtual void load() {
glGetBooleanv(GL_COLOR_LOGIC_OP, &color_state);
glDisable(GL_COLOR_LOGIC_OP); // this disables inversion of color, but also messes with fog behaviour
glColor3f(color[0], color[1], color[2] );
glEnable (GL_TEXTURE_1D);
glBindTexture (GL_TEXTURE_1D, 0);
}
virtual void unload() {
glDisable (GL_TEXTURE_1D);
if (color_state) {
glEnable(GL_COLOR_LOGIC_OP);
}
}
void setColor(double *val) {
GLubyte color[3];
memcpy(color, &val[colordim], 3);
glColor3ubv(color);
}
void setColor(float *val) {
GLubyte color[3];
memcpy(color, &val[colordim], 3);
glColor3ubv(color);
}
void setColor(short *val) {
GLubyte color[3];
memcpy(color, &val[colordim], 3);
glColor3ubv(color);
}
virtual void setColor(signed char *val) {
GLubyte color[3];
memcpy(color, &val[colordim], 3);
glColor3ubv(color);
}
private:
unsigned int colordim;
GLboolean color_state;
};
#endif

101
.svn/pristine/0d/0dfc9d538db93f19080667f0cd6fbde1e213574f.svn-base Normal file
View file

@ -0,0 +1,101 @@
/**
* @file fbr_global.h
* @brief Globally used headers, functions, structures
* @author HamidReza Houshiar. Jacobs University Bremen gGmbH, Germany.
* @date 2012/05/9 14:00
*/
#ifndef FBR_GLOBAL_H_
#define FBR_GLOBAL_H_
#include <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_ */

27
.svn/pristine/15/15833af3aa316bca80246ea5be32252af93605f2.svn-base Normal file
View file

@ -0,0 +1,27 @@
/**
* @file
* @brief IO of a 3D scan in riegl_txt file format with added color information
* @author Thomas Escher
*/
#ifndef __SCAN_IO_RIEGL_RGB_H__
#define __SCAN_IO_RIEGL_RGB_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for Riegl scans
*
* The compiled class is available as shared object file
*/
class ScanIO_riegl_rgb : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

466
.svn/pristine/16/16e86bc6017730cb99fa4313a99581e6712cb05d.svn-base Normal file
View file

@ -0,0 +1,466 @@
/*
* scan_red implementation
*
* Copyright (C) Dorit Borrmann, Razvan-George Mihalyi, Remus Dumitru
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main program for reducing 3D scans.
*
* Program to reduce scans for use with slam6d
* Usage: bin/scan_red -r <NR> 'dir',
* Use -r for octree based reduction (voxel size=<NR>)
* and 'dir' the directory of a set of scans
* Reduced scans will be written to 'dir/reduced'
*
* @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany.
*/
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#define WANT_STREAM ///< define the WANT stream :)
#include <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <fstream>
using std::ofstream;
#include <errno.h>
#include "slam6d/metaScan.h"
#include "slam6d/io_utils.h"
#include "slam6d/scan.h"
#include "slam6d/fbr/fbr_global.h"
#include "slam6d/fbr/panorama.h"
#include "slam6d/fbr/scan_cv.h"
#include "scanserver/clientInterface.h"
#include "slam6d/globals.icc"
#ifdef _OPENMP
#include <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
//Vertical angle of view of scanner
#define MAX_ANGLE 60.0
#define MIN_ANGLE -40.0
#define IMAGE_HEIGHT 1000
#define IMAGE_WIDTH 3600
using namespace fbr;
projection_method strToPMethod(string method){
if(strcasecmp(method.c_str(), "EQUIRECTANGULAR") == 0) return EQUIRECTANGULAR;
else if(strcasecmp(method.c_str(), "CYLINDRICAL") == 0) return CYLINDRICAL;
else if(strcasecmp(method.c_str(), "MERCATOR") == 0) return MERCATOR;
else if(strcasecmp(method.c_str(), "CONIC") == 0) return CONIC;
else throw std::runtime_error(std::string("projection method ") + method + std::string(" is unknown"));
}
/**
* Explains the usage of this program's command line parameters
*/
void usage(char* prog)
{
#ifndef _MSC_VER
const string bold("\033[1m");
const string normal("\033[m");
#else
const string bold("");
const string normal("");
#endif
cout << endl
<< bold << "USAGE " << normal << endl
<< " " << prog << " [options] -r <NR> directory" << endl << endl;
cout << bold << "OPTIONS" << normal << endl
<< bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
<< " start at scan NR (i.e., neglects the first NR scans)" << endl
<< " [ATTENTION: counting naturally starts with 0]" << endl
<< endl
<< bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
<< " end after scan NR" << endl
<< endl
<< bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
<< " using shared library F for input" << endl
<< " (choose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl
<< endl
<< bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
<< " neglegt all data points with a distance larger than NR 'units'" << endl
<< endl
<< bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
<< " neglegt all data points with a distance smaller than NR 'units'" << endl
<< endl
<< bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
<< " if NR >= 0, turns on octree based point reduction (voxel size=<NR>)" << endl
<< " if NR < 0, turns on rescaling based reduction" << endl
<< endl
<< bold << " -I" << normal << " NR," << bold << "--rangeimage=" << normal << "NR" << endl
<< " use rescaling of the range image as reduction method" << endl
<< " if NR = 1 recovers ranges from range image" << endl
<< " if NR = 2 interpolates 3D points in the image map" << endl
<< " if NR is omitted, then NR=1 is selected" << endl
<< endl
<< bold << " -p" << normal << " MET," << bold << "--projection=" << normal << "MET" << endl
<< " create range image using the MET projection method" << endl
<< " (choose MET from [EQUIRECTANGULAR|CYLINDRICAL|MERCATOR|CONIC])" << endl
<< bold << " -S, --scanserver" << normal << endl
<< " Use the scanserver as an input method and handling of scan data" << endl
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " -m 500 -r 5 dat" << endl
<< " " << prog << " --max=5000 -r 10.2 dat" << endl
<< " " << prog << " -s 2 -e 10 -r dat" << endl
<< " " << prog << " -s 0 -e 1 -r 10 -I=1 dat " << endl << endl;
exit(1);
}
/** A function that parses the command-line arguments and sets the respective flags.
* @param argc the number of arguments
* @param argv the arguments
* @param dir the directory
* @param red using point reduction?
* @param rand use randomized point reduction?
* @param start starting at scan number 'start'
* @param end stopping at scan number 'end'
* @param maxDist - maximal distance of points being loaded
* @param minDist - minimal distance of points being loaded
* @param projection - projection method for building range image
* @param quiet switches on/off the quiet mode
* @param veryQuiet switches on/off the 'very quiet' mode
* @return 0, if the parsing was successful. 1 otherwise
*/
int parseArgs(int argc, char **argv, string &dir, double &red,
int &start, int &end, int &maxDist, int &minDist,
string &projection, int &octree, IOType &type,
int &rangeimage, bool &scanserver)
{
bool reduced = false;
int c;
// from unistd.h:
extern char *optarg;
extern int optind;
WriteOnce<IOType> w_type(type);
WriteOnce<int> w_start(start), w_end(end);
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "format", required_argument, 0, 'f' },
{ "max", required_argument, 0, 'm' },
{ "min", required_argument, 0, 'M' },
{ "start", required_argument, 0, 's' },
{ "end", required_argument, 0, 'e' },
{ "reduce", required_argument, 0, 'r' },
{ "octree", optional_argument, 0, 'O' },
{ "rangeimage", optional_argument, 0, 'I' },
{ "projection", required_argument, 0, 'p' },
{ "scanserver", no_argument, 0, 'S' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:O:p:", longopts, NULL)) != -1)
switch (c)
{
case 'r':
red = atof(optarg);
reduced = true;
break;
case 's':
w_start = atoi(optarg);
if (w_start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'e':
w_end = atoi(optarg);
if (w_end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
if (w_end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
break;
case 'f':
try {
w_type = formatname_to_io_type(optarg);
} catch (...) { // runtime_error
cerr << "Format " << optarg << " unknown." << endl;
abort();
}
break;
case 'm':
maxDist = atoi(optarg);
break;
case 'O':
if (optarg) {
octree = atoi(optarg);
} else {
octree = 1;
}
break;
case 'M':
minDist = atoi(optarg);
break;
case 'I':
if (optarg) {
rangeimage = atoi(optarg);
} else {
rangeimage = 1;
}
break;
case 'p':
projection = optarg;
break;
case 'S':
scanserver = true;
break;
case '?':
usage(argv[0]);
return 1;
default:
abort ();
}
if(!reduced) {
cerr << "\n*** Reduction method missed ***" << endl;
usage(argv[0]);
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
usage(argv[0]);
}
dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
parseFormatFile(dir, w_type, w_start, w_end);
return 0;
}
/**
* Main program for reducing scans.
* Usage: bin/scan_red -r <NR> 'dir',
* Use -r for octree based reduction (voxel size=<NR>)
* and 'dir' the directory of a set of scans
* Reduced scans will be written to 'dir/reduced'
*
*/
int main(int argc, char **argv)
{
cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl;
if (argc <= 1) {
usage(argv[0]);
}
// parsing the command line parameters
// init, default values if not specified
string dir;
double red = -1.0;
int start = 0, end = -1;
int maxDist = -1;
int minDist = -1;
string projection = "EQUIRECTANGULAR";
int octree = 0;
IOType type = RIEGL_TXT;
int rangeimage = 0;
bool scanserver = false;
parseArgs(argc, argv, dir, red, start, end, maxDist, minDist, projection,
octree, type, rangeimage, scanserver);
if (scanserver) {
try {
ClientInterface::create();
} catch(std::runtime_error& e) {
cerr << "ClientInterface could not be created: " << e.what() << endl;
cerr << "Start the scanserver first." << endl;
exit(-1);
}
}
// Get Scans
string reddir = dir + "reduced";
#ifdef _MSC_VER
int success = mkdir(reddir.c_str());
#else
int success = mkdir(reddir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing scans to " << reddir << endl;
} else if(errno == EEXIST) {
cout << "Directory " << reddir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << reddir << " failed" << endl;
exit(1);
}
Scan::openDirectory(scanserver, dir, type, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
string scanFileName;
string poseFileName;
/// Use the OCTREE based reduction
if (rangeimage == 0) {
cout << endl << "Reducing point cloud using octrees" << endl;
int scan_number = start;
for(std::vector<Scan*>::iterator it = Scan::allScans.begin();
it != Scan::allScans.end();
++it, ++scan_number) {
Scan* scan = *it;
const double* rPos = scan->get_rPos();
const double* rPosTheta = scan->get_rPosTheta();
scan->setRangeFilter(maxDist, minDist);
scan->setReductionParameter(red, octree);
// get reduced points
DataXYZ xyz_r(scan->get("xyz reduced"));
unsigned int nPoints = xyz_r.size();
const char* id = scan->getIdentifier();
cout << "Writing Scan No. " << id;
cout << " with " << xyz_r.size() << " points" << endl;
scanFileName = reddir + "/scan" + id + ".3d";
poseFileName = reddir + "/scan" + id + ".pose";
ofstream redptsout(scanFileName.c_str());
for(unsigned int j = 0; j < nPoints; j++) {
redptsout << xyz_r[j][0] << " "
<< xyz_r[j][1] << " "
<< xyz_r[j][2] << endl;
}
redptsout.close();
redptsout.clear();
ofstream posout(poseFileName.c_str());
posout << rPos[0] << " "
<< rPos[1] << " "
<< rPos[2] << endl
<< deg(rPosTheta[0]) << " "
<< deg(rPosTheta[1]) << " "
<< deg(rPosTheta[2]) << endl;
posout.close();
posout.clear();
if (scanserver) {
scan->clear("xyz reduced");
}
}
} else { /// use the RESIZE based reduction
cout << endl << "Reducing point cloud by rescaling the range image" << endl;
Scan::openDirectory(false, dir, type, start, end);
if (Scan::allScans.size() <= 0) {
cerr << "No scans found!" << endl;
exit(-1);
}
for (int scan_number = start; scan_number <= end; scan_number++) {
Scan* scan = Scan::allScans[scan_number];
scan->setRangeFilter(maxDist, minDist);
const double* rPos = scan->get_rPos();
const double* rPosTheta = scan->get_rPosTheta();
scanFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".3d";
poseFileName = dir + "reduced/scan" + to_string(scan_number, 3) + ".pose";
// Create a panorama. The iMap inside does all the tricks for us.
scan_cv sScan(dir, scan_number, type);
sScan.convertScanToMat();
/// Project point cloud using the selected projection method
panorama image(IMAGE_WIDTH, IMAGE_HEIGHT, strToPMethod(projection));
image.createPanorama(sScan.getMatScan());
image.getDescription();
/// Resize the range image, specify desired interpolation method
double scale = 1.0/red;
cv::Mat range_image_resized; // reflectance_image_resized;
string ofilename;
stringstream ss;
ss << setw(3) << setfill('0') << (scan_number);
ofilename = reddir + "/scan" + ss.str() + ".3d";
if (rangeimage == 1) {
resize(image.getRangeImage(), range_image_resized, cv::Size(),
scale, scale, cv::INTER_NEAREST);
// Recover point cloud from image and write scan to file
stringstream ss;
ss << setw(3) << setfill('0') << (scan_number);
image.recoverPointCloud(range_image_resized, ofilename);
} else {
resize(image.getMap(), range_image_resized, cv::Size(),
scale, scale, cv::INTER_NEAREST);
ofstream redptsout(ofilename.c_str());
// Convert back to 3D.
for(int i = 0; i < range_image_resized.rows; i++) {
for(int j = 0; j < range_image_resized.cols; j++) {
cv::Vec3f vec = range_image_resized.at<cv::Vec3f>(i, j);
double x = vec[0];
double y = vec[1];
double z = vec[2];
redptsout << x << " " << y << " " << z << endl;
}
}
}
ofstream posout(poseFileName.c_str());
posout << rPos[0] << " "
<< rPos[1] << " "
<< rPos[2] << endl
<< deg(rPosTheta[0]) << " "
<< deg(rPosTheta[1]) << " "
<< deg(rPosTheta[2]) << endl;
posout.clear();
posout.close();
}
}
cout << endl << endl;
cout << "Normal program end." << endl << endl;
if (scanserver) {
Scan::closeDirectory();
ClientInterface::destroy();
}
}

27
.svn/pristine/27/273b9c7a4add695164edbbd786206862ef4ae722.svn-base Normal file
View file

@ -0,0 +1,27 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_UOS_H__
#define __SCAN_IO_UOS_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for UOS scans
*
* The compiled class is available as shared object file
*/
class ScanIO_uos : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

265
.svn/pristine/2c/2cf4e891f6f5a378f076d29d0b9feb822ab8c7d8.svn-base Normal file
View file

@ -0,0 +1,265 @@
/*
* 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_temperature;
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> temperature(m_scan, m_prefetch_temperature);
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_TEMPERATURE) vec = &temperature;
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_TEMPERATURE) temperature.create();
if(prefetch & DATA_AMPLITUDE) amplitude.create();
if(prefetch & DATA_TYPE) type.create();
if(prefetch & DATA_DEVIATION) deviation.create();
// request data from the ScanIO
try {
PointFilter filter(m_scan->getPointFilter());
sio->readScan(m_scan->getDirPath(), m_scan->getIdentifier(),
filter,
xyz.get(), rgb.get(), reflectance.get(), temperature.get(), amplitude.get(), type.get(), deviation.get());
} catch(std::runtime_error& e) {
// INFO
// cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanIO runtime_error: " << e.what() << endl;
// rethrow
throw e;
} catch(std::bad_alloc& e) {
// INFO
// cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanIO bad_alloc: " << e.what() << endl;
// rethrow
throw e;
}
}
// after successful loading, allocate enough cache space
unsigned int size = vec->size();
void* data_ptr;
try {
data_ptr = m_manager->allocateCacheObject(m_object, size);
} catch(runtime_error& e) {
// INFO
// cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] CacheManager error: " << e.what() << endl;
// rethrow
throw e;
}
// write data into the cache object and clean up the vector
vec->write(data_ptr);
// save all vectors that still hold their data for prefetching
xyz.save();
rgb.save();
reflectance.save();
temperature.save();
amplitude.save();
type.save();
deviation.save();
// INFO
//cout << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanHandler::load successful" << endl << endl;
#ifdef WITH_METRICS
ServerMetric::scan_loading.end(t);
#endif //WITH_METRICS
return true;
}
void ScanHandler::save(unsigned char* data, unsigned int size)
{
// INFO
//cout << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanHandler::save" << endl;
// if global binary scan caching is enabled, save to file for later faster reloading
if(binary_caching) {
// duplicate writes of static data are handled in TemporaryHandler already
TemporaryHandler::save(data, size);
}
}
void ScanHandler::setBinaryCaching()
{
binary_caching = true;
}

454
.svn/pristine/2d/2d589cc71a969f101c026cc2c3bca3ce32594578.svn-base Normal file
View file

@ -0,0 +1,454 @@
/*
* show_menu implementation
*
* Copyright (C) Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Functions for the menu panels of the viewer software
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "show/colormanager.h"
// GUI variables
GLUI *glui1, ///< pointer to the glui window(s)
*glui2; ///< pointer to the glui window(s)
/** GLUI spinner for the fog */
GLUI_Spinner *fog_spinner;
/** GLUI spinner for the point size */
GLUI_Spinner *ps_spinner;
/** GLUI spinner for the current angle */
GLUI_Spinner *cangle_spinner;
/** GLUI spinner for the current angle */
GLUI_Spinner *pzoom_spinner;
/** GLUI spinner for the factor for the image size */
GLUI_Spinner *image_spinner;
/** GLUI_Spinner for the depth to select groups of points */
GLUI_Spinner *depth_spinner;
GLUI_Spinner *brushsize_spinner;
GLUI_Spinner *frame_spinner;
GLUI_Spinner *fps_spinner;
GLUI_Spinner *farplane_spinner;
GLUI_Spinner *nearplane_spinner;
GLUI_Spinner *lod_spinner;
int window_id_menu1, ///< menue window ids
window_id_menu2; ///< menue window ids
/** User IDs for callbacks */
#define BOX_ID 201
/** User IDs for callbacks */
#define ENABLE_ID 300
/** User IDs for callbacks */
#define DISABLE_ID 301
/** User IDs for callbacks */
#define SHOW_ID 302
/** User IDs for callbacks */
#define HIDE_ID 303
/** Pointer to the panels */
GLUI_Panel *ground_panel;
/** Pointer to the panels */
GLUI_Panel *points_panel;
/** Pointer to the panels */
GLUI_Panel *wireframe_panel;
/** Pointer to the panels */
GLUI_Panel *path_panel;
/** Pointer to the panels */
GLUI_Panel *pose_panel;
/** Pointer to the panels */
GLUI_Panel *selection_panel;
/** Pointer to the panels */
GLUI_Panel *color_panel;
/** Pointer to the panels */
GLUI_Panel *camera_panel;
/** Pointer to the panels */
GLUI_Panel *nav_panel;
/** Pointer to the panels */
GLUI_Panel *mode_panel;
/** Pointer to the panels */
GLUI_Panel *settings_panel;
/** Pointer to the panels */
GLUI_Panel *advanced_panel;
/** Pointer to the button */
GLUI_Button *button1;
/** Pointer to the edit text box*/
GLUI_EditText *path_filename_edit;
/** Pointer to the edit text box*/
GLUI_EditText *pose_filename_edit;
/** Pointer to the edit text box*/
GLUI_EditText *selection_filename_edit;
/** Pointer to the rotation button */
GLUI_Rotation *rotButton;
/** used for GLUI menue */
float obj_pos_button[3];
/** used for GLUI menue */
GLfloat view_rotate_button[16];
/** used for GLUI menue */
GLfloat obj_pos_button_old[3];
/** used for GLUI menue */
GLfloat X_button;
/** used for GLUI menue */
GLfloat Y_button;
/** used for GLUI menue */
GLfloat Z_button;
/** GLUI spinner for choosing the camera */
GLUI_Spinner *cam_spinner;
/** GLUI spinner for choosing the animation delay */
GLUI_Spinner *anim_spinner;
/** Panel for the camera controls **/
GLUI_Panel *cam_panel;
/** ListBox for choosing which value to map to a color */
GLUI_Listbox *value_listbox;
/** ListBox for choosing which color map to use */
GLUI_Listbox *colormap_listbox;
GLUI_Spinner *mincol_spinner;
GLUI_Spinner *maxcol_spinner;
/** Checkboxes for changing point display mode **/
GLUI_Checkbox *always_box;
GLUI_Checkbox *never_box;
/** Checkbox for changing interpolation mode **/
GLUI_Checkbox *interpol_box;
/**
* Generate the menu for the application.
* It consists of control and selection menus.
*/
void newMenu()
{
/*** Create the bottom subwindow ***/
glui2 = GLUI_Master.create_glui("3D_Viewer - Controls");
window_id_menu2 = glui2->get_glut_window_id();
glutSetWindow(window_id_menu2);
glutPositionWindow(START_X, START_Y + START_HEIGHT + 65);
glutSetWindow(window_id);
glui2->set_main_gfx_window( window_id );
settings_panel = glui2->add_panel("Settings: ");
cangle_spinner = glui2->add_spinner_to_panel(settings_panel, "Field of View : ", GLUI_SPINNER_FLOAT, &cangle);
cangle_spinner->set_float_limits( 1.0, 180.0 );
cangle_spinner->set_speed( 20.0 );
cangle_spinner->set_float_val(60.0);
cangle_spinner->set_alignment( GLUI_ALIGN_RIGHT );
pzoom_spinner = glui2->add_spinner_to_panel(settings_panel, "Parallel Zoom :", GLUI_SPINNER_FLOAT, &pzoom);
pzoom_spinner->set_float_limits( 10.0, 50000.0 );
pzoom_spinner->set_speed( 5.0 );
pzoom_spinner->set_float_val(2000.0);
pzoom_spinner->set_alignment( GLUI_ALIGN_RIGHT );
pzoom_spinner->disable();
glui2->add_column( true );
mode_panel = glui2->add_panel("Mode: ");
/****** Top view *****/
glui2->add_button_to_panel(mode_panel, "Top view", 0, callTopView )->set_alignment( GLUI_ALIGN_CENTER );
/****** Reset button *****/
glui2->add_button_to_panel(mode_panel, "Reset position", 0, resetView )->set_alignment( GLUI_ALIGN_CENTER );
glui2->add_column( true );
/****** Add Camera View *****/
camera_panel = glui2->add_panel("Camera: ");
cam_spinner = glui2->add_spinner_to_panel(camera_panel, "Choose Camera", GLUI_SPINNER_INT, &cam_choice);
cam_spinner->set_int_limits( 0, 0 );
cam_spinner->set_speed( 1 );
cam_spinner->set_alignment( GLUI_ALIGN_LEFT );
glui2->add_button_to_panel(camera_panel, "Add Camera", 1, callAddCamera )->set_alignment( GLUI_ALIGN_CENTER );
glui2->add_button_to_panel(camera_panel, "Delete Camera", 0, callDeleteCamera )->set_alignment( GLUI_ALIGN_CENTER );
/******* Other navigation controls**********/
glui2->add_column(true);
nav_panel = glui2->add_panel("Navigation: ");
rotButton = glui2->add_rotation_to_panel(nav_panel, "Rotation", view_rotate_button, -1, update_view_rotate);
glui2->add_column_to_panel(nav_panel, true );
glui2->add_translation_to_panel(nav_panel, "Move XY", GLUI_TRANSLATION_XY,
obj_pos_button, -1, update_view_translation);
glui2->add_column_to_panel(nav_panel, false );
glui2->add_translation_to_panel(nav_panel, "Move X", GLUI_TRANSLATION_X,
&obj_pos_button[0], -1, update_view_translation);
glui2->add_column_to_panel(nav_panel, false );
glui2->add_translation_to_panel(nav_panel, "Move Y", GLUI_TRANSLATION_Y,
&obj_pos_button[1], -1, update_view_translation);
glui2->add_column_to_panel(nav_panel, false );
glui2->add_translation_to_panel(nav_panel, "Move Z", GLUI_TRANSLATION_Z,
&obj_pos_button[2], -1, update_view_translation);
glui2->add_column_to_panel(nav_panel, false);
glui2->add_checkbox_to_panel(nav_panel, "MouseNav", &cameraNavMouseMode );
static int dummy4;
always_box = glui2->add_checkbox_to_panel(nav_panel, "Always all Points", &dummy4, 0, &changePointMode);
glui2->set_glutMouseFunc(CallBackMouseFuncMoving);
static int dummy5 = 1;
never_box = glui2->add_checkbox_to_panel(nav_panel, "Always reduce Points", &dummy5, 1, &changePointMode );
/*** Create the right subwindow ***/
glui1 = GLUI_Master.create_glui("3D_Viewer - Selection");
window_id_menu1 = glui1->get_glut_window_id();
glutSetWindow(window_id_menu1);
glutPositionWindow(START_X + START_WIDTH + 50, START_Y + 30);
glutSetWindow(window_id);
glui1->set_main_gfx_window( window_id );
glui1->add_checkbox( "Draw Points", &show_points );
glui1->add_checkbox( "Draw Camera", &show_cameras);
glui1->add_checkbox( "Draw Path", &show_path);
ps_spinner = glui1->add_spinner( "Point Size:", GLUI_SPINNER_FLOAT, &pointsize);
ps_spinner->set_float_limits( 0.0000001, 10.0 );
ps_spinner->set_speed( 25.0 );
ps_spinner->set_float_val(pointsize);
ps_spinner->set_alignment( GLUI_ALIGN_LEFT );
/**** Fog Panel *****/
GLUI_Panel *fogt_panel = glui1->add_rollout("Fog :", false );
fogt_panel ->set_alignment( GLUI_ALIGN_LEFT );
GLUI_RadioGroup *fogt = glui1-> add_radiogroup_to_panel( fogt_panel, &show_fog );
glui1->add_radiobutton_to_group( fogt, "No Fog" );
glui1->add_radiobutton_to_group( fogt, "Fog Exp" );
glui1->add_radiobutton_to_group( fogt, "Fog Exp2" );
glui1->add_radiobutton_to_group( fogt, "Fog Linear" );
glui1->add_radiobutton_to_group( fogt, "inverted Fog Exp" );
glui1->add_radiobutton_to_group( fogt, "inverted Fog Exp2" );
glui1->add_radiobutton_to_group( fogt, "inverted Fog Linear" );
fog_spinner = glui1->add_spinner( "Fog Density:", GLUI_SPINNER_FLOAT, &fogDensity);
fog_spinner->set_float_limits( 0.0, 1.0 );
fog_spinner->set_speed( 0.5 );
fog_spinner->set_float_val(0.001);
fog_spinner->set_alignment( GLUI_ALIGN_LEFT );
/****** Color Controls *****/
color_panel = glui1->add_rollout("Color :", false );
color_panel ->set_alignment( GLUI_ALIGN_LEFT );
GLUI_Panel *color_ro = glui1->add_rollout_to_panel(color_panel, "Color values:", false);
color_ro->set_alignment(GLUI_ALIGN_LEFT);
GLUI_RadioGroup *color_rog = glui1->add_radiogroup_to_panel( color_ro, &listboxColorVal, 0, &mapColorToValue );
glui1->add_radiobutton_to_group( color_rog, "height");
GLUI_RadioButton *rbrefl = glui1->add_radiobutton_to_group( color_rog, "reflectance");
GLUI_RadioButton *rbtemp = glui1->add_radiobutton_to_group( color_rog, "temperature");
GLUI_RadioButton *rbampl = glui1->add_radiobutton_to_group( color_rog, "amplitude");
GLUI_RadioButton *rbdevi = glui1->add_radiobutton_to_group( color_rog, "deviation");
GLUI_RadioButton *rbtype = glui1->add_radiobutton_to_group( color_rog, "type");
//if (!(types & PointType::USE_REFLECTANCE)) rbrefl->disable();
//if (!(types & PointType::USE_AMPLITUDE)) rbampl->disable();
//if (!(types & PointType::USE_DEVIATION)) rbdevi->disable();
//if (!(types & PointType::USE_TYPE)) rbtype->disable();
if (!(pointtype.hasReflectance())) rbrefl->disable();
if (!(pointtype.hasTemperature())) rbtemp->disable();
if (!(pointtype.hasAmplitude())) rbampl->disable();
if (!(pointtype.hasDeviation())) rbdevi->disable();
if (!(pointtype.hasType())) rbtype->disable();
GLUI_Panel *colorm_ro = glui1->add_rollout_to_panel(color_panel, "Colormap:", false);
colorm_ro->set_alignment(GLUI_ALIGN_LEFT);
GLUI_RadioGroup *colorm_rog = glui1->add_radiogroup_to_panel(colorm_ro, &listboxColorMapVal, 0, &changeColorMap);
glui1->add_radiobutton_to_group(colorm_rog, "Solid");
glui1->add_radiobutton_to_group(colorm_rog, "Grey");
glui1->add_radiobutton_to_group(colorm_rog, "HSV");
glui1->add_radiobutton_to_group(colorm_rog, "Jet");
glui1->add_radiobutton_to_group(colorm_rog, "Hot");
glui1->add_radiobutton_to_group(colorm_rog, "Rand");
glui1->add_radiobutton_to_group(colorm_rog, "SHSV");
glui1->add_radiobutton_to_group(colorm_rog, "TEMP");
GLUI_Panel *scans_color = glui1->add_rollout_to_panel(color_panel, "Color type:", false);
scans_color->set_alignment(GLUI_ALIGN_LEFT);
GLUI_RadioGroup *scans_colored = glui1->add_radiogroup_to_panel(scans_color, &colorScanVal, 0, &setScansColored);
glui1->add_radiobutton_to_group(scans_colored, "None");
glui1->add_radiobutton_to_group(scans_colored, "Id Scans by Color");
GLUI_RadioButton *colorb = glui1->add_radiobutton_to_group( scans_colored, "Color by Points");
if (!(pointtype.hasColor())) colorb->disable();
mincol_spinner = glui1->add_spinner_to_panel(color_panel, "Min Val:", GLUI_SPINNER_FLOAT, &mincolor_value, 0, &minmaxChanged);
mincol_spinner->set_alignment(GLUI_ALIGN_RIGHT);
maxcol_spinner = glui1->add_spinner_to_panel(color_panel, "Max Val:", GLUI_SPINNER_FLOAT, &maxcolor_value, 0, &minmaxChanged);
maxcol_spinner->set_alignment(GLUI_ALIGN_RIGHT);
glui1->add_button_to_panel(color_panel, "Reset Min/Max", 0, &resetMinMax )->set_alignment( GLUI_ALIGN_CENTER );
glui1->add_separator();
/****** Invert button *****/
glui1->add_button( "Invert", 0, invertView )->set_alignment( GLUI_ALIGN_CENTER );
/****** Animate button *****/
anim_spinner = glui1->add_spinner( "Anim delay:", GLUI_SPINNER_INT, &anim_delay);
anim_spinner->set_int_limits( 0, 100 );
anim_spinner->set_speed( 1 );
glui1->add_button( "Animate", 0, startAnimation )->set_alignment( GLUI_ALIGN_CENTER );
glui1->add_separator();
/**** Path panel *******/
path_panel = glui1->add_rollout("Camera Path :", false );
path_panel ->set_alignment( GLUI_ALIGN_LEFT );
path_filename_edit = glui1->add_edittext_to_panel(path_panel,"File: ",GLUI_EDITTEXT_TEXT, path_file_name);
path_filename_edit->set_alignment( GLUI_ALIGN_LEFT );
glui1->add_button_to_panel(path_panel, "Save Path ", 0, savePath)->set_alignment( GLUI_ALIGN_CENTER);
glui1->add_button_to_panel(path_panel, "Load Path ", 0, loadPath)->set_alignment( GLUI_ALIGN_CENTER);
glui1->add_button_to_panel(path_panel, "Load Robot P.", 0, drawRobotPath )->set_alignment( GLUI_ALIGN_CENTER );
glui1->add_separator_to_panel(path_panel);
glui1->add_checkbox_to_panel(path_panel, "Save Animation", &save_animation);
interpol_box = glui1->add_checkbox_to_panel(path_panel, "Interpolate by Distance", &inter_by_dist, -1, &callCameraUpdate);
//always_box = glui2->add_checkbox_to_panel(nav_panel, "Always all Points", &dummy4, 0, &changePointMode);
//glui1->add_checkbox_to_panel(path_panel, "Interpolate by Distance", &inter_by_dist);
glui1->add_button_to_panel(path_panel, "Animate Path", 0, pathAnimate)->set_alignment( GLUI_ALIGN_CENTER);
/**** Position panel *******/
pose_panel = glui1->add_rollout("Position :", false );
pose_panel ->set_alignment( GLUI_ALIGN_LEFT );
pose_filename_edit = glui1->add_edittext_to_panel(pose_panel,"File: ",GLUI_EDITTEXT_TEXT, pose_file_name);
pose_filename_edit->set_alignment( GLUI_ALIGN_LEFT );
glui1->add_button_to_panel(pose_panel, "Save Pose ", 0, savePose)->set_alignment( GLUI_ALIGN_CENTER);
glui1->add_button_to_panel(pose_panel, "Load Pose ", 0, loadPose)->set_alignment( GLUI_ALIGN_CENTER);
image_spinner = glui1->add_spinner_to_panel(pose_panel, "Factor : ",
GLUI_SPINNER_INT, &factor);
image_spinner->set_int_limits( 1, 10 );
image_spinner->set_speed( 1 );
image_spinner->set_alignment(GLUI_ALIGN_RIGHT);
glui1->add_button_to_panel(pose_panel, "Save Image ", 0, saveImage)->set_alignment( GLUI_ALIGN_CENTER);
glui1->add_separator();
/**** Selection panel ******/
selection_panel = glui1->add_rollout("Selection :", false );
selection_panel ->set_alignment( GLUI_ALIGN_LEFT );
selection_filename_edit = glui1->add_edittext_to_panel(selection_panel,"File: ",GLUI_EDITTEXT_TEXT, selection_file_name);
selection_filename_edit->set_alignment( GLUI_ALIGN_LEFT );
glui1->add_button_to_panel(selection_panel, "Save selected points ", 0, saveSelection)->set_alignment( GLUI_ALIGN_CENTER);
glui1->add_button_to_panel(selection_panel, "Clear selected points ", 0, clearSelection)->set_alignment( GLUI_ALIGN_CENTER);
glui1->add_checkbox_to_panel(selection_panel, "Select/Unselect", &selectOrunselect);
glui1->add_checkbox_to_panel(selection_panel, "Select Voxels", &select_voxels);
depth_spinner = glui1->add_spinner_to_panel(selection_panel, "Depth : ",
GLUI_SPINNER_INT, &selection_depth);
depth_spinner->set_int_limits( 1, 100 );
depth_spinner->set_speed( 1 );
depth_spinner->set_alignment(GLUI_ALIGN_RIGHT);
brushsize_spinner = glui1->add_spinner_to_panel(selection_panel, "Brushsize : ",
GLUI_SPINNER_INT, &brush_size);
brushsize_spinner->set_int_limits( 0, 100 );
brushsize_spinner->set_speed( 1 );
brushsize_spinner->set_alignment(GLUI_ALIGN_RIGHT);
glui1->add_separator();
/**** Advanced panel ******/
if (advanced_controls) {
advanced_panel = glui1->add_rollout("Advanced :", false );
advanced_panel->set_alignment( GLUI_ALIGN_LEFT );
// glui1->add_edittext_to_panel(advanced_panel,"Frame #: ",GLUI_EDITTEXT_TEXT, current_frame)->set_alignment( GLUI_ALIGN_LEFT );
frame_spinner = glui1->add_spinner_to_panel(advanced_panel, "Frame #: ",
GLUI_SPINNER_INT, &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();
}

184
.svn/pristine/32/32bed354a46354672f5ec89a85c9b4140371b3ab.svn-base Normal file
View file

@ -0,0 +1,184 @@
/*
* scan_io_uso_rgb implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "scanio/scan_io_uos_rgb.h"
#include <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>* temperature, 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

225
.svn/pristine/37/3725536eaceee10825f0f4fcaaae5f9a0b8b210d.svn-base Normal file
View file

@ -0,0 +1,225 @@
/*
* point_type implementation
*
* Copyright (C) Jan Elseberg, Dorit Borrmann.
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Representation of a 3D point type
* @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany.
*/
#include "slam6d/point_type.h"
#include "slam6d/scan.h"
#include <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[8] = 1;
dimensionmap[0] = 1; // height
}
PointType::PointType(unsigned int _types) : types(_types) {
dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = dimensionmap[8] = 1; // choose height per default
dimensionmap[0] = 1; // height
pointdim = 3;
if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++;
if (types & PointType::USE_TEMPERATURE) dimensionmap[2] = pointdim++;
if (types & PointType::USE_AMPLITUDE) dimensionmap[3] = pointdim++;
if (types & PointType::USE_DEVIATION) dimensionmap[4] = pointdim++;
if (types & PointType::USE_TYPE) dimensionmap[5] = pointdim++;
if (types & PointType::USE_COLOR) dimensionmap[6] = pointdim++;
if (types & PointType::USE_TIME) dimensionmap[7] = pointdim++;
if (types & PointType::USE_INDEX) dimensionmap[8] = pointdim++;
}
bool PointType::hasReflectance() {
return hasType(USE_REFLECTANCE);
}
bool PointType::hasTemperature() {
return hasType(USE_TEMPERATURE);
}
bool PointType::hasAmplitude() {
return hasType(USE_AMPLITUDE);
}
bool PointType::hasDeviation() {
return hasType(USE_DEVIATION);
}
bool PointType::hasType() {
return hasType(USE_TYPE);
}
bool PointType::hasColor() {
return hasType(USE_COLOR);
}
bool PointType::hasTime() {
return hasType(USE_TIME);
}
bool PointType::hasIndex() {
return hasType(USE_INDEX);
}
unsigned int PointType::getReflectance() {
return dimensionmap[1];
}
unsigned int PointType::getTemperature() {
return dimensionmap[2];
}
unsigned int PointType::getAmplitude() {
return dimensionmap[3];
}
unsigned int PointType::getDeviation() {
return dimensionmap[4];
}
unsigned int PointType::getTime() {
return dimensionmap[7];
}
unsigned int PointType::getIndex() {
return dimensionmap[8];
}
unsigned int PointType::getType() {
return dimensionmap[5];
}
unsigned int PointType::getType(unsigned int type) {
if (type == USE_NONE ) {
return dimensionmap[0];
} else if (type == USE_HEIGHT) {
return dimensionmap[0];
} else if (type == USE_REFLECTANCE) {
return dimensionmap[1];
} else if (type == USE_TEMPERATURE) {
return dimensionmap[2];
} else if (type == USE_AMPLITUDE) {
return dimensionmap[3];
} else if (type == USE_DEVIATION) {
return dimensionmap[4];
} else if (type == USE_TYPE) {
return dimensionmap[5];
} else if (type == USE_COLOR) {
return dimensionmap[6];
} else if (type == USE_TIME) {
return dimensionmap[7];
} else {
return 0;
}
}
unsigned int PointType::getPointDim() { return pointdim; }
PointType PointType::deserialize(std::ifstream &f) {
unsigned int types;
f.read(reinterpret_cast<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_TEMPERATURE = 2;
const unsigned int PointType::USE_AMPLITUDE = 4;
const unsigned int PointType::USE_DEVIATION = 8;
const unsigned int PointType::USE_HEIGHT = 16;
const unsigned int PointType::USE_TYPE = 32;
const unsigned int PointType::USE_COLOR = 64;
const unsigned int PointType::USE_TIME = 128;
const unsigned int PointType::USE_INDEX = 256;
void PointType::useScan(Scan* scan)
{
// clear pointers first
m_xyz = 0; m_rgb = 0; m_reflectance = 0; m_temperature = 0; m_amplitude = 0; m_type = 0; m_deviation = 0;
// collectively load data to avoid unneccessary loading times due to split get("") calls
unsigned int types = DATA_XYZ;
if(hasColor()) types |= DATA_RGB;
if(hasReflectance()) types |= DATA_REFLECTANCE;
if(hasTemperature()) types |= DATA_TEMPERATURE;
if(hasAmplitude()) types |= DATA_AMPLITUDE;
if(hasType()) types |= DATA_TYPE;
if(hasDeviation()) types |= DATA_DEVIATION;
scan->get(types);
// access data
try {
m_xyz = new DataXYZ(scan->get("xyz"));
if(hasColor()) m_rgb = new DataRGB(scan->get("rgb"));
if(hasReflectance()) m_reflectance = new DataReflectance(scan->get("reflectance"));
if(hasTemperature()) m_temperature = new DataTemperature(scan->get("temperature"));
if(hasAmplitude()) m_amplitude = new DataAmplitude(scan->get("amplitude"));
if(hasType()) m_type = new DataType(scan->get("type"));
if(hasDeviation()) m_deviation = new DataDeviation(scan->get("deviation"));
// check if data is available, otherwise reset pointer to indicate that the scan doesn't prove this value
if(m_rgb && !m_rgb->valid()) { delete m_rgb; m_rgb = 0; }
if(m_reflectance && !m_reflectance->valid()) { delete m_reflectance; m_reflectance = 0; }
if(m_temperature && !m_temperature->valid()) { delete m_temperature; m_temperature = 0; }
if(m_amplitude && !m_amplitude->valid()) { delete m_amplitude; m_amplitude = 0; }
if(m_type && !m_type->valid()) { delete m_type; m_type = 0; }
if(m_deviation && !m_deviation->valid()) { delete m_deviation; m_deviation = 0; }
} catch(runtime_error& e) {
// unlock everything again
clearScan();
throw e;
}
}
void PointType::clearScan()
{
// unlock data access
if(m_xyz) delete m_xyz;
if(hasColor() && m_rgb) delete m_rgb;
if(hasReflectance() && m_reflectance) delete m_reflectance;
if(hasTemperature() && m_temperature) delete m_temperature;
if(hasAmplitude() && m_amplitude) delete m_amplitude;
if(hasType() && m_type) delete m_type;
if(hasDeviation() && m_deviation) delete m_deviation;
// TODO: scan->clear() on all of these types
}
unsigned int PointType::getScanSize(Scan* scan)
{
return scan->size<DataXYZ>("xyz");
}

27
.svn/pristine/38/38638870846ffabe1b5a9ba2fffe492cc2b456b4.svn-base Normal file
View file

@ -0,0 +1,27 @@
/**
* @file
* @brief IO of a 3D scan in riegl_txt file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_RIEGL_H__
#define __SCAN_IO_RIEGL_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for Riegl scans
*
* The compiled class is available as shared object file
*/
class ScanIO_riegl_txt : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

54
.svn/pristine/39/3904271b6c0880687d06770b926d116b0b928fa4.svn-base Normal file
View file

@ -0,0 +1,54 @@
if(WIN32)
add_library(pointfilter STATIC ../slam6d/pointfilter.cc)
else(WIN32)
add_library(pointfilter SHARED ../slam6d/pointfilter.cc)
endif(WIN32)
set(SCANIO_LIBNAMES
uos uosr uos_rgb uos_rrgbt ks ks_rgb riegl_txt riegl_rgb rts velodyne
)
if(WITH_RIVLIB)
set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} rxp)
if(LIBXML2_FOUND)
include_directories(${LIBXML2_INCLUDE_DIR})
# set(SCANIO_LIBNAMES ${SCANIO_LIBNAMES} riegl_project)
# target_link_libraries(scan_io_riegl_project ${RIVLIB} scan_io_rxp ${LIBXML2_LIBRARIES})
endif(LIBXML2_FOUND)
endif(WITH_RIVLIB)
#IF (WITH_CAD)
# IF(NOT WIN32)
# add_library(scan_io_cad SHARED scan_io_cad.cc)
# target_link_libraries(scan_io_cad ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
# ENDIF(NOT WIN32)
#ENDIF (WITH_CAD)
foreach(libname ${SCANIO_LIBNAMES})
if(WIN32)
#add_library(scan_io_${libname} STATIC scan_io_${libname}.cc)
add_library(scan_io_${libname} SHARED scan_io_${libname}.cc)
else(WIN32)
add_library(scan_io_${libname} SHARED scan_io_${libname}.cc)
endif(WIN32)
target_link_libraries(scan_io_${libname} pointfilter ${Boost_LIBRARIES} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY})
endforeach(libname)
if(WITH_RIVLIB)
target_link_libraries(scan_io_rxp ${RIVLIB})
if(LIBXML2_FOUND)
target_link_libraries(scan_io_rxp ${LIBXML2_LIBRARIES}) #scan_io_riegl_project ${RIVLIB})
endif(LIBXML2_FOUND)
endif(WITH_RIVLIB)
if(WIN32)
add_library(scanio STATIC scan_io.cc ../slam6d/io_types.cc)
else(WIN32)
add_library(scanio SHARED scan_io.cc ../slam6d/io_types.cc)
endif(WIN32)
if(UNIX)
target_link_libraries(scanio dl)
endif(UNIX)

831
.svn/pristine/42/420f66c60f981fa8a46571f136e828917bef1901.svn-base Normal file
View file

@ -0,0 +1,831 @@
/*
* wxshow implementation
*
* Copyright (C) Jan Elseberg
*
* Released under the GPL version 3.
*
*/
#include "show_common.cc"
#include "wx/wx.h"
#include "wx/sizer.h"
#include "wx/glcanvas.h"
#include "show/wxshow.h"
#include "show/selectionframe.h"
class SelectionImpl : public Selection {
public:
SelectionImpl( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Selection"), bool advanced_controls = false, const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( -1,-1 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL ) : Selection(parent, id, title, pos, size, style, advanced_controls) {
if (pointtype.hasReflectance()) m_choice11->Append(wxT("reflectance"));
if (pointtype.hasTemperature()) m_choice11->Append(wxT("temperature"));
if (pointtype.hasAmplitude()) m_choice11->Append(wxT("amplitude"));
if (pointtype.hasDeviation()) m_choice11->Append(wxT("deviation"));
if (pointtype.hasType()) m_choice11->Append(wxT("type"));
};
// Virtual event handlers, overide them in your derived class
//
virtual void OnDrawPoints( wxCommandEvent& event ) {
if (event.IsChecked()) {
show_points = true;
} else {
show_points = false;
}
haveToUpdate = 1;
event.Skip();
}
virtual void OnDrawCameras( wxCommandEvent& event )
{
if (event.IsChecked()) {
show_cameras = true;
} else {
show_cameras = false;
}
haveToUpdate = 1;
event.Skip();
}
virtual void OnDrawPaths( wxCommandEvent& event )
{
if (event.IsChecked()) {
show_path = true;
} else {
show_path = false;
}
haveToUpdate = 1;
event.Skip();
}
virtual void OnPointSize( wxSpinEvent& event )
{
pointsize = event.GetPosition();
haveToUpdate = 1;
event.Skip();
}
virtual void OnFogChoice( wxCommandEvent& event )
{
show_fog = event.GetSelection();
haveToUpdate = 1;
event.Skip();
}
virtual void OnFogDensity( wxSpinEvent& event ) {
wxSpinCtrlDbl *spinner = (wxSpinCtrlDbl*)event.GetEventObject();
fogDensity = spinner->GetValue();
haveToUpdate = 1;
event.Skip();
}
virtual void OnColorValue( wxCommandEvent& event )
{
// we can't use the glui-show function for changing the color as a listbox is used here instead
switch (event.GetSelection()) {
case 0:
cm->setCurrentType(PointType::USE_HEIGHT);
break;
case 1:
if (pointtype.hasReflectance()) {
cm->setCurrentType(PointType::USE_REFLECTANCE);
break;
}
case 2:
if (pointtype.hasTemperature()) {
cm->setCurrentType(PointType::USE_TEMPERATURE);
break;
}
case 3:
if (pointtype.hasAmplitude()) {
cm->setCurrentType(PointType::USE_AMPLITUDE);
break;
}
case 4:
if (pointtype.hasDeviation()) {
cm->setCurrentType(PointType::USE_DEVIATION);
break;
}
case 5:
if (pointtype.hasType()) {
cm->setCurrentType(PointType::USE_TYPE);
break;
}
default:
break;
};
haveToUpdate = 1;
resetMinMax(0);
event.Skip();
}
virtual void OnColorMap( wxCommandEvent& event )
{
listboxColorMapVal = event.GetSelection();
changeColorMap(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnColorType( wxCommandEvent& event ) {
colorScanVal = event.GetSelection();
setScansColored(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnColorMinVal( wxSpinEvent& event ) {
//mincolor_value = event.GetPosition();
wxSpinCtrlDbl *spinner = (wxSpinCtrlDbl*)event.GetEventObject();
mincolor_value = spinner->GetValue();
minmaxChanged(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnColorMaxVal( wxSpinEvent& event ) {
//maxcolor_value = event.GetPosition();
wxSpinCtrlDbl *spinner = (wxSpinCtrlDbl*)event.GetEventObject();
maxcolor_value = spinner->GetValue();
minmaxChanged(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnColorResetMinMax( wxCommandEvent& event ) {
resetMinMax(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnInvert( wxCommandEvent& event ) {
invertView(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnAnimDelay( wxSpinEvent& event ) {
anim_delay = event.GetPosition();
event.Skip();
}
virtual void OnAnimate( wxCommandEvent& event ) {
startAnimation(0);
event.Skip();
}
virtual void OnCameraFile( wxCommandEvent& event ) {
wxString s = event.GetString();
const wxCharBuffer buffer = wxString(event.GetString()).mb_str(wxConvISO8859_1);
const char* cc = buffer.data();
strcpy(path_file_name, cc);
event.Skip();
}
virtual void OnCameraSavePath( wxCommandEvent& event ) {
savePath(0);
event.Skip();
}
virtual void OnCameraLoadPath( wxCommandEvent& event ) {
loadPath(0);
event.Skip();
}
virtual void OnCameraLoadRobotPath( wxCommandEvent& event ) {
for(unsigned int i = 0; i < MetaMatrix.size(); i++){
//temp variable
double *temp;
//Now, lets go to the last of each frame file to
//extract the transformation matrix obtained
//after scan matching has been done.
glMultMatrixd(MetaMatrix[i].back());
//temp is final transformation matrix
temp = MetaMatrix[i].back();
Point campos(temp[12], temp[13] + 100, temp[14]);
// calculate lookat point
Point lookat(0, 0, 50);
Point up(0, 50, 0);
double tmat[16];
for (int i =0;i<16;i++) tmat[i] = temp[i];
lookat.transform(tmat);
lookat.x = lookat.x ;
lookat.y = lookat.y + 100;
lookat.z = lookat.z ;
up.transform(tmat);
up.x = up.x ;
up.y = up.y + 100;
up.z = up.z ;
cams.push_back(campos);
lookats.push_back(lookat);
ups.push_back(up);
}
//signal for the update of scene
haveToUpdate = 1;
event.Skip();
}
virtual void OnSaveAnimation( wxCommandEvent& event ) {
if (event.IsChecked()) {
save_animation = true;
} else {
save_animation = false;
}
haveToUpdate = 1;
event.Skip();
}
virtual void OnAnimatePath( wxCommandEvent& event ) {
pathAnimate(0);
event.Skip();
}
virtual void OnPositionFile( wxCommandEvent& event ) {
wxString s = event.GetString();
const wxCharBuffer buffer = wxString(event.GetString()).mb_str(wxConvISO8859_1);
const char* cc = buffer.data();
strcpy(pose_file_name, cc);
event.Skip();
}
virtual void OnPositionSave( wxCommandEvent& event ) {
savePose(0);
event.Skip();
}
virtual void OnPositionLoad( wxCommandEvent& event ) {
loadPose(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnFactor( wxSpinEvent& event ) {
factor = event.GetPosition();
event.Skip();
}
virtual void OnSaveImage( wxCommandEvent& event ) {
saveImage(0);
event.Skip();
}
virtual void OnSelectionFile( wxCommandEvent& event ) {
wxString s = event.GetString();
const wxCharBuffer buffer = wxString(event.GetString()).mb_str(wxConvISO8859_1);
const char* cc = buffer.data();
strcpy(selection_file_name, cc);
event.Skip();
}
virtual void OnSelectionSave( wxCommandEvent& event ) {
saveSelection(0);
event.Skip();
}
virtual void OnSelectionClear( wxCommandEvent& event ) {
clearSelection(0);
event.Skip();
}
virtual void OnSelectionSU( wxCommandEvent& event ) {
if (event.IsChecked()) {
selectOrunselect = true;
} else {
selectOrunselect = false;
}
event.Skip();
}
virtual void OnSelectionSV( wxCommandEvent& event ) {
if (event.IsChecked()) {
select_voxels = true;
} else {
select_voxels = false;
}
event.Skip();
}
virtual void OnSelectionDepth( wxSpinEvent& event ) {
selection_depth = event.GetPosition();
event.Skip();
}
virtual void OnSelectionBrushsize( wxSpinEvent& event ) {
brush_size = event.GetPosition();
event.Skip();
}
virtual void OnFrameSpinner( wxSpinEvent& event ) {
current_frame = event.GetPosition();
haveToUpdate = 1;
event.Skip();
}
virtual void OnFramerateSpinner( wxSpinEvent& event ) {
idealfps = event.GetPosition();
haveToUpdate = 1;
event.Skip();
}
virtual void OnFarplaneSpinner( wxSpinEvent& event ) {
maxfardistance = event.GetPosition();
haveToUpdate = 1;
event.Skip();
}
virtual void OnNearplaneSpinner( wxSpinEvent& event ) {
neardistance = event.GetPosition();
haveToUpdate = 1;
event.Skip();
}
virtual void OnCycleLOD( wxCommandEvent& event ) {
::cycleLOD();
haveToUpdate = 1;
event.Skip();
}
virtual void OnLODAdaption( wxSpinEvent& event ) {
adaption_rate = ((wxSpinCtrlDbl*)event.GetEventObject())->GetValue();
haveToUpdate = 1;
event.Skip();
}
public:
void updateControls() {
if (!MetaMatrix.empty()) {
frame_spin->SetRange(0, MetaMatrix[0].size()-1);
frame_spin->SetValue(current_frame);
}
m_spinCtrl61->SetValue(mincolor_value);
m_spinCtrl6->SetValue(maxcolor_value);
}
};
class ControlImpl : public Controls {
public:
ControlImpl( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Controls"), const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( -1,-1 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL )
: Controls( parent, id, title, pos, size, style ) {}
virtual void OnApexAngle( wxSpinEvent& event ) {
haveToUpdate = 2;
cangle = ((wxSpinCtrlDbl*)event.GetEventObject())->GetValue();
event.Skip();
}
virtual void OnParallelZoom( wxSpinEvent& event ) {
haveToUpdate = 2;
pzoom = ((wxSpinCtrlDbl*)event.GetEventObject())->GetValue();
event.Skip();
}
virtual void OnTopView( wxCommandEvent& event ) {
topView(); // TODO update controls
haveToUpdate = 2;
event.Skip();
}
virtual void OnResetPosition( wxCommandEvent& event ) {
resetView(0);
event.Skip();
}
virtual void OnChooseCamera( wxSpinEvent& event ) {
cam_choice = event.GetPosition();
haveToUpdate = 1;
event.Skip();
}
virtual void OnAddCamera( wxCommandEvent& event ) {
callAddCamera(1);
haveToUpdate = 1;
event.Skip();
}
virtual void OnDeleteCamera( wxCommandEvent& event ) {
callDeleteCamera(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnMouseNav( wxCommandEvent& event ) {
/*
if (event.IsChecked()) {
cameraNavMouseMode = true;
} else {
cameraNavMouseMode = false;
}*/
// TODO implement secondary navigation procedure
event.Skip();
}
virtual void OnAlwaysAllPoints( wxCommandEvent& event ) {
changePointMode(0);
haveToUpdate = 1;
event.Skip();
}
virtual void OnAlwaysReducePoints( wxCommandEvent& event ) {
changePointMode(1);
haveToUpdate = 1;
event.Skip();
}
public:
void updateControls() {
apex_spinner->SetValue(cangle);
parallel_spinner->SetValue(pzoom);
if(showTopView) {
apex_spinner->Disable();
parallel_spinner->Enable();
} else {
parallel_spinner->Disable();
apex_spinner->Enable();
}
switch(pointmode) {
case -1:
always_box->SetValue(false);
alwaysred_box->SetValue(true);
break;
case 0:
always_box->SetValue(false);
alwaysred_box->SetValue(false);
break;
case 1:
always_box->SetValue(true);
alwaysred_box->SetValue(false);
break;
}
camera_spinner->SetRange(1, cams.size());
camera_spinner->SetValue(cam_choice);
}
};
class wxShow: public wxApp
{
virtual bool OnInit();
SelectionImpl *selection;
ControlImpl *controls;
wxFrame *frame;
BasicGLPane * glPane;
public:
void OnClose(wxCloseEvent& event);
void updateControls(){
controls->updateControls();
selection->updateControls();
}
};
static wxShow *globalGUI = 0;
void wxShow::OnClose(wxCloseEvent& event) {
exit(0);
}
IMPLEMENT_APP(wxShow)
bool wxShow::OnInit()
{
globalGUI = this;
// wxwidgets saves the argv as wxchar, which is 2-byte unicode, so we must convert it back
char **new_argv = new char*[argc];
for (int i = 0; i < argc; i++) {
const wxCharBuffer buffer = wxString(wxApp::argv[i]).mb_str(wxConvISO8859_1);
const char* cc = buffer.data();
new_argv[i] = new char[ strlen(cc) +1 ];
strcpy(new_argv[i], cc);
}
initShow(argc, new_argv);
//glClearColor(0.0, 0.0, 0.0, 0.0);
wxBoxSizer* sizer = new wxBoxSizer(wxHORIZONTAL);
frame = new wxFrame((wxFrame *)NULL, -1, wxT("Viewer"), wxPoint(START_X, START_Y), wxSize(START_WIDTH, START_HEIGHT));
int args[] = {WX_GL_RGBA, WX_GL_DOUBLEBUFFER, WX_GL_DEPTH_SIZE, 16, 0};
glPane = new BasicGLPane( (wxFrame*) frame, args);
sizer->Add(glPane, 1, wxEXPAND);
frame->SetSizer(sizer);
frame->SetAutoLayout(true);
selection = new SelectionImpl( (wxWindow*)NULL, wxID_ANY, wxT("Selection"), advanced_controls, wxPoint(START_X + START_WIDTH + 50, START_Y + 30) );
controls = new ControlImpl( (wxWindow*)NULL, wxID_ANY, wxT("Controls"), wxPoint(START_X, START_Y + START_HEIGHT + 20 ) );
selection->SetSize(wxSize(200,393) );
selection->SetAutoLayout(true);
selection ->Show();
selection ->Layout();
controls->SetAutoLayout(true);
controls ->Show();
frame->Show();
frame->Connect( wxEVT_CLOSE_WINDOW, wxCloseEventHandler( wxShow::OnClose ) );
return true;
}
BEGIN_EVENT_TABLE(BasicGLPane, wxGLCanvas)
EVT_IDLE(BasicGLPane::idle_event)
EVT_MOTION(BasicGLPane::mouseMoved)
EVT_MOUSE_EVENTS(BasicGLPane::mouseEvent)
//EVT_LEFT_DOWN(BasicGLPane::mouseDown)
//EVT_LEFT_UP(BasicGLPane::mouseReleased)
//EVT_RIGHT_DOWN(BasicGLPane::rightClick)
//EVT_LEAVE_WINDOW(BasicGLPane::mouseLeftWindow)
EVT_SIZE(BasicGLPane::resized)
EVT_KEY_DOWN(BasicGLPane::keyPressed)
//EVT_KEY_UP(BasicGLPane::keyReleased)
//EVT_MOUSEWHEEL(BasicGLPane::mouseWheelMoved)
EVT_PAINT(BasicGLPane::render)
END_EVENT_TABLE()
// some useful events to use
void BasicGLPane::mouseEvent(wxMouseEvent& event)
{
SetFocus();
int x = event.GetX();
int y = event.GetY();
if (event.Dragging()) {
} else if (event.IsButton()) {
int button, state;
if (event.ButtonDown()) {
state = GLUT_DOWN;
} else {
state = GLUT_UP;
}
if (event.Button(1)) {
button = GLUT_LEFT_BUTTON;
} else if (event.Button(2)) {
button = GLUT_MIDDLE_BUTTON;
} else if (event.Button(3)) {
button = GLUT_RIGHT_BUTTON;
} else {
button = 0;
}
CallBackMouseFunc(button, state, x, y);
}
}
void BasicGLPane::idle() {
if(glutGetWindow() != window_id)
glutSetWindow(window_id);
/*
static unsigned long start = GetCurrentTimeInMilliSec();
// return as nothing has to be updated
if (haveToUpdate == 0) {
if ((GetCurrentTimeInMilliSec()-start) > 100) {
paint(true);
start = GetCurrentTimeInMilliSec();
}
return;
}
start = GetCurrentTimeInMilliSec();
*/
if (haveToUpdate == 0) {
if (!fullydisplayed && !mousemoving && !keypressed) {
glDrawBuffer(buffermode);
//Call the display function
DisplayItFunc(GL_RENDER, true);
}
return;
}
// case: display is invalid - update it
if (haveToUpdate == 1) {
paint();
haveToUpdate = 0;
return;
}
// case: display is invalid - update it with all points
/*if (haveToUpdate == 7) {
showall = true;
paint();
haveToUpdate = 0;
return;
}
*/
// case: camera angle is changed - instead of repeating code call Reshape,
// since these OpenGL commands are the same
if (haveToUpdate == 2) {
int viewport[4];
glGetIntegerv(GL_VIEWPORT, viewport);
CallBackReshapeFunc(viewport[2],viewport[3]);
paint();
haveToUpdate = 0;
return;
}
// case: animation
if(haveToUpdate == 3 ){
frameNr += 1;
if(!(MetaMatrix.size() > 1 && frameNr < (int) MetaMatrix[1].size())){
frameNr = 0;
haveToUpdate = 4;
return;
}
paint();
if(save_animation){
string filename = scan_dir + "animframe" + to_string(frameNr,5) + ".ppm";
cout << "write " << filename << endl;
glDumpWindowPPM(filename.c_str(),0);
}
}
#ifdef _MSC_VER
Sleep(300);
Sleep(anim_delay);
#else
usleep(anim_delay * 10000);
#endif
if (haveToUpdate == 4) { // stop animation
frameNr = 0; // delete these lines if you want a 'continue' functionality.
haveToUpdate = 1;
}
// case: path animation
if(haveToUpdate == 6){
if (path_iterator == 0) {
oldcamNavMode = cameraNavMouseMode; // remember state of old mousenav
cameraNavMouseMode = 0;
}
//check if the user wants to animate both
//scan matching and the path at the same
//time
//cout << "path_iterator: " << path_iterator << endl;
if(path_iterator < path_vectorX.size()){ // standard animation case
//call the path animation function
//hide both the cameras and the path
show_cameras = 0;
show_path = 0;
//increase the iteration count
path_iterator += 1;
//cout << "i am here" << endl;
//repaint the screen
paint();
//save the animation
if(save_animation){
string filename = scan_dir + "animframe" + to_string(path_iterator,5) + ".ppm";
string jpgname = scan_dir + "animframe" + to_string(path_iterator,5) + ".jpg";
cout << "written " << filename << " of " << path_vectorX.size() << " files" << endl;
glWriteImagePPM(filename.c_str(), factor, 0);
haveToUpdate = 6;
}
}else{ // animation has just ended
cameraNavMouseMode = oldcamNavMode;
show_cameras = 1;
show_path = 1;
//cout << "i am here instead" << endl;
haveToUpdate = 0;
}
}
}
void BasicGLPane::idle_event(wxIdleEvent& event)
{
idle();
//event.RequestMore();
}
void BasicGLPane::mouseMoved(wxMouseEvent& event)
{
int x = event.GetX();
int y = event.GetY();
CallBackMouseMotionFunc(x, y);
//idle();
}
void BasicGLPane::mouseDown(wxMouseEvent& event) {}
void BasicGLPane::mouseWheelMoved(wxMouseEvent& event) {}
void BasicGLPane::mouseReleased(wxMouseEvent& event) {}
void BasicGLPane::rightClick(wxMouseEvent& event) {}
void BasicGLPane::mouseLeftWindow(wxMouseEvent& event) {}
void BasicGLPane::keyPressed(wxKeyEvent& event) {
KeyboardFunc(event.GetKeyCode(), event.CmdDown(), event.AltDown(), event.ShiftDown());
}
void BasicGLPane::keyReleased(wxKeyEvent& event) {}
// Vertices and faces of a simple cube to demonstrate 3D render
// source: http://www.opengl.org/resources/code/samples/glut_examples/examples/cube.c
GLfloat v[8][3];
GLint faces[6][4] = { /* Vertex indices for the 6 faces of a cube. */
{0, 1, 2, 3}, {3, 2, 6, 7}, {7, 6, 5, 4},
{4, 5, 1, 0}, {5, 6, 2, 1}, {7, 4, 0, 3} };
BasicGLPane::BasicGLPane(wxFrame* parent, int* args) :
wxGLCanvas(parent, wxID_ANY, args, wxDefaultPosition, wxDefaultSize, wxFULL_REPAINT_ON_RESIZE||wxWANTS_CHARS)
{
m_context = new wxGLContext(this);
// prepare a simple cube to demonstrate 3D render
// source: http://www.opengl.org/resources/code/samples/glut_examples/examples/cube.c
v[0][0] = v[1][0] = v[2][0] = v[3][0] = -1;
v[4][0] = v[5][0] = v[6][0] = v[7][0] = 1;
v[0][1] = v[1][1] = v[4][1] = v[5][1] = -1;
v[2][1] = v[3][1] = v[6][1] = v[7][1] = 1;
v[0][2] = v[3][2] = v[4][2] = v[7][2] = 1;
v[1][2] = v[2][2] = v[5][2] = v[6][2] = -1;
// To avoid flashing on MSW
SetBackgroundStyle(wxBG_STYLE_CUSTOM);
}
BasicGLPane::~BasicGLPane()
{
delete m_context;
}
void BasicGLPane::resized(wxSizeEvent& evt)
{
// wxGLCanvas::OnSize(evt);
wxSize s = evt.GetSize();
CallBackReshapeFunc(s.GetWidth(), s.GetHeight());
Refresh();
}
/** Inits the OpenGL viewport for drawing in 3D. */
void BasicGLPane::prepare3DViewport(int topleft_x, int topleft_y, int bottomrigth_x, int bottomrigth_y)
{
glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // Black Background
glClearDepth(1.0f); // Depth Buffer Setup
glEnable(GL_DEPTH_TEST); // Enables Depth Testing
glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
glEnable(GL_COLOR_MATERIAL);
glViewport(topleft_x, topleft_y, bottomrigth_x-topleft_x, bottomrigth_y-topleft_y);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
float ratio_w_h = (float)(bottomrigth_x-topleft_x)/(float)(bottomrigth_y-topleft_y);
gluPerspective(45 /*view angle*/, ratio_w_h, 0.1 /*clip close*/, 200 /*clip far*/);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
int BasicGLPane::getWidth()
{
return GetSize().x;
}
int BasicGLPane::getHeight()
{
return GetSize().y;
}
void BasicGLPane::render( wxPaintEvent& evt )
{
paint();
}
void BasicGLPane::paint(bool interruptable)
{
if(!IsShown()) return;
wxGLCanvas::SetCurrent(*m_context);
wxPaintDC(this); // only to be used in paint events. use wxClientDC to paint outside the paint event
glDrawBuffer(buffermode);
// delete framebuffer and z-buffer
//Call the display function
DisplayItFunc(GL_RENDER, interruptable);
// show the rednered scene
if (!interruptable) { // interruptible draw is single buffered
SwapBuffers();
}
// ProcessPendingEvents();
}
void updateControls() {
globalGUI->updateControls();
}
void updatePointModeControls() {}
void updateTopViewControls() {}
void resetRotationButton() {}
void updateCamControls() {}
static bool interrupted;
void interruptDrawing() {
interrupted = true;
}
void checkForInterrupt() {
interrupted = false;
}
bool isInterrupted() {
globalGUI->Dispatch();
globalGUI->ProcessPendingEvents();
return interrupted;
}

23
.svn/pristine/46/461046f9b09c0819a22b163755769774c6596509.svn-base Normal file
View file

@ -0,0 +1,23 @@
FIND_PACKAGE(OpenCV REQUIRED)
SET(FBR_IO_SRC scan_cv.cc)
add_library(fbr_cv_io STATIC ${FBR_IO_SRC})
SET(FBR_PANORAMA_SRC panorama.cc)
add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC})
SET(FBR_FEATURE_SRC feature.cc)
add_library(fbr_feature STATIC ${FBR_FEATURE_SRC})
SET(FBR_FEATURE_MATCHER_SRC feature_matcher.cc)
add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC})
SET(FBR_REGISTRATION_SRC registration.cc)
add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC})
IF(WITH_FBR)
SET(FBR_LIBS scan ANN ${OpenCV_LIBS})
add_executable(featurebasedregistration feature_based_registration.cc fbr_global.cc)
target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS})
ENDIF(WITH_FBR)

256
.svn/pristine/49/49302f71941a2340035fed68892f230e95be8063.svn-base Normal file
View file

@ -0,0 +1,256 @@
/**
* @file
* @brief Representation of a 3D point type
* @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany.
*/
#ifndef __POINT_TYPE_H__
#define __POINT_TYPE_H__
#include "point.h"
#include "data_types.h"
#include <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <iostream>
#include <fstream>
#include <string.h>
class Scan;
class PointType {
public:
static const unsigned int USE_NONE;
static const unsigned int USE_REFLECTANCE;
static const unsigned int USE_TEMPERATURE;
static const unsigned int USE_AMPLITUDE;
static const unsigned int USE_DEVIATION;
static const unsigned int USE_HEIGHT;
static const unsigned int USE_TYPE;
static const unsigned int USE_COLOR;
static const unsigned int USE_TIME;
static const unsigned int USE_INDEX;
PointType();
PointType(unsigned int _types);
bool hasReflectance();
bool hasTemperature();
bool hasAmplitude();
bool hasDeviation();
bool hasType();
bool hasColor();
bool hasTime();
bool hasIndex();
unsigned int getReflectance();
unsigned int getTemperature();
unsigned int getAmplitude();
unsigned int getDeviation();
unsigned int getTime();
unsigned int getIndex();
unsigned int getType();
unsigned int getType(unsigned int type);
unsigned int getPointDim();
static PointType deserialize(std::ifstream &f);
void serialize(std::ofstream &f);
unsigned int toFlags() const;
template <class T>
T *createPoint(const Point &P, unsigned int index=0);
template <class T>
Point createPoint(T *p);
//! Aquire DataPointer objects from \a scan, determined by its types
void useScan(Scan* scan);
//! Release the DataPointer objects
void clearScan();
//! Create a point with attributes via the DataPointers from the scan
template<typename T>
T* createPoint(unsigned int i, unsigned int index = 0);
//! Create an array with coordinate+attribute array per point with transfer of ownership
template<typename T>
T** createPointArray(Scan* scan);
private:
/**
* collection of flags
*/
unsigned int types;
/**
* Derived from types: 3 spatial dimensions + 1 for each flag set
**/
unsigned int pointdim;
/**
* Stores the size of each point in bytes
**/
unsigned int pointbytes;
/**
* Derived from types, to map type to the array index for each point
**/
int dimensionmap[9];
bool hasType(unsigned int type);
unsigned int getScanSize(Scan* scan);
DataXYZ* m_xyz;
DataRGB* m_rgb;
DataReflectance* m_reflectance;
DataTemperature* m_temperature;
DataAmplitude* m_amplitude;
DataType* m_type;
DataDeviation* m_deviation;
};
template <class T>
T *PointType::createPoint(const Point &P, unsigned int index ) {
unsigned int counter = 0;
T *p = new T[pointdim];
p[counter++] = P.x;
p[counter++] = P.y;
p[counter++] = P.z;
if (types & USE_REFLECTANCE) {
p[counter++] = P.reflectance;
}
if (types & USE_TEMPERATURE) {
p[counter++] = P.temperature;
}
if (types & USE_AMPLITUDE) {
p[counter++] = P.amplitude;
}
if (types & USE_DEVIATION) {
p[counter++] = P.deviation;
}
if (types & USE_TYPE) {
p[counter++] = P.type;
}
if (types & USE_COLOR) {
memcpy(&p[counter], P.rgb, 3);
counter++;
}
if (types & USE_TIME) {
// p[counter++] = P.timestamp;
}
if (types & USE_INDEX) {
p[counter++] = index;
}
return p;
}
template <class T>
Point PointType::createPoint(T *p) {
Point P;
unsigned int counter = 0;
P.x = p[counter++];
P.y = p[counter++];
P.z = p[counter++];
if (types & USE_REFLECTANCE) {
P.reflectance = p[counter++];
}
if (types & USE_TEMPERATURE) {
P.temperature = p[counter++];
}
if (types & USE_AMPLITUDE) {
P.amplitude = p[counter++];
}
if (types & USE_DEVIATION) {
P.deviation = p[counter++];
}
if (types & USE_TYPE) {
P.type = p[counter++];
}
if (types & USE_COLOR) {
memcpy(P.rgb, &p[counter], 3);
counter++;
}
if (types & USE_TIME) {
// P.timestamp = p[counter++];
}
return P;
}
template <class T>
T *PointType::createPoint(unsigned int i, unsigned int index) {
unsigned int counter = 0;
T* p = new T[pointdim];
for(unsigned int j = 0; j < 3; ++j)
p[counter++] = (*m_xyz)[i][j];
// if a type is requested try to write the value if the scan provided one
if (types & USE_REFLECTANCE) {
p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0);
}
if (types & USE_TEMPERATURE) {
p[counter++] = (m_temperature? (*m_temperature)[i]: 0);
}
if (types & USE_AMPLITUDE) {
p[counter++] = (m_amplitude? (*m_amplitude)[i]: 0);
}
if (types & USE_DEVIATION) {
p[counter++] = (m_deviation? (*m_deviation)[i]: 0);
}
if (types & USE_TYPE) {
p[counter++] = (m_type? (*m_type)[i]: 0);
}
if (types & USE_COLOR) {
if(m_rgb)
memcpy(&p[counter], (*m_rgb)[i], 3);
else
p[counter] = 0;
counter++;
}
if (types & USE_TIME) {
}
if (types & USE_INDEX) {
p[counter++] = index;
}
return p;
}
template<typename T>
T** PointType::createPointArray(Scan* scan)
{
// access data with prefetching
useScan(scan);
// create a float array with requested attributes by pointtype via createPoint
unsigned int nrpts = getScanSize(scan);
T** pts = new T*[nrpts];
for(unsigned int i = 0; i < nrpts; i++) {
pts[i] = createPoint<T>(i);
}
// unlock access to data, remove unneccessary data fields
clearScan();
return pts;
}
#endif

90
.svn/pristine/58/58db7cca2a4b1aa9365d0494071c6923398d845a.svn-base Normal file
View file

@ -0,0 +1,90 @@
/**
* @file
* @brief IO of a 3D scan
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#ifndef __SCAN_IO_H__
#define __SCAN_IO_H__
#include "slam6d/io_types.h"
#include "slam6d/pointfilter.h"
#include <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>* temperature = 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

615
.svn/pristine/5b/5b6818356d295bdacd23f82f454bd7bf0fc53d8c.svn-base Normal file
View file

@ -0,0 +1,615 @@
/*
* scan_io_velodyne implementation
*
* Copyright (C) Mohammad Faisal, Dorit Borrmann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Andreas Nuechter. Jacobs University Bremen gGmbH
* @author Dorit Borrmann. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
* @author Mohammad Faisal. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
*/
#include "slam6d/point.h"
#include "veloslam/velodefs.h"
#include "scanio/scan_io_velodyne.h"
#include "slam6d/globals.icc"
#include <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>* temperature,
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

787
.svn/pristine/5f/5f47049d166655f86d07d17e8437be1aa553b183.svn-base Normal file
View file

@ -0,0 +1,787 @@
/*
* 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_TEMPERATURE) clear("temperature");
if(types & DATA_AMPLITUDE) clear("amplitude");
if(types & DATA_TYPE) clear("type");
if(types & DATA_DEVIATION) clear("deviation");
}
SearchTree* Scan::getSearchTree()
{
// if the search tree hasn't been created yet, calculate everything
if(kd == 0) {
createSearchTree();
}
return kd;
}
void Scan::toGlobal() {
calcReducedPoints();
transform(transMatOrg, INVALID);
}
/**
* Computes a search tree depending on the type.
*/
void Scan::createSearchTree()
{
// multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation
// boost::lock_guard<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;
}

117
.svn/pristine/6b/6b9e1fb3cb0b5d60d3717aca31176336255f00eb.svn-base Normal file
View file

@ -0,0 +1,117 @@
### TOOLS
IF(WITH_TOOLS)
FIND_PACKAGE(OpenCV REQUIRED)
### SCAN_RED
add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc)
IF(UNIX)
target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS})
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_red scan ANN XGetopt)
ENDIF(WIN32)
### SCAN_DIFF
add_executable(scan_diff scan_diff.cc)
# add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc)
IF(UNIX)
target_link_libraries(scan_diff scan dl ANN)
# target_link_libraries(scan_diff2d scan dl ANN)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_diff scan ANN XGetopt)
# target_link_libraries(scan_diff2d scan ANN XGetopt)
ENDIF(WIN32)
add_executable(frame_to_graph frame_to_graph.cc)
add_executable(convergence convergence.cc)
add_executable(graph_balancer graph_balancer.cc)
add_executable(exportPoints exportPoints.cc)
add_executable(frames2riegl frames2riegl.cc)
add_executable(frames2pose frames2pose.cc)
add_executable(pose2frames pose2frames.cc)
add_executable(riegl2frames riegl2frames.cc)
add_executable(toGlobal toGlobal.cc)
IF(UNIX)
target_link_libraries(graph_balancer scan ${Boost_GRAPH_LIBRARY} ${Boost_SERIALIZATION_LIBRARY} ${Boost_REGEX_LIBRARY})
target_link_libraries(exportPoints scan dl ANN)
target_link_libraries(toGlobal scan)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(frame_to_graph XGetopt ${Boost_LIBRARIES})
target_link_libraries(convergence XGetopt ${Boost_LIBRARIES})
target_link_libraries(graph_balancer scan XGetopt ${Boost_LIBRARIES})
target_link_libraries(exportPoints scan ANN XGetopt ${Boost_LIBRARIES})
target_link_libraries(frames2pose XGetopt ${Boost_LIBRARIES})
target_link_libraries(pose2frames XGetopt ${Boost_LIBRARIES})
target_link_libraries(frames2riegl XGetopt ${Boost_LIBRARIES})
target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES})
target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES})
ENDIF(WIN32)
ENDIF(WITH_TOOLS)
### SCANLIB
SET(SCANLIB_SRCS
kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc
graph.cc icp6Dapx.cc icp6D.cc icp6Dsvd.cc
icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc
icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc
ghelix6DQ2.cc gapx6D.cc graphToro.cc ann_kd.cc
graphHOG-Man.cc elch6D.cc elch6Dquat.cc elch6DunitQuat.cc
elch6Dslerp.cc elch6Deuler.cc loopToro.cc loopHOG-Man.cc
point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc
allocator.cc
scan.cc basicScan.cc managedScan.cc metaScan.cc
io_types.cc io_utils.cc pointfilter.cc
)
if(WITH_METRICS)
set(SCANLIB_SRCS ${SCANLIB_SRCS} metrics.cc)
endif(WITH_METRICS)
add_library(scan STATIC ${SCANLIB_SRCS})
target_link_libraries(scan scanclient scanio)
IF(UNIX)
target_link_libraries(scan dl)
ENDIF(UNIX)
### EXPORT SHARED LIBS
IF(EXPORT_SHARED_LIBS)
add_library(scan_s SHARED ${SCANLIB_SRCS})
#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat)
ENDIF(EXPORT_SHARED_LIBS)
### SLAM6D
IF(WITH_CUDA)
CUDA_COMPILE(CUDA_FILES cuda/CIcpGpuCuda.cu )
add_executable(slam6D slam6D.cc cuda/icp6Dcuda.cc ${CUDA_FILES})
target_link_libraries(slam6D ${CUDA_LIBRARIES} ANN cudpp64)
CUDA_ADD_CUBLAS_TO_TARGET(slam6D)
CUDA_ADD_CUTIL_TO_TARGET(slam6D)
ELSE(WITH_CUDA)
add_executable(slam6D slam6D.cc)
ENDIF(WITH_CUDA)
IF(UNIX)
target_link_libraries(slam6D scan newmat sparse ANN)
ENDIF(UNIX)
IF(WIN32)
target_link_libraries(slam6D scan newmat sparse ANN XGetopt ${Boost_LIBRARIES})
ENDIF(WIN32)
#IF(MSVC)
# INSTALL(TARGETS slam6D RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/windows)
#ENDIF(MSVC)

226
.svn/pristine/6d/6d4722e02f91538af3c17843aa935f7ecd44c693.svn-base Normal file
View file

@ -0,0 +1,226 @@
/**
* @file
* @brief Basic DataPointer class and its derivates SingleArray and TripleArray
*
* This file contains several classes for array-like access. The SingleArray
* and TripleArray classes and their typedefs to DataXYZ/... overload
* the operator[] and have a size function to act as their native arrays.
* Similar to the array classes, SingleObject represents a whole object with
* all its members in that allocated space.
*
* If an array of pointers to the elements of a TripleArray is required it can
* create a temporary class PointerArray which holds creates and deletes a
* native pointer array and follows the RAII-pattern.
*/
#ifndef DATA_TYPES_H
#define DATA_TYPES_H
/**
* Representation of a pointer to a data field with no access methods.
*
* Type-specialized access is gained by deriving from this class and
* implementing access functions like operator[] and size().
*
* The PrivateImplementation feature enables RAII-type locking mechanisms
* used in the scanserver for holding CacheObject-locks. It is protected so
* that scanserver-unaware code can only construct this class with a pointer
* and size. Initialization of a derived class with these locking mechanisms
* creates this class with the private implementation value, which will be
* deleted in this dtor when it completely falls out of scope.
*/
class DataPointer {
protected:
//! Subclass for storing further members and attaching an overloadable dtor
class PrivateImplementation {
public:
virtual ~PrivateImplementation() {}
};
public:
// DataPointer& operator=(const DataPointer&) = delete;
// DataPointer(const DataPointer&) = delete;
/**
* Ctor for the initial creation
*
* @param pointer base pointer to the data
* @param size of the pointed data in bytes
*/
DataPointer(unsigned char* pointer, unsigned int size,
PrivateImplementation* private_impl = 0) :
m_pointer(pointer), m_size(size), m_private_impl(private_impl) {
}
/**
* Copy-Ctor for passing along via return by value
*
* The type-specialized classes (B) will be called with their
* B(DataPointer&&) temporary ctor and call this constructor, so the private
* imlementation has to be taken away. The temporary inside that constructor
* isn't seen as temporary anymore, so we need a simple reference-ctor.
*/
DataPointer(DataPointer& other) {
m_pointer = other.m_pointer;
m_size = other.m_size;
// take ownership of this value, other is a temporary and will deconstruct
m_private_impl = other.m_private_impl;
other.m_private_impl = 0;
};
/**
* Same as DataPointer(DataPointer&), except this is for functions returning
* DataPointer instead of derived classes, so the temporary-ctor is used.
*/
DataPointer(DataPointer&& other) {
m_pointer = other.m_pointer;
m_size = other.m_size;
// take ownership of this value, other is a temporary and will deconstruct
m_private_impl = other.m_private_impl;
other.m_private_impl = 0;
}
//! Delete the private implementation with its derived dtor
~DataPointer() {
if(m_private_impl != 0)
delete m_private_impl;
}
//! Indicator for nullpointer / no data contained if false
inline bool valid() {
return m_size != 0;
}
inline unsigned char* get_raw_pointer() const { return m_pointer; }
protected:
unsigned char* m_pointer;
unsigned int m_size;
private:
PrivateImplementation* m_private_impl;
};
template<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> DataTemperature;
typedef SingleArray<float> DataAmplitude;
typedef SingleArray<int> DataType;
typedef SingleArray<float> DataDeviation;
#endif //DATA_TYPES_H

97
.svn/pristine/6d/6d7767e09cde7fcee4cc7a84392da17afd831084.svn-base Normal file
View file

@ -0,0 +1,97 @@
/**
* @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> DataTemperature;
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

27
.svn/pristine/78/78fdca4abe08e977169de18e7ab1794a388fcf80.svn-base Normal file
View file

@ -0,0 +1,27 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_KS_H__
#define __SCAN_IO_KS_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for KS scans
*
* The compiled class is available as shared object file
*/
class ScanIO_ks : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

208
.svn/pristine/79/798da527fce49381e8e2e8700cfac5604f0577c0.svn-base Normal file
View file

@ -0,0 +1,208 @@
/*
* scan_io_rts implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_rts.h"
#include <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>* temperature, 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

82
.svn/pristine/7d/7dfbf49a9d26347c51d0395601c913fc47033666.svn-base Normal file
View file

@ -0,0 +1,82 @@
/**
* @file panorama.h
* @brife create panorama images from 3D scans.
* This class is a panorama image container with different projections.
* It creats panoramic images with specified resolutions.
* @author HamidReza Houshiar. Jacobs University Bremen gGmbH, Germany.
* @date Date: 2012/05/23 2:00
*/
#ifndef PANORAMA_H_
#define PANORAMA_H_
#include "fbr_global.h"
using namespace std;
namespace fbr{
/**
* @class panorama : create panorama images with different projection methods from input scan files(Mat from scan_cv class) in opencv Mat format
* @param iReflectance panorama image from reflectance data
* @param iRange panorama image from range data
* @param iMap panorama map of 3D cartesian coordinate of input scan(same points as iRange and iReflectance)
* @param extendedIMap 3D vector as panorama map with all the points
* @param iWidth width of the panorama image (cols in opencv)
* @param iHeight height of panorama image (rows in opencv)
* @param pMethod projection method for panorama creation
* @param nImage number of images per scan specially for Rectilinear, Pannini and Stereographic projections
* @param pParam special d parameter of Pannini projection (Master Thesis for more info) or special R parameter of Stereographic projection (Master Thesis for more info)
*/
class panorama{
cv::Mat iReflectance;
cv::Mat iMap;
cv::Mat iRange;
vector<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);
/**
* @brief recovers the point cloud from the panorama image and range information
* @param image - input range image to be converted to point cloud
* @param file - destination of .3d file containing the point cloud
*/
void recoverPointCloud(const cv::Mat& range_image, const std::string& file);
unsigned int getImageWidth();
unsigned int getImageHeight();
projection_method getProjectionMethod();
unsigned int getNumberOfImages();
double getProjectionParam();
cv::Mat getReflectanceImage();
cv::Mat getMap();
cv::Mat getRangeImage();
vector<vector<vector<cv::Vec3f> > > getExtendedMap();
panorama_map_method getMapMethod();
void getDescription();
};
}
#endif /* PANORAMA_H_ */

2047
.svn/pristine/7e/7e83972a871ef8774dfcdd91945515d308e06b77.svn-base Normal file
View file

File diff suppressed because it is too large Load diff

405
.svn/pristine/7f/7f316f2c49e978d7de57d0e7dec3d98009b24e38.svn-base Normal file
View file

@ -0,0 +1,405 @@
/*
* basicScan implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
#include "slam6d/basicScan.h"
#include "scanio/scan_io.h"
#include "slam6d/kd.h"
#include "slam6d/Boctree.h"
#include "slam6d/ann_kd.h"
#ifdef WITH_METRICS
#include "slam6d/metrics.h"
#endif //WITH_METRICS
#include <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> temperature;
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,
&temperature,
&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_TEMPERATURE && !temperature.empty()) {
float* data = reinterpret_cast<float*>(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer());
for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i];
}
if(types & DATA_AMPLITUDE && !amplitude.empty()) {
int* data = reinterpret_cast<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 == "temperature") get(DATA_TEMPERATURE); else
if(identifier == "amplitude") get(DATA_AMPLITUDE); else
if(identifier == "type") get(DATA_TYPE); else
if(identifier == "deviation") get(DATA_DEVIATION); else
// reduce on demand
if(identifier == "xyz reduced") calcReducedOnDemand(); else
if(identifier == "xyz reduced original") calcReducedOnDemand(); else
// show requests reduced points, manipulate in showing the same entry
if(identifier == "xyz reduced show") {
calcReducedOnDemand();
m_data["xyz reduced show"] = m_data["xyz reduced"];
} else
if(identifier == "octtree") {
createOcttree();
}
it = m_data.find(identifier);
}
// if nothing can be loaded, return an empty pointer
if(it == m_data.end())
return DataPointer(0, 0);
else
return DataPointer(it->second.first, it->second.second);
}
DataPointer BasicScan::create(const std::string& identifier, unsigned int size)
{
map<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));
}

178
.svn/pristine/80/804aef16c807604ac0a4c1ec56fbeb52cc2f8d10.svn-base Normal file
View file

@ -0,0 +1,178 @@
/*
* scan_io_uos implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "scanio/scan_io_uos.h"
#include <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>* temperature, 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

210
.svn/pristine/81/819fd950bf1502a5f4dd46137719b6f40802aa18.svn-base Normal file
View file

@ -0,0 +1,210 @@
/*
* scan_io_ks_rgb implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "scanio/scan_io_ks_rgb.h"
#include <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>* temperature, 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

29
.svn/pristine/84/84cb681f916fed5a1d04b8bcad34e3e4b9942627.svn-base Normal file
View file

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

27
.svn/pristine/85/856df877a4996d8a6edc6f8d285145db631721ae.svn-base Normal file
View file

@ -0,0 +1,27 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_KS_RGB_H__
#define __SCAN_IO_KS_RGB_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for KS RGB scans
*
* The compiled class is available as shared object file
*/
class ScanIO_ks_rgb : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

754
.svn/pristine/85/858b68b55776fa6aa3f3a25444e5303b2c280145.svn-base Normal file
View file

@ -0,0 +1,754 @@
/*
* panorama implementation
*
* Copyright (C) HamidReza Houshiar
*
* Released under the GPL version 3.
*
*/
#include "slam6d/fbr/panorama.h"
using namespace std;
namespace fbr{
void panorama::init(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){
iWidth = width;
iHeight = height;
pMethod = method;
nImages = numberOfImages;
pParam = param;
if(mMethod == FARTHEST){
iMap.create(iHeight, iWidth, CV_32FC(3));
iMap = cv::Scalar::all(0);
}
else if(mMethod == EXTENDED){
extendedIMap.resize(iHeight);
for (unsigned int i = 0; i < iHeight; i++)
extendedIMap[i].resize(iWidth);
}
iReflectance.create(iHeight, iWidth, CV_8U);
iReflectance = cv::Scalar::all(0);
iRange.create(iHeight, iWidth, CV_32FC(1));
iRange = cv::Scalar::all(0);
mapMethod = mMethod;
}
panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param, panorama_map_method mMethod){
init(width, height, method, numberOfImages, param, mMethod);
}
panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages, double param){
init(width, height, method, numberOfImages, param, FARTHEST);
}
panorama::panorama(unsigned int width, unsigned int height, projection_method method, unsigned int numberOfImages){
double param = 0;
if(method == PANNINI)
param = 1;
else if (method == STEREOGRAPHIC)
param = 2;
init(width, height, method, numberOfImages, param, FARTHEST);
}
panorama::panorama(unsigned int width, unsigned int height, projection_method method){
double param = 0;
unsigned int numberOfImages = 1;
if(method == RECTILINEAR)
numberOfImages = 3;
else if(method == PANNINI){
numberOfImages = 3;
param = 1;
} else if (method == STEREOGRAPHIC){
numberOfImages = 3;
param = 2;
}
init(width, height, method, numberOfImages, param, FARTHEST);
}
void panorama::map(int x, int y, cv::MatIterator_<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);
}
}
}
void panorama::recoverPointCloud(const cv::Mat& range_image, const string& file ) {
std::ofstream scan_file (file.c_str());
//recover from EQUIRECTANGULAR projection
if(pMethod == EQUIRECTANGULAR) {
double xFactor = (double) range_image.size().width / 2 / M_PI;
//int widthMax = range_image.size().width - 1;
double yFactor = (double) range_image.size().height / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI);
double heightLow = (0 - MIN_ANGLE) / 360 * 2 * M_PI;
int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float theta = (heightMax - row) / yFactor - heightLow;
float phi = col / xFactor;
phi *= 180.0 / M_PI;
phi = 360.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
//recover from CYLINDRICAL projection
if(pMethod == CYLINDRICAL) {
double xFactor = (double) range_image.size().width / 2 / M_PI;
//int widthMax = range_image.size().width - 1;
double yFactor = (double) range_image.size().height / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI));
double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI;
//int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float theta = atan2(row + yFactor * tan(heightLow), yFactor);
float phi = col / xFactor;
phi *= 180.0 / M_PI;
phi = 360.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
//recover from MERCATOR projection
if(pMethod == MERCATOR) {
double xFactor = (double) range_image.size().width / 2 / M_PI;
double yFactor = (double) range_image.size().height / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) );
double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI)));
int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float theta = 2 * atan2(exp((heightMax - row) / yFactor + heightLow), 1.) - M_PI_2;
float phi = col / xFactor;
phi *= 180.0 / M_PI;
phi = 180.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
//recover from CONIC projection
if(pMethod == CONIC) {
// set up maximum latitude and longitude angles of the robot
double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0,
MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI;
// set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html
double Lat0 = 0., Long0 = 0.;
double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0;
double n = (sin(Phi1) + sin(Phi2)) / 2.;
double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1);
double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n;
// set up max values for x and y and add the longitude to x axis and latitude to y axis
double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0));
double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0));
double xFactor = (double) range_image.size().width / ( xmax - xmin );
double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 ));
double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 ));
double yFactor = (double) range_image.size().height / ( ymax - ymin );
int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float x = col * 1. / xFactor - fabs(xmin);
float y = (heightMax - row) * 1. / yFactor - fabs(ymin);
float theta = asin((C - (x*x + (Rho0 - y) * (Rho0 - y)) * n * n) / (2 * n));
float phi = Long0 + (1./n) * atan2(x, Rho0 - y);
phi *= 180.0 / M_PI;
phi = 360.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
//if ( std::isnan(cartesian[0]) || std::isnan(cartesian[1]) || std::isnan(cartesian[2]) ) continue;
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
scan_file.close();
}
unsigned int panorama::getImageWidth(){
return iWidth;
}
unsigned int panorama::getImageHeight(){
return iHeight;
}
projection_method panorama::getProjectionMethod(){
return pMethod;
}
unsigned int panorama::getNumberOfImages(){
return nImages;
}
double panorama::getProjectionParam(){
return pParam;
}
cv::Mat panorama::getReflectanceImage(){
return iReflectance;
}
cv::Mat panorama::getMap(){
return iMap;
}
cv::Mat panorama::getRangeImage(){
return iRange;
}
vector<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;
}
}

154
.svn/pristine/88/884ca405924cd5b55bbc514b37378486cb476353.svn-base Normal file
View file

@ -0,0 +1,154 @@
/*
* io_tpyes implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
#include "slam6d/io_types.h"
#include "slam6d/globals.icc"
#ifdef _MSC_VER
//#include <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, "uos_rrgbt") == 0) return UOS_RRGBT;
else if (strcasecmp(string, "uosr") == 0) return UOSR;
else if (strcasecmp(string, "old") == 0) return OLD;
else if (strcasecmp(string, "rts") == 0) return RTS;
else if (strcasecmp(string, "rts_map") == 0) return RTS_MAP;
else if (strcasecmp(string, "ifp") == 0) return IFP;
else if (strcasecmp(string, "riegl_txt") == 0) return RIEGL_TXT;
else if (strcasecmp(string, "riegl_project") == 0) return RIEGL_PROJECT;
else if (strcasecmp(string, "riegl_rgb") == 0) return RIEGL_RGB;
else if (strcasecmp(string, "riegl_bin") == 0) return RIEGL_BIN;
else if (strcasecmp(string, "zahn") == 0) return ZAHN;
else if (strcasecmp(string, "ply") == 0) return PLY;
else if (strcasecmp(string, "wrl") == 0) return WRL;
else if (strcasecmp(string, "xyz") == 0) return XYZ;
else if (strcasecmp(string, "zuf") == 0) return ZUF;
else if (strcasecmp(string, "asc") == 0) return ASC;
else if (strcasecmp(string, "iais") == 0) return IAIS;
else if (strcasecmp(string, "front") == 0) return FRONT;
else if (strcasecmp(string, "x3d") == 0) return X3D;
else if (strcasecmp(string, "rxp") == 0) return RXP;
else if (strcasecmp(string, "ais") == 0) return AIS;
else if (strcasecmp(string, "oct") == 0) return OCT;
else if (strcasecmp(string, "txyzr") == 0) return TXYZR;
else if (strcasecmp(string, "xyzr") == 0) return XYZR;
else if (strcasecmp(string, "xyz_rgb") == 0) return XYZ_RGB;
else if (strcasecmp(string, "ks") == 0) return KS;
else if (strcasecmp(string, "ks_rgb") == 0) return KS_RGB;
else if (strcasecmp(string, "stl") == 0) return STL;
else if (strcasecmp(string, "leica") == 0) return LEICA;
else if (strcasecmp(string, "pcl") == 0) return PCL;
else if (strcasecmp(string, "pci") == 0) return PCI;
else if (strcasecmp(string, "cad") == 0) return UOS_CAD;
else if (strcasecmp(string, "velodyne") == 0) return VELODYNE;
else if (strcasecmp(string, "velodyne_frames") == 0) return VELODYNE_FRAMES;
else throw std::runtime_error(std::string("Io type ") + string + std::string(" is unknown"));
}
const char * io_type_to_libname(IOType type)
{
switch (type) {
case UOS:
return "scan_io_uos";
case UOSR:
return "scan_io_uosr";
case UOS_MAP:
return "scan_io_uos_map";
case UOS_FRAMES:
return "scan_io_uos_frames";
case UOS_MAP_FRAMES:
return "scan_io_uos_map_frames";
case UOS_RGB:
return "scan_io_uos_rgb";
case UOS_RRGBT:
return "scan_io_uos_rrgbt";
case OLD:
return "scan_io_old";
case RTS:
return "scan_io_rts";
case RTS_MAP:
return "scan_io_rts_map";
case IFP:
return "scan_io_ifp";
case RIEGL_TXT:
return "scan_io_riegl_txt";
case RIEGL_PROJECT:
return "scan_io_riegl_project";
case RIEGL_RGB:
return "scan_io_riegl_rgb";
case RIEGL_BIN:
return "scan_io_riegl_bin";
case ZAHN:
return "scan_io_zahn";
case PLY:
return "scan_io_ply";
case WRL:
return "scan_io_wrl";
case XYZ:
return "scan_io_xyz";
case ZUF:
return "scan_io_zuf";
case ASC:
return "scan_io_asc";
case IAIS:
return "scan_io_iais";
case FRONT:
return "scan_io_front";
case X3D:
return "scan_io_x3d";
case RXP:
return "scan_io_rxp";
case AIS:
return "scan_io_ais";
case OCT:
return "scan_io_oct";
case TXYZR:
return "scan_io_txyzr";
case XYZR:
return "scan_io_xyzr";
case XYZ_RGB:
return "scan_io_xyz_rgb";
case KS:
return "scan_io_ks";
case KS_RGB:
return "scan_io_ks_rgb";
case STL:
return "stl";
case LEICA:
return "leica_txt";
case PCL:
return "pcl";
case PCI:
return "pci";
case UOS_CAD:
return "cad";
case VELODYNE:
return "scan_io_velodyne";
case VELODYNE_FRAMES:
return "scan_io_velodyne_frames";
default:
throw std::runtime_error(std::string("Io type ") + to_string(type) + std::string(" could not be matched to a library name"));
}
}

1165
.svn/pristine/89/89d59fde8504d5c4792e36cc8576ff3cded2ea7a.svn-base Normal file
View file

File diff suppressed because it is too large Load diff

83
.svn/pristine/8e/8e9f9f6ff76474503ac8cca8bcc2d666997014f8.svn-base Normal file
View file

@ -0,0 +1,83 @@
/**
* @file
* @brief IO of a 3D scan in rxp file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_RXP_H__
#define __SCAN_IO_RXP_H__
#include "scan_io.h"
#include "slam6d/point.h"
#include "riegl/scanlib.hpp"
using namespace scanlib;
using namespace std;
using namespace std::tr1;
class importer;
/**
* @brief 3D scan loader for RXP scans
*
* The compiled class is available as shared object file
*/
class ScanIO_rxp : public ScanIO {
public:
virtual std::list<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>* temperature, 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

282
.svn/pristine/96/965bbbc5f2364d7184365a9d7f60a841a452e99c.svn-base Normal file
View file

@ -0,0 +1,282 @@
/*
* 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>();
}
DataTemperature SharedScan::getTemperature() {
return m_temperature->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);
}

410
.svn/pristine/97/97e66ffc29fe809cd755cf257a825a6166e8f334.svn-base Normal file
View file

@ -0,0 +1,410 @@
cmake_minimum_required (VERSION 2.6.1)
SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH})
project (Slam6D)
#include_directories(OPENGL_INCLUDE_DIR)
IF(WIN32)
set(Boost_USE_STATIC_LIBS TRUE)
ELSE(WIN32)
set(Boost_USE_STATIC_LIBS FALSE)
ENDIF(WIN32)
SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49")
IF(WIN32)
# for some unknown reason no one variant works on all windows platforms
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
ELSE(WIN32)
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
ENDIF(WIN32)
if(Boost_FOUND)
link_directories(${BOOST_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIRS})
add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
endif()
#################################################
# Declare Options and modify build accordingly ##
#################################################
## FreeGLUT
OPTION(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON)
IF(WITH_FREEGLUT)
MESSAGE(STATUS "With freeglut")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT")
ELSE(WITH_FREEGLUT)
MESSAGE(STATUS "Without freeglut")
ENDIF(WITH_FREEGLUT)
## Show
OPTION(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON)
IF(WITH_SHOW)
FIND_PACKAGE(OpenGL REQUIRED)
FIND_PACKAGE(GLUT REQUIRED)
MESSAGE(STATUS "With show")
ELSE(WITH_SHOW)
# SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE)
MESSAGE(STATUS "Without show")
ENDIF(WITH_SHOW)
## WXShow
OPTION(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF)
IF(WITH_WXSHOW)
FIND_PACKAGE(OpenGL REQUIRED)
FIND_PACKAGE(GLUT REQUIRED)
find_package(wxWidgets COMPONENTS core base gl REQUIRED)
# set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration
SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE)
# wxWidgets include (this will do all the magic to configure everything)
include( ${wxWidgets_USE_FILE})
MESSAGE(STATUS "With wxshow")
ELSE(WITH_XWSHOW)
MESSAGE(STATUS "Without wxshow")
ENDIF(WITH_WXSHOW)
## Shapes
OPTION(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF)
IF(WITH_SHAPE_DETECTION)
MESSAGE(STATUS "With shape detection")
ELSE(WITH_SHAPE_DETECTION)
MESSAGE(STATUS "Without shape detection")
ENDIF(WITH_SHAPE_DETECTION)
## Interior reconstruction
option(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF)
if(WITH_MODEL)
message(STATUS "With interior reconstruction")
else(WITH_MODEL)
message(STATUS "Without interior reconstruction")
endif(WITH_MODEL)
## Thermo
OPTION(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF)
IF(WITH_THERMO)
FIND_PACKAGE(OpenCV REQUIRED)
add_subdirectory(3rdparty/cvblob)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
MESSAGE(STATUS "With thermo")
ELSE(WITH_THERMO)
MESSAGE(STATUS "Without thermo")
ENDIF(WITH_THERMO)
## Octree
OPTION(WITH_OCTREE_DISPLAY "Whether to use octree display for efficiently culling scans ON/OFF" ON)
IF(WITH_OCTREE_DISPLAY)
MESSAGE(STATUS "Using octree display")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_GL_POINTS")
ELSE(WITH_OCTREE_DISPLAY)
MESSAGE(STATUS "Using displaylists: Warning potentially much slower")
ENDIF(WITH_OCTREE_DISPLAY)
#SET (WITH_OCTREE_DISPLAY ${WITH_OCTREE_DISPLAY} CACHE BOOL
#"Whether to use octree display for efficiently culling scans ON/OFF" FORCE)
## Octree
OPTION(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF)
IF(WITH_COMPACT_OCTREE)
MESSAGE(STATUS "Using compact octrees")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE")
ELSE(WITH_COMPACT_OCTREE)
MESSAGE(STATUS "Not using compact octreees: Warning uses more memory")
ENDIF(WITH_COMPACT_OCTREE)
## Glee?
OPTION(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF)
IF(WITH_GLEE)
MESSAGE(STATUS "Using opengl extensions")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE")
ELSE(WITH_GLEE)
MESSAGE(STATUS "Not using opengl extensions")
ENDIF(WITH_GLEE)
## Gridder
OPTION(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF)
IF(WITH_GRIDDER)
MESSAGE(STATUS "With 2DGridder")
ELSE(WITH_GRIDDER)
MESSAGE(STATUS "Without 2DGridder")
ENDIF(WITH_GRIDDER)
## Dynamic VELOSLAM
OPTION(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF)
IF(WITH_VELOSLAM)
MESSAGE(STATUS "With VELOSLAM")
ELSE(WITH_VELOSLAM)
MESSAGE(STATUS "Without VELOSLAM")
ENDIF(WITH_VELOSLAM)
## Home-made Laserscanner
OPTION(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF)
IF(WITH_DAVID_3D_SCANNER)
MESSAGE(STATUS "With David scanner")
ELSE(WITH_DAVID_3D_SCANNER)
MESSAGE(STATUS "Without David scanner")
ENDIF(WITH_DAVID_3D_SCANNER)
## Tools
OPTION(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF)
IF(WITH_TOOLS)
MESSAGE(STATUS "With Tools")
ELSE(WITH_TOOLS)
MESSAGE(STATUS "Without Tools")
ENDIF(WITH_TOOLS)
## CAD matching
OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)
IF (WITH_CAD)
MESSAGE (STATUS "With CAD import")
find_package (Boost COMPONENTS program_options filesystem REQUIRED)
ELSE (WITH_CAD)
MESSAGE (STATUS "Without CAD import")
ENDIF (WITH_CAD)
## RivLib
OPTION(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF)
IF(WITH_RIVLIB)
MESSAGE(STATUS "Compiling a scan IO for RXP files")
include_directories(${CMAKE_SOURCE_DIR}/3rdparty)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty)
SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/)
IF(WIN32)
SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib)
ELSE(WIN32)
SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread)
ENDIF(WIN32)
FIND_PACKAGE(LibXml2 )
ELSE(WITH_RIVLIB)
MESSAGE(STATUS "Do NOT compile a scan IO for RXP")
ENDIF(WITH_RIVLIB)
## CUDA support, TODO depend on CUDA_FIND
OPTION(WITH_CUDA "Compile with CUDA support" OFF)
IF(WITH_CUDA)
MESSAGE(STATUS "Compiling WITH CUDA support")
FIND_PACKAGE(CUDA)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA")
ELSE(WITH_CUDA)
MESSAGE(STATUS "Compiling WITHOUT CUDA support")
ENDIF(WITH_CUDA)
## PMD
OPTION(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF)
IF(WITH_PMD)
FIND_PACKAGE(OpenGL REQUIRED)
MESSAGE(STATUS "With Tools")
ELSE(WITH_PMD)
MESSAGE(STATUS "Without Tools")
ENDIF(WITH_PMD)
## FBR
OPTION(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF)
IF(WITH_FBR)
MESSAGE(STATUS "With FBR ")
ELSE(WITH_FBR)
MESSAGE(STATUS "Without FBR")
ENDIF(WITH_FBR)
## Special treatment for system specifics
IF(APPLE)
add_definitions(-Dfopen64=fopen)
ENDIF(APPLE)
## Multiple Cores
IF(APPLE)
SET(PROCESSOR_COUNT 2)
ELSE(APPLE)
INCLUDE(CountProcessors)
SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" )
ENDIF(APPLE)
# OPEN
FIND_PACKAGE(OpenMP)
IF(OPENMP_FOUND)
OPTION(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON)
ENDIF(OPENMP_FOUND)
IF(OPENMP_FOUND AND WITH_OPENMP)
MESSAGE(STATUS "With OpenMP ")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${PROCESSOR_COUNT} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP")
ELSE(OPENMP_FOUND AND WITH_OPENMP)
MESSAGE(STATUS "Without OpenMP")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1")
ENDIF(OPENMP_FOUND AND WITH_OPENMP)
## TORO
OPTION(WITH_TORO "Whether to use TORO. ON/OFF" OFF)
IF(WITH_TORO)
IF(WIN32)
SET(Subversion_SVN_EXECUTABLE "svn.exe")
ENDIF(WIN32)
cmake_minimum_required (VERSION 2.8)
include(ExternalProject)
ExternalProject_Add(toro3d
SVN_REPOSITORY https://www.openslam.org/data/svn/toro/trunk
SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/toro"
CONFIGURE_COMMAND ""
BUILD_COMMAND make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/toro/toro3d ${CMAKE_SOURCE_DIR}/bin/
)
MESSAGE(STATUS "With TORO ")
ELSE(WITH_TORO)
MESSAGE(STATUS "Without TORO")
ENDIF(WITH_TORO)
## HOGMAN
OPTION(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF)
IF(WITH_HOGMAN)
# dependant on libqt4-devi
find_package( Qt4 REQUIRED)
# CMake of earlier versions do not have external project capabilities
cmake_minimum_required (VERSION 2.8)
include(ExternalProject)
ExternalProject_Add(hogman
SVN_REPOSITORY https://svn.openslam.org/data/svn/hog-man/trunk
SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/hogman"
CONFIGURE_COMMAND <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)

391
.svn/pristine/99/991ffadeaeba8b7efaaf67cabfbbe66e943705cf.svn-base Normal file
View file

@ -0,0 +1,391 @@
/*
* scan_diff implementation
*
* Copyright (C) Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main program calculating difference of 3D scans.
*
* Program for calculating the difference between scans after matching with slam6d
* Usage: bin/scan_diff -s <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 <vector>
#include "slam6d/globals.icc"
#include "slam6d/io_utils.h"
#include "slam6d/scan.h"
#include "scanserver/clientInterface.h"
#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
<< bold << " -S, --scanserver" << normal << endl
<< " Use the scanserver as an input method and handling of scan data" << endl
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " -m 500 -d 5 dat" << endl
<< " " << prog << " --max=5000 -d 10.2 dat" << endl
<< endl;
exit(1);
}
/** A function that parses the command-line arguments and sets the respective flags.
* @param argc the number of arguments
* @param argv the arguments
* @param dir the directory
* @param start first scan number 'start'
* @param end last scan number 'end'
* @param maxDist - maximal distance of points being loaded
* @param minDist - minimal distance of points being loaded
* @param dist the maximal distance for a point pair
* @param type the scan format
* @param desc true if start is greater than end
* @return 0, if the parsing was successful. 1 otherwise
*/
int parseArgs(int argc, char **argv, string &dir,
int &start, int &end, int &maxDist, int &minDist, double &dist,
IOType &type, bool &desc, bool scanserver)
{
int c;
// from unistd.h:
extern char *optarg;
extern int optind;
WriteOnce<IOType> w_type(type);
WriteOnce<int> w_start(start), w_end(end);
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "format", required_argument, 0, 'f' },
{ "max", required_argument, 0, 'm' },
{ "min", required_argument, 0, 'M' },
{ "start", required_argument, 0, 's' },
{ "end", required_argument, 0, 'e' },
{ "dist", required_argument, 0, 'd' },
{ "scanserver", no_argument, 0, 'S' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "f:d:s:e:m:M:", longopts, NULL)) != -1)
switch (c)
{
case 'd':
dist = atof(optarg);
break;
case 's':
w_start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'e':
w_end = atoi(optarg);
if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
break;
case 'f':
try {
type = formatname_to_io_type(optarg);
} catch (...) { // runtime_error
cerr << "Format " << optarg << " unknown." << endl;
abort();
}
break;
case 'm':
maxDist = atoi(optarg);
break;
case 'M':
minDist = atoi(optarg);
break;
case '?':
usage(argv[0]);
return 1;
case 'S':
scanserver = true;
break;
default:
abort ();
}
if(start < 0 || end < 0 || start == end) {
cerr << "\n*** You need two different scans for difference calculations ***" << endl;
usage(argv[0]);
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
usage(argv[0]);
}
dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
if(start > end) {
double tmp = start;
start = end;
end = tmp;
desc = true;
}
parseFormatFile(dir, w_type, w_start, w_end);
return 0;
}
/**
* Main program for calculating the difference of two scans.
* Usage: bin/scan_diff -d <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, 2012" << endl << endl;
if (argc <= 1) {
usage(argv[0]);
}
// parsing the command line parameters
// init, default values if not specified
string dir;
double dist = 0;
int start = -1, end = -1;
int maxDist = -1;
int minDist = -1;
IOType type = RIEGL_TXT;
bool desc = false;
bool scanserver = false;
parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc, scanserver);
if (scanserver) {
try {
ClientInterface::create();
} catch(std::runtime_error& e) {
cerr << "ClientInterface could not be created: " << e.what() << endl;
cerr << "Start the scanserver first." << endl;
exit(-1);
}
}
string diffdir = dir + "diff";
#ifdef _MSC_VER
int success = mkdir(diffdir.c_str());
#else
int success = mkdir(diffdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing scans to " << diffdir << endl;
} else if(errno == EEXIST) {
cout << "Directory " << diffdir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << diffdir << " failed" << endl;
exit(1);
}
string scanFileName;
string framesFileName;
ifstream frames_in;
double inMatrix0[17];
double inMatrix1[17];
cout << "Reading Scan No. " << start << endl;
framesFileName = dir + "scan" + to_string(start,3) + ".frames";
frames_in.open(framesFileName.c_str());
if(!frames_in.good()) {
cerr << "Couldn't read frames " << end << endl;
exit(1);
}
while(frames_in.good()) {
for (unsigned int i = 0; i < 17; frames_in >> inMatrix0[i++]);
}
frames_in.close();
frames_in.clear();
for(int i = 0; i < 16; i++) {
cout << inMatrix0[i] << " ";
}
cout << endl;
cout << "Reading Scan No. " << end << endl;
framesFileName = dir + "scan" + to_string(end,3) + ".frames";
frames_in.open(framesFileName.c_str());
if(!frames_in.good()) {
cerr << "Couldn't read frames " << end << endl;
exit(1);
}
while(frames_in.good()) {
for (unsigned int i = 0; i < 17; frames_in >> inMatrix1[i++]);
}
frames_in.close();
frames_in.clear();
for(int i = 0; i < 16; i++) {
cout << inMatrix1[i] << " ";
}
cout << endl;
Scan::openDirectory(scanserver, dir, type, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
Scan* scan_first = *(Scan::allScans.begin());
Scan* scan_second = *(Scan::allScans.end()-1);
scan_first->transform(inMatrix0, Scan::INVALID);
scan_second->transform(inMatrix1, Scan::INVALID);
DataXYZ xyz_first(scan_first->get("xyz"));
DataXYZ xyz_second(scan_second->get("xyz"));
cout << "Scan " << scan_first->getIdentifier() << " with " << xyz_first.size() << " points" << endl;
cout << "Scan " << scan_second->getIdentifier() << " with " << xyz_second.size() << " points" << endl;
int thread_num = 0;
std::vector<double*> diff;
double transMat[16];
if(desc) {
Scan::getNoPairsSimple(diff, scan_second, scan_first, thread_num, dist);
M4inv(inMatrix1, transMat);
scanFileName = dir + "diff/scan" + to_string(end,3) + ".3d";
} else {
Scan::getNoPairsSimple(diff, scan_first, scan_second, thread_num, dist);
M4inv(inMatrix0, transMat);
scanFileName = dir + "diff/scan" + to_string(start,3) + ".3d";
}
cout << endl;
for(int i = 0; i < 16; i++) {
cout << transMat[i] << " ";
}
cout << endl;
for(int i = 0; i < 16; i++) {
cout << inMatrix0[i] << " ";
}
cout << endl;
ofstream ptsout(scanFileName.c_str());
for(unsigned int i = 0; i < diff.size(); i++) {
Point p(diff[i]);
p.transform(transMat);
ptsout << p.x << " " << p.y << " " << p.z << " " << endl;
}
ptsout.close();
ptsout.clear();
if (scanserver) {
scan_first->clear("xyz");
scan_second->clear("xyz");
}
cout << endl << endl;
cout << "Normal program end." << endl << endl;
if (scanserver) {
Scan::closeDirectory();
}
return 0;
}

31
.svn/pristine/9a/9a2b77870ce86d532bd74b7def02202e729c5eb1.svn-base Normal file
View file

@ -0,0 +1,31 @@
/**
* @file
* @brief Scan types and mapping functions.
*
* @author Thomas Escher, Billy Okal, Dorit Borrmann
*/
#ifndef IO_TYPES_H
#define IO_TYPES_H
//! IO types for file formats, distinguishing the use of ScanIOs
enum IOType {
UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, UOS_RRGBT, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES
};
//! Data channels in the scans
enum IODataType {
DATA_XYZ = 1<<0,
DATA_RGB = 1<<1,
DATA_REFLECTANCE = 1<<2,
DATA_TEMPERATURE = 1<<3,
DATA_AMPLITUDE = 1<<4,
DATA_TYPE = 1<<5,
DATA_DEVIATION = 1<<6
};
IOType formatname_to_io_type(const char * string);
const char * io_type_to_libname(IOType type);
#endif //IO_TYPES_H

27
.svn/pristine/9e/9ee28ee2356366bb5a56c6e3edf0f26c0d3d9c71.svn-base Normal file
View file

@ -0,0 +1,27 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_UOS_RGB_H__
#define __SCAN_IO_UOS_RGB_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for UOS scans with color information
*
* The compiled class is available as shared object file
*/
class ScanIO_uos_rgb : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

287
.svn/pristine/a2/a2c23051daf890ddd824c51a4fabcbc94be18b76.svn-base Normal file
View file

@ -0,0 +1,287 @@
/*
* scan_io_rxp implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_rxp.h"
#include "riegl/scanlib.hpp"
#include <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>* temperature, 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

137
.svn/pristine/a5/a5f14802187a6a91366cf4341e365d2606036cf4.svn-base Normal file
View file

@ -0,0 +1,137 @@
/**
* @file
* @brief Representation of a 3D point
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __POINT_H__
#define __POINT_H__
#include <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; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
/**
* Copy constructor
*/
inline Point(const Point& p) { x = p.x; y = p.y; z = p.z; type = p.type; point_id = p.point_id;
reflectance = p.reflectance; temperature = p.temperature; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];};
/**
* Constructor with an array, i.e., vecctor of coordinates
*/
inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];};
/**
* Constructor with three double values
*/
inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double _x, const double _y, const double _z, const char _r,
const char _g, const char _b) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;};
static inline Point cross(const Point &X, const Point &Y) {
Point res;
res.x = X.y * Y.z - X.z * Y.y;
res.y = X.z * Y.x - X.x * Y.z;
res.z = X.x * Y.y - X.y * Y.x;
return res;
};
static inline Point norm(const Point &p) {
double l = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
Point res(p.x/l, p.y/l, p.z/l);
return res;
};
inline Point operator+(const Point &p) const {
Point res;
res.x = x + p.x;
res.y = y + p.y;
res.z = z + p.z;
return res;
};
inline Point operator-(const Point &p) const {
Point res;
res.x = x - p.x;
res.y = y - p.y;
res.z = z - p.z;
return res;
};
inline Point& operator-=(const Point &p) {
x -= p.x;
y -= p.y;
z -= p.z;
return *this;
};
inline Point& operator+=(const Point &p) {
x += p.x;
y += p.y;
z += p.z;
return *this;
};
inline void transform(const double alignxf[16]);
inline double distance(const Point& p);
inline friend ostream& operator<<(ostream& os, const Point& p);
inline friend istream& operator>>(istream& is, Point& p);
// also public; set/get functions not necessary here
/// x coordinate in 3D space
double x;
/// y coordinate in 3D space
double y;
/// z coordinate in 3D space
double z;
/// additional information about the point, e.g., semantic
/// also used in veloscan for distiuguish moving or static
int type;
/////////////////////////for veloslam/////////////////////////////
double rad;
/// tang in cylindrical coordinates for veloscan
double tan_theta;
// point id in points for veloscan , you can use it find point.
long point_id;
/////////////////////////for veloslam/////////////////////////////
// color information of the point between 0 and 255
// rgb
unsigned char rgb[3];
float reflectance;
float temperature;
float amplitude;
float deviation;
};
inline Point operator*(const double &v, const Point &p) {
Point res;
res.x = v * p.x;
res.y = v * p.y;
res.z = v * p.z;
return res;
}
#include "point.icc"
#endif

192
.svn/pristine/b1/b1864c308cf24f4ea259589fcff8e3c66aaafc0f.svn-base Normal file
View file

@ -0,0 +1,192 @@
/*
* scan_io_ks implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "scanio/scan_io_ks.h"
#include <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 "ScanPos"
#define DATA_PATH_SUFFIX " - Scan001.txt"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
std::list<std::string> ScanIO_ks::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::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
// read 6 plain doubles
for(i = 0; i < 6; ++i) pose_file >> pose[i];
pose_file.close();
// CAD map -> pose correction [ks x/y/z -> slam -z/y/x]
double tmp;
tmp = pose[0];
pose[0] = - pose[2];
pose[2] = tmp;
// convert coordinate to cm
for(i = 0; i < 3; ++i) pose[i] *= 100.0;
// convert angles from deg to rad
for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]);
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_ks::supports(IODataType type)
{
return !!(type & (DATA_XYZ));
}
void ScanIO_ks::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* temperature, 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
char dummy[255];
data_file.getline(dummy, 255);
// read points
double point[3];
double tmp;
while(data_file.good()) {
try {
for(i = 0; i < 3; ++i) data_file >> point[i];
} catch(std::ios_base::failure& e) {
break;
}
// the enemy's x/y/z is mapped to slam's x/z/y, shuffle time!
tmp = point[1];
point[1] = point[2];
point[2] = tmp;
// TODO: offset is application specific, handle with care
// correct constant offset (in slam coordinates)
point[0] -= 70000.0; // x
point[2] -= 20000.0; // z
// convert coordinate to cm
for(i = 0; i < 3; ++i) point[i] *= 100.0;
// apply filter and insert point
if(filter.check(point)) {
for(i = 0; i < 3; ++i) xyz->push_back(point[i]);
}
}
data_file.close();
}
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) ScanIO* create()
#else
extern "C" ScanIO* create()
#endif
{
return new ScanIO_ks;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
#else
extern "C" void destroy(ScanIO *sio)
#endif
{
delete sio;
}
#ifdef _MSC_VER
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
{
return TRUE;
}
#endif

46
.svn/pristine/b5/b5117f12dc472b13429f166b092d2a532347f9ac.svn-base Normal file
View file

@ -0,0 +1,46 @@
/*
* serverScan implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
#include "scanserver/serverScan.h"
#include "scanserver/cache/cacheManager.h"
#include "scanserver/scanHandler.h"
#include "scanserver/temporaryHandler.h"
ServerScan::ServerScan(const ip::allocator<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_temperature = cm->createCacheObject();
m_temperature->setCacheHandler(new ScanHandler(m_temperature.get(), cm, this, DATA_TEMPERATURE));
m_amplitude = cm->createCacheObject();
m_amplitude->setCacheHandler(new ScanHandler(m_amplitude.get(), cm, this, DATA_AMPLITUDE));
m_type = cm->createCacheObject();
m_type->setCacheHandler(new ScanHandler(m_type.get(), cm, this, DATA_TYPE));
m_deviation = cm->createCacheObject();
m_deviation->setCacheHandler(new ScanHandler(m_deviation.get(), cm, this, DATA_DEVIATION));
m_xyz_reduced = cm->createCacheObject();
m_xyz_reduced->setCacheHandler(new TemporaryHandler(m_xyz_reduced.get(), cm, this));
m_xyz_reduced_original = cm->createCacheObject();
m_xyz_reduced_original->setCacheHandler(new TemporaryHandler(m_xyz_reduced_original.get(), cm, this, true));
m_show_reduced = cm->createCacheObject();
m_show_reduced->setCacheHandler(new TemporaryHandler(m_show_reduced.get(), cm, this, true));
m_octtree = cm->createCacheObject();
m_octtree->setCacheHandler(new TemporaryHandler(m_octtree.get(), cm, this, true));
}

28
.svn/pristine/b7/b7523d794a1f3568cfd172daf46910f686009fc8.svn-base Normal file
View file

@ -0,0 +1,28 @@
/**
* @file
* @brief IO of a 3D scan in uos file format with reflectance, color and
* temperature information
* @author Dorit Borrmann
*/
#ifndef __SCAN_IO_UOS_RRGBT_H__
#define __SCAN_IO_UOS_RRGBT_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for UOS scans
*
* The compiled class is available as shared object file
*/
class ScanIO_uos_rrgbt : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

58
.svn/pristine/b8/b8bcad940414d7024206bef39c6334a9b786d9ac.svn-base Normal file
View file

@ -0,0 +1,58 @@
/**
* @file
* @brief
*
* @author Thomas Escher
* @author Dorit Borrmann
*/
#ifndef SCAN_HANDLER_H
#define SCAN_HANDLER_H
#include "scanserver/temporaryHandler.h"
#include <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_temperature;
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

32
.svn/pristine/cd/cd0b815ad1356c0cbdbffaba48ab1707a8a63fc6.svn-base Normal file
View file

@ -0,0 +1,32 @@
/**
* @file
* @brief IO of a 3D scan in velodyne raw file format
* @author Liwei. Computer School of Wuhan University, China.
*/
#ifndef __SCAN_IO_VELODYNE_H__
#define __SCAN_IO_VELODYNE_H__
#include <string>
using std::string;
#include <vector>
using std::vector;
#include "scan_io.h"
/**
* @brief 3D scan loader for VELODYNE scans
*
* The compiled class is available as shared object file
*/
class ScanIO_velodyne : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
int fileCounter;
};
#endif

197
.svn/pristine/d2/d2a995bdc5236be7eb16b69c039450eb85d2edda.svn-base Normal file
View file

@ -0,0 +1,197 @@
/**
* @file
* @brief Scan containing all interprocess shared data and management values.
*
* @author Thomas Escher
*/
#ifndef SHARED_SCAN_H
#define SHARED_SCAN_H
#include "scanserver/frame.h"
#include "slam6d/io_types.h"
#include "slam6d/data_types.h"
#include "slam6d/pointfilter.h"
#include <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();
DataTemperature getTemperature();
DataAmplitude getAmplitude();
DataType getType();
DataDeviation getDeviation();
//! Reduced transformed points
DataXYZ getXYZReduced();
//! Create a new set of reduced points
DataXYZ createXYZReduced(unsigned int size);
//! Reduced untransformed points
DataXYZ getXYZReducedOriginal();
//! Create a new set of reduced points originals
DataXYZ createXYZReducedOriginal(unsigned int size);
//! Individual reduced points to use in show if requested
TripleArray<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_temperature, m_amplitude, m_type, m_deviation,
m_xyz_reduced, m_xyz_reduced_original,
m_show_reduced, m_octtree;
FrameVector m_frames;
//! invalidate full cache objects
void invalidateFull();
//! invalidate reduced cache objects
void invalidateReduced();
//! invalidate show related cache objects
void invalidateShow();
private:
//! Static callback for cache misses to send a request to the server interface
static void onCacheMiss(CacheObject* obj);
//! Static callback for cache object creation calls
static void onAllocation(CacheObject* obj, unsigned int size);
//! Static callback for cache object invalidation
static void onInvalidation(CacheObject* obj);
};
#endif //SHARED_SCAN_H

212
.svn/pristine/d4/d47898a6f11b520604ed059c5724d9011b082f1a.svn-base Normal file
View file

@ -0,0 +1,212 @@
/*
* scan_io_riegl_txt implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_riegl_txt.h"
#include <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>* temperature, 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

30
.svn/pristine/d5/d5ed1a9c335f39e399358cbea8a7dd3d4fa78c04.svn-base Normal file
View file

@ -0,0 +1,30 @@
/**
* @file
* @brief IO of a 3D scan in rts file format
* @author Thomas Escher
*/
#ifndef __SCAN_IO_RTS_H__
#define __SCAN_IO_RTS_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for RTS scans
*
* The compiled class is available as shared object file
*/
class ScanIO_rts : public ScanIO {
public:
virtual std::list<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>* temperature, 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

220
.svn/pristine/dd/ddefa9bff7fd57daa456850e1bb62d848dd5dd14.svn-base Normal file
View file

@ -0,0 +1,220 @@
/*
* scan_io_riegl_rgb implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_riegl_rgb.h"
#include <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>* temperature, 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

376
.svn/pristine/e2/e2820a7a775a73f50e6e86be6304efb6c416adfc.svn-base Normal file
View file

@ -0,0 +1,376 @@
/*
* managedScan implementation
*
* Copyright (C) Kai Lingemann, Thomas Escher
*
* Released under the GPL version 3.
*
*/
#include "slam6d/managedScan.h"
#include "scanserver/clientInterface.h"
#include "slam6d/Boctree.h"
#include "slam6d/kdManaged.h"
#ifdef WITH_METRICS
#include "slam6d/metrics.h"
#endif //WITH_METRICS
#include <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 == "temperature") {
return m_shared_scan->getTemperature();
} else
if(identifier == "amplitude") {
return m_shared_scan->getAmplitude();
} else
if(identifier == "type") {
return m_shared_scan->getType();
} else
if(identifier == "deviation") {
return m_shared_scan->getDeviation();
} else
if(identifier == "xyz reduced") {
// if this is a fresh run, initialize reduced properly via original or creating it anew
if(!m_reduced_ready) {
calcReducedOnDemand();
}
return m_shared_scan->getXYZReduced();
} else
if(identifier == "xyz reduced original") {
// if reduction has completed, original will exist (either from last run or created in this run)
if(!m_reduced_ready) {
calcReducedOnDemand();
}
return m_shared_scan->getXYZReducedOriginal();
} else
if(identifier == "xyz reduced show") {
if(m_shared_scan->getXYZReducedShow().valid())
return m_shared_scan->getXYZReducedShow();
calcReducedShow();
return m_shared_scan->getXYZReducedShow();
} else
if(identifier == "octtree") {
if(m_shared_scan->getOcttree().valid())
return m_shared_scan->getOcttree();
createOcttree();
return m_shared_scan->getOcttree();
}
{
throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::get. Upgrade SharedScan for this data field.");
}
}
void ManagedScan::get(unsigned int types)
{
m_shared_scan->prefetch(types);
}
DataPointer ManagedScan::create(const std::string& identifier, unsigned int size)
{
// map identifiers to functions in SharedScan and scale back size from bytes to number of points
if(identifier == "xyz reduced") {
return m_shared_scan->createXYZReduced(size / (3*sizeof(double)));
} else
if(identifier == "xyz reduced original") {
return m_shared_scan->createXYZReducedOriginal(size / (3*sizeof(double)));
} else
{
throw runtime_error(string("Identifier '") + identifier + "' not compatible with ManagedScan::create. Upgrade SharedScan for this data field.");
}
}
void ManagedScan::clear(const std::string& identifier)
{
// nothing to do here
// TODO: mark CacheObjects with a low priority for faster removal by the manager
}
void ManagedScan::createSearchTreePrivate()
{
switch(searchtree_nnstype)
{
case simpleKD:
kd = new KDtreeManaged(this);
break;
case BOCTree:
kd = new BOctTree<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));
}

53
.svn/pristine/e9/e997c5ebf8c9d5197819fb044063ad31d33d4580.svn-base Normal file
View file

@ -0,0 +1,53 @@
# CLIENT LIBRARY
# build by source
set(CLIENT_SRCS
clientInterface.cc sharedScan.cc cache/cacheObject.cc
cache/cacheDataAccess.cc
)
if(WITH_METRICS)
set(CLIENT_SRCS ${CLIENT_SRCS} ../slam6d/metrics.cc)
endif(WITH_METRICS)
add_library(scanclient STATIC ${CLIENT_SRCS})
# add libraries
# boost::interprocess
set(CLIENT_LIBS ${Boost_LIBRARIES} pointfilter)
if(UNIX AND NOT APPLE)
# boost::interprocess uses pthread, requiring librt
#set(CLIENT_LIBS ${CLIENT_LIBS} rt)
endif(UNIX AND NOT APPLE)
target_link_libraries(scanclient ${CLIENT_LIBS})
# SERVER EXECUTABLE
# build by source
set(SERVER_SRCS
scanserver.cc serverInterface.cc frame_io.cc serverScan.cc
cache/cacheManager.cc cache/cacheHandler.cc scanHandler.cc
temporaryHandler.cc cacheIO.cc
)
add_executable(scanserver ${SERVER_SRCS})
# add libraries
# boost::interprocess/filesystem
# scanclient basic functionality
# scanio for ScanHandler input
set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio)
if(UNIX)
# boost::interprocess uses pthread, requiring librt
#set(SERVER_LIBS ${SERVER_LIBS} rt)
endif(UNIX)
if(WIN32)
# 3rd party getopt library
set(SERVER_LIBS ${SERVER_LIBS} XGetopt)
endif(WIN32)
target_link_libraries(scanserver ${SERVER_LIBS})

177
.svn/pristine/fb/fb1dec70f3bde4e42b7ec439ec8d54720f0658cc.svn-base Normal file
View file

@ -0,0 +1,177 @@
/*
* scan_io_uos_rrgbt implementation
*
* Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Dorit Borrmann. School of Engineering and Science, Jacobs University * Bremen, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "scanio/scan_io_uos_rrgbt.h"
#include <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_rrgbt::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_rrgbt::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
// read 6 plain doubles
for(i = 0; i < 6; ++i) pose_file >> pose[i];
pose_file.close();
// convert angles from deg to rad
for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]);
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_uos_rrgbt::supports(IODataType type)
{
return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB | DATA_TEMPERATURE));
}
void ScanIO_uos_rrgbt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* temperature, 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 && temperature != 0) {
// open data file
ifstream data_file(data_path);
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
// overread the first line ignoring the header information
char dummy[255];
data_file.getline(dummy, 255);
// read points
double point[4];
unsigned int color[3];
double temp;
while(data_file.good()) {
try {
for(i = 0; i < 4; ++i) data_file >> point[i];
for(i = 0; i < 3; ++i) data_file >> color[i];
data_file >> temp;
} catch(std::ios_base::failure& e) {
break;
}
// apply filter and insert point
if(filter.check(point)) {
for(i = 0; i < 3; ++i) xyz->push_back(point[i]);
reflectance->push_back(point[3]);
for(i = 0; i < 3; ++i) rgb->push_back(
static_cast<unsigned char>(color[i]));
temperature->push_back(temp);
}
}
data_file.close();
}
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) ScanIO* create()
#else
extern "C" ScanIO* create()
#endif
{
return new ScanIO_uos_rrgbt;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
#else
extern "C" void destroy(ScanIO *sio)
#endif
{
delete sio;
}
#ifdef _MSC_VER
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
{
return TRUE;
}
#endif

BIN
.svn/wc.db
View file

Binary file not shown.

19
CMakeLists.txt
View file

@ -166,25 +166,6 @@ ENDIF(WITH_TOOLS)
## Scan reduction
OPTION(WITH_SCAN_REDUCTION "Whether to build the scan reduction binary scan_red ON/OFF" OFF)
IF(WITH_SCAN_REDUCTION)
MESSAGE(STATUS "With scan_red")
ELSE(WITH_SCAN_REDUCTION)
MESSAGE(STATUS "Without scan_red")
ENDIF(WITH_SCAN_REDUCTION)
## Scan difference
OPTION(WITH_SCAN_DIFF "Whether to build the scan_diff binary ON/OFF" OFF)
IF(WITH_SCAN_DIFF)
MESSAGE(STATUS "With scan_diff")
ELSE(WITH_SCAN_DIFF)
MESSAGE(STATUS "Without scan_diff")
ENDIF(WITH_SCAN_DIFF)
## CAD matching
OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)

2
include/scanio/scan_io.h
View file

@ -53,7 +53,7 @@ public:
* @param identifier IO-specific identifier for the particular scan
* @param filter Filter object which each point is tested on by its position
*/
virtual void readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<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;
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>* temperature = 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.

2
include/scanio/scan_io_ks.h
View file

@ -20,7 +20,7 @@ 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

2
include/scanio/scan_io_ks_rgb.h
View file

@ -20,7 +20,7 @@ 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

4
include/scanio/scan_io_riegl_rgb.h
View file

@ -1,6 +1,6 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @brief IO of a 3D scan in riegl_txt file format with added color information
* @author Thomas Escher
*/
@ -20,7 +20,7 @@ class ScanIO_riegl_rgb : public ScanIO {
public:
virtual std::list<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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

4
include/scanio/scan_io_riegl_txt.h
View file

@ -1,6 +1,6 @@
/**
* @file
* @brief IO of a 3D scan in uos file format
* @brief IO of a 3D scan in riegl_txt file format
* @author Thomas Escher
*/
@ -20,7 +20,7 @@ class ScanIO_riegl_txt : public ScanIO {
public:
virtual std::list<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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

6
include/scanio/scan_io_rts.h
View file

@ -4,8 +4,8 @@
* @author Thomas Escher
*/
#ifndef __SCAN_IO_UOS_H__
#define __SCAN_IO_UOS_H__
#ifndef __SCAN_IO_RTS_H__
#define __SCAN_IO_RTS_H__
#include "scan_io.h"
@ -20,7 +20,7 @@ class ScanIO_rts : public ScanIO {
public:
virtual std::list<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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
private:
std::string cached_dir;

2
include/scanio/scan_io_rxp.h
View file

@ -28,7 +28,7 @@ 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
ScanIO_rxp() : dec(0), imp(0) {}

2
include/scanio/scan_io_uos.h
View file

@ -20,7 +20,7 @@ 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

2
include/scanio/scan_io_uos_rgb.h
View file

@ -20,7 +20,7 @@ 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

28
include/scanio/scan_io_uos_rrgbt.h Normal file
View file

@ -0,0 +1,28 @@
/**
* @file
* @brief IO of a 3D scan in uos file format with reflectance, color and
* temperature information
* @author Dorit Borrmann
*/
#ifndef __SCAN_IO_UOS_RRGBT_H__
#define __SCAN_IO_UOS_RRGBT_H__
#include "scan_io.h"
/**
* @brief 3D scan loader for UOS scans
*
* The compiled class is available as shared object file
*/
class ScanIO_uos_rrgbt : public ScanIO {
public:
virtual std::list<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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};
#endif

2
include/scanio/scan_io_uosr.h
View file

@ -22,7 +22,7 @@ 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
};

2
include/scanio/scan_io_velodyne.h
View file

@ -23,7 +23,7 @@ class ScanIO_velodyne : 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 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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation);
virtual bool supports(IODataType type);
int fileCounter;

1
include/scanserver/multiArray.h
View file

@ -64,6 +64,7 @@ public:
typedef TripleArray<double> DataXYZ;
typedef TripleArray<unsigned char> DataRGB;
typedef SingleArray<float> DataReflectance;
typedef SingleArray<float> DataTemperature;
typedef SingleArray<float> DataAmplitude;
typedef SingleArray<int> DataType;
typedef SingleArray<float> DataDeviation;

2
include/scanserver/scanHandler.h
View file

@ -3,6 +3,7 @@
* @brief
*
* @author Thomas Escher
* @author Dorit Borrmann
*/
#ifndef SCAN_HANDLER_H
@ -48,6 +49,7 @@ private:
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_temperature;
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;

3
include/scanserver/sharedScan.h
View file

@ -102,6 +102,7 @@ public:
DataXYZ getXYZ();
DataRGB getRGB();
DataReflectance getReflectance();
DataTemperature getTemperature();
DataAmplitude getAmplitude();
DataType getType();
DataDeviation getDeviation();
@ -168,7 +169,7 @@ private:
protected:
ip::offset_ptr<double> m_pose;
ip::offset_ptr<CacheObject> m_xyz, m_rgb, m_reflectance, m_amplitude, m_type, m_deviation,
ip::offset_ptr<CacheObject> m_xyz, m_rgb, m_reflectance, m_temperature, m_amplitude, m_type, m_deviation,
m_xyz_reduced, m_xyz_reduced_original,
m_show_reduced, m_octtree;
FrameVector m_frames;

31
include/show/colormanager.h
View file

@ -21,7 +21,8 @@ class ColorMap {
HSV = 2,
JET = 3,
HOT = 4,
SHSV = 5
SHSV = 5,
TEMP = 6
};
virtual ~ColorMap() {};
@ -96,6 +97,34 @@ class HotMap : public ColorMap {
}
};
class TempMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets) {
float t = 1.0 - (float)i/(float)buckets;
if(t <= 1.0/5.0) {
d[1] = d[2] = 0.0;
d[0] = t/(1.0/5.0);
} else if(t <=2.0/5.0) {
d[0] = 1.0;
d[1] = (t-(1.0/5.0))/(1.0/5.0);
d[2] = 0.0;
} else if(t <=3.0/5.0) {
d[0] = 1.0 - ((t-(2.0/5.0))/(1.0/5.0));
d[1] = 1.0;
d[2] = (t-(2.0/5.0))/(1.0/5.0);
} else if(t <=4.0/5.0) {
d[0] = 0.0;
d[1] = 1.0 - ((t-(3.0/5.0))/(1.0/5.0));
d[2] = 1.0;
} else {
d[0] = 0.0;
d[1] = 0.0;
d[2] = 1.0 - ((t-(4.0/5.0))/(1.3/5.0));
}
}
};
class DiffMap : public ColorMap {
public:
virtual void calcColor(float *d, unsigned int i, unsigned int buckets);

1
include/slam6d/data_types.h
View file

@ -218,6 +218,7 @@ typedef TripleArray<double> DataXYZ;
typedef TripleArray<float> DataXYZFloat;
typedef TripleArray<unsigned char> DataRGB;
typedef SingleArray<float> DataReflectance;
typedef SingleArray<float> DataTemperature;
typedef SingleArray<float> DataAmplitude;
typedef SingleArray<int> DataType;
typedef SingleArray<float> DataDeviation;

2
include/slam6d/fbr/fbr_global.h
View file

@ -36,7 +36,7 @@ namespace fbr{
PANNINI,
STEREOGRAPHIC,
ZAXIS,
CONIC,
CONIC
};
/**
* @enum panorama_map_method

7
include/slam6d/fbr/panorama.h
View file

@ -59,6 +59,13 @@ namespace fbr{
* @brief creates the panorama reflectance image and map.
*/
void createPanorama(cv::Mat scan);
/**
* @brief recovers the point cloud from the panorama image and range information
* @param image - input range image to be converted to point cloud
* @param file - destination of .3d file containing the point cloud
*/
void recoverPointCloud(const cv::Mat& range_image, const std::string& file);
unsigned int getImageWidth();
unsigned int getImageHeight();
projection_method getProjectionMethod();

11
include/slam6d/io_types.h
View file

@ -2,7 +2,7 @@
* @file
* @brief Scan types and mapping functions.
*
* @author Thomas Escher, Billy Okal
* @author Thomas Escher, Billy Okal, Dorit Borrmann
*/
#ifndef IO_TYPES_H
@ -10,7 +10,7 @@
//! IO types for file formats, distinguishing the use of ScanIOs
enum IOType {
UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES
UOS, UOSR, UOS_MAP, UOS_FRAMES, UOS_MAP_FRAMES, UOS_RGB, UOS_RRGBT, OLD, RTS, RTS_MAP, RIEGL_TXT, RIEGL_PROJECT, RIEGL_RGB, RIEGL_BIN, IFP, ZAHN, PLY, WRL, XYZ, ZUF, ASC, IAIS, FRONT, X3D, RXP, KIT, AIS, OCT, TXYZR, XYZR, XYZ_RGB, KS, KS_RGB, STL, LEICA, PCL, PCI, UOS_CAD, VELODYNE, VELODYNE_FRAMES
};
//! Data channels in the scans
@ -18,9 +18,10 @@ enum IODataType {
DATA_XYZ = 1<<0,
DATA_RGB = 1<<1,
DATA_REFLECTANCE = 1<<2,
DATA_AMPLITUDE = 1<<3,
DATA_TYPE = 1<<4,
DATA_DEVIATION = 1<<5
DATA_TEMPERATURE = 1<<3,
DATA_AMPLITUDE = 1<<4,
DATA_TYPE = 1<<5,
DATA_DEVIATION = 1<<6
};
IOType formatname_to_io_type(const char * string);

15
include/slam6d/point.h
View file

@ -25,24 +25,24 @@ public:
/**
* Default constructor
*/
inline Point() { x = y = z = 0.0; point_id = 0; type = 0; reflectance = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point() { x = y = z = 0.0; point_id = 0; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
/**
* Copy constructor
*/
inline Point(const Point& p) { x = p.x; y = p.y; z = p.z; type = p.type; point_id = p.point_id;
reflectance = p.reflectance; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];};
reflectance = p.reflectance; temperature = p.temperature; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];};
/**
* Constructor with an array, i.e., vecctor of coordinates
*/
inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; amplitude = 0.0; deviation = 0.0;
rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];};
inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];};
/**
* Constructor with three double values
*/
inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; };
inline Point(const double _x, const double _y, const double _z, const char _r, const char _g, const char _b) { x = _x; y = _y; z = _z; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;};
inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double _x, const double _y, const double _z, const char _r,
const char _g, const char _b) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;};
static inline Point cross(const Point &X, const Point &Y) {
Point res;
@ -118,6 +118,7 @@ public:
unsigned char rgb[3];
float reflectance;
float temperature;
float amplitude;
float deviation;
};

15
include/slam6d/point_type.h
View file

@ -30,6 +30,7 @@ public:
static const unsigned int USE_NONE;
static const unsigned int USE_REFLECTANCE;
static const unsigned int USE_TEMPERATURE;
static const unsigned int USE_AMPLITUDE;
static const unsigned int USE_DEVIATION;
static const unsigned int USE_HEIGHT;
@ -43,6 +44,7 @@ public:
PointType(unsigned int _types);
bool hasReflectance();
bool hasTemperature();
bool hasAmplitude();
bool hasDeviation();
bool hasType();
@ -51,6 +53,7 @@ public:
bool hasIndex();
unsigned int getReflectance();
unsigned int getTemperature();
unsigned int getAmplitude();
unsigned int getDeviation();
unsigned int getTime();
@ -104,7 +107,7 @@ private:
/**
* Derived from types, to map type to the array index for each point
**/
int dimensionmap[8];
int dimensionmap[9];
bool hasType(unsigned int type);
@ -113,6 +116,7 @@ private:
DataXYZ* m_xyz;
DataRGB* m_rgb;
DataReflectance* m_reflectance;
DataTemperature* m_temperature;
DataAmplitude* m_amplitude;
DataType* m_type;
DataDeviation* m_deviation;
@ -130,6 +134,9 @@ T *PointType::createPoint(const Point &P, unsigned int index ) {
if (types & USE_REFLECTANCE) {
p[counter++] = P.reflectance;
}
if (types & USE_TEMPERATURE) {
p[counter++] = P.temperature;
}
if (types & USE_AMPLITUDE) {
p[counter++] = P.amplitude;
}
@ -164,6 +171,9 @@ Point PointType::createPoint(T *p) {
if (types & USE_REFLECTANCE) {
P.reflectance = p[counter++];
}
if (types & USE_TEMPERATURE) {
P.temperature = p[counter++];
}
if (types & USE_AMPLITUDE) {
P.amplitude = p[counter++];
}
@ -196,6 +206,9 @@ T *PointType::createPoint(unsigned int i, unsigned int index) {
if (types & USE_REFLECTANCE) {
p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0);
}
if (types & USE_TEMPERATURE) {
p[counter++] = (m_temperature? (*m_temperature)[i]: 0);
}
if (types & USE_AMPLITUDE) {
p[counter++] = (m_amplitude? (*m_amplitude)[i]: 0);
}

2
src/scanio/CMakeLists.txt
View file

@ -5,7 +5,7 @@ else(WIN32)
endif(WIN32)
set(SCANIO_LIBNAMES
uos uosr uos_rgb ks ks_rgb riegl_txt riegl_rgb rts velodyne
uos uosr uos_rgb uos_rrgbt ks ks_rgb riegl_txt riegl_rgb rts velodyne
)
if(WITH_RIVLIB)

2
src/scanio/scan_io_ks.cc
View file

@ -102,7 +102,7 @@ bool ScanIO_ks::supports(IODataType type)
return !!(type & (DATA_XYZ));
}
void ScanIO_ks::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
void ScanIO_ks::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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;

2
src/scanio/scan_io_ks_rgb.cc
View file

@ -104,7 +104,7 @@ bool ScanIO_ks_rgb::supports(IODataType type)
return !!(type & (DATA_XYZ | DATA_RGB | DATA_REFLECTANCE | DATA_AMPLITUDE));
}
void ScanIO_ks_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;

2
src/scanio/scan_io_riegl_rgb.cc
View file

@ -118,7 +118,7 @@ bool ScanIO_riegl_rgb::supports(IODataType type)
return !!(type & (DATA_XYZ | DATA_RGB | DATA_REFLECTANCE));
}
void ScanIO_riegl_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;

2
src/scanio/scan_io_riegl_txt.cc
View file

@ -118,7 +118,7 @@ bool ScanIO_riegl_txt::supports(IODataType type)
return !!(type & (DATA_XYZ | DATA_REFLECTANCE));
}
void ScanIO_riegl_txt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;

2
src/scanio/scan_io_rts.cc
View file

@ -125,7 +125,7 @@ bool ScanIO_rts::supports(IODataType type)
return !!(type & (DATA_XYZ));
}
void ScanIO_rts::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
// TODO: Type and other columns?
unsigned int i;

2
src/scanio/scan_io_rxp.cc
View file

@ -122,7 +122,7 @@ bool ScanIO_rxp::supports(IODataType type)
return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_AMPLITUDE | DATA_DEVIATION | DATA_TYPE));
}
void ScanIO_rxp::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
path data_path(dir_path);

2
src/scanio/scan_io_uos.cc
View file

@ -92,7 +92,7 @@ bool ScanIO_uos::supports(IODataType type)
return !!(type & (DATA_XYZ));
}
void ScanIO_uos::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;

4
src/scanio/scan_io_uos_rgb.cc
View file

@ -91,7 +91,7 @@ bool ScanIO_uos_rgb::supports(IODataType type)
return !!(type & (DATA_XYZ | DATA_RGB));
}
void ScanIO_uos_rgb::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
@ -101,7 +101,7 @@ void ScanIO_uos_rgb::readScan(const char* dir_path, const char* identifier, Poin
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0 || rgb != 0) {
if(xyz != 0 && rgb != 0) {
// open data file
ifstream data_file(data_path);
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);

177
src/scanio/scan_io_uos_rrgbt.cc Normal file
View file

@ -0,0 +1,177 @@
/*
* scan_io_uos_rrgbt implementation
*
* Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Dorit Borrmann. School of Engineering and Science, Jacobs University * Bremen, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "scanio/scan_io_uos_rrgbt.h"
#include <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_rrgbt::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_rrgbt::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
// read 6 plain doubles
for(i = 0; i < 6; ++i) pose_file >> pose[i];
pose_file.close();
// convert angles from deg to rad
for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]);
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_uos_rrgbt::supports(IODataType type)
{
return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB | DATA_TEMPERATURE));
}
void ScanIO_uos_rrgbt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* temperature, 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 && temperature != 0) {
// open data file
ifstream data_file(data_path);
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
// overread the first line ignoring the header information
char dummy[255];
data_file.getline(dummy, 255);
// read points
double point[4];
unsigned int color[3];
double temp;
while(data_file.good()) {
try {
for(i = 0; i < 4; ++i) data_file >> point[i];
for(i = 0; i < 3; ++i) data_file >> color[i];
data_file >> temp;
} catch(std::ios_base::failure& e) {
break;
}
// apply filter and insert point
if(filter.check(point)) {
for(i = 0; i < 3; ++i) xyz->push_back(point[i]);
reflectance->push_back(point[3]);
for(i = 0; i < 3; ++i) rgb->push_back(
static_cast<unsigned char>(color[i]));
temperature->push_back(temp);
}
}
data_file.close();
}
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) ScanIO* create()
#else
extern "C" ScanIO* create()
#endif
{
return new ScanIO_uos_rrgbt;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
#else
extern "C" void destroy(ScanIO *sio)
#endif
{
delete sio;
}
#ifdef _MSC_VER
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
{
return TRUE;
}
#endif

2
src/scanio/scan_io_uosr.cc
View file

@ -91,7 +91,7 @@ bool ScanIO_uosr::supports(IODataType type)
return !!(type & ( DATA_REFLECTANCE | DATA_XYZ ));
}
void ScanIO_uosr::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
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>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;

1
src/scanio/scan_io_velodyne.cc
View file

@ -515,6 +515,7 @@ void ScanIO_velodyne::readScan(
std::vector<double>* xyz,
std::vector<unsigned char>* rgb,
std::vector<float>* reflectance,
std::vector<float>* temperature,
std::vector<float>* amplitude,
std::vector<int>* type,
std::vector<float>* deviation)

4
src/scanserver/CMakeLists.txt
View file

@ -18,7 +18,7 @@ set(CLIENT_LIBS ${Boost_LIBRARIES} pointfilter)
if(UNIX AND NOT APPLE)
# boost::interprocess uses pthread, requiring librt
set(CLIENT_LIBS ${CLIENT_LIBS} rt)
#set(CLIENT_LIBS ${CLIENT_LIBS} rt)
endif(UNIX AND NOT APPLE)
target_link_libraries(scanclient ${CLIENT_LIBS})
@ -42,7 +42,7 @@ set(SERVER_LIBS ${Boost_LIBRARIES} scanclient scanio)
if(UNIX)
# boost::interprocess uses pthread, requiring librt
set(SERVER_LIBS ${SERVER_LIBS} rt)
#set(SERVER_LIBS ${SERVER_LIBS} rt)
endif(UNIX)
if(WIN32)

7
src/scanserver/scanHandler.cc
View file

@ -30,6 +30,7 @@ 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_temperature;
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;
@ -158,6 +159,7 @@ bool ScanHandler::load()
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> temperature(m_scan, m_prefetch_temperature);
PrefetchVector<float> amplitude(m_scan, m_prefetch_amplitude);
PrefetchVector<int> type(m_scan, m_prefetch_type);
PrefetchVector<float> deviation(m_scan, m_prefetch_deviation);
@ -167,6 +169,7 @@ bool ScanHandler::load()
if(m_data == DATA_XYZ) vec = &xyz;
else if(m_data == DATA_RGB) vec = &rgb;
else if(m_data == DATA_REFLECTANCE) vec = &reflectance;
else if(m_data == DATA_TEMPERATURE) vec = &temperature;
else if(m_data == DATA_AMPLITUDE) vec = &amplitude;
else if(m_data == DATA_TYPE) vec = &type;
else if(m_data == DATA_DEVIATION) vec = &deviation;
@ -187,6 +190,7 @@ bool ScanHandler::load()
if(prefetch & DATA_XYZ) xyz.create();
if(prefetch & DATA_RGB) rgb.create();
if(prefetch & DATA_REFLECTANCE) reflectance.create();
if(prefetch & DATA_TEMPERATURE) temperature.create();
if(prefetch & DATA_AMPLITUDE) amplitude.create();
if(prefetch & DATA_TYPE) type.create();
if(prefetch & DATA_DEVIATION) deviation.create();
@ -196,7 +200,7 @@ bool ScanHandler::load()
PointFilter filter(m_scan->getPointFilter());
sio->readScan(m_scan->getDirPath(), m_scan->getIdentifier(),
filter,
xyz.get(), rgb.get(), reflectance.get(), amplitude.get(), type.get(), deviation.get());
xyz.get(), rgb.get(), reflectance.get(), temperature.get(), amplitude.get(), type.get(), deviation.get());
} catch(std::runtime_error& e) {
// INFO
// cerr << "[" << m_scan->getIdentifier() << "][" << m_data << "] ScanIO runtime_error: " << e.what() << endl;
@ -229,6 +233,7 @@ bool ScanHandler::load()
xyz.save();
rgb.save();
reflectance.save();
temperature.save();
amplitude.save();
type.save();
deviation.save();

2
src/scanserver/serverScan.cc
View file

@ -25,6 +25,8 @@ ServerScan::ServerScan(const ip::allocator<void, SegmentManager> & allocator,
m_rgb->setCacheHandler(new ScanHandler(m_rgb.get(), cm, this, DATA_RGB));
m_reflectance = cm->createCacheObject();
m_reflectance->setCacheHandler(new ScanHandler(m_reflectance.get(), cm, this, DATA_REFLECTANCE));
m_temperature = cm->createCacheObject();
m_temperature->setCacheHandler(new ScanHandler(m_temperature.get(), cm, this, DATA_TEMPERATURE));
m_amplitude = cm->createCacheObject();
m_amplitude->setCacheHandler(new ScanHandler(m_amplitude.get(), cm, this, DATA_AMPLITUDE));
m_type = cm->createCacheObject();

4
src/scanserver/sharedScan.cc
View file

@ -213,6 +213,10 @@ DataReflectance SharedScan::getReflectance() {
return m_reflectance->getCacheData<SharedScan::onCacheMiss>();
}
DataTemperature SharedScan::getTemperature() {
return m_temperature->getCacheData<SharedScan::onCacheMiss>();
}
DataAmplitude SharedScan::getAmplitude() {
return m_amplitude->getCacheData<SharedScan::onCacheMiss>();
}

11
src/show/show_common.cc
View file

@ -379,6 +379,10 @@ void usage(char* prog)
<< " use reflectivity values for coloring point clouds" << endl
<< " only works when using octree display" << endl
<< endl
<< bold << " -D, --temperature, --degree" << normal << endl
<< " use temperature values for coloring point clouds" << endl
<< " only works when using octree display" << endl
<< endl
<< bold << " -a, --amplitude" << endl << normal
<< " use amplitude values for coloring point clouds" << endl
<< " only works when using octree display" << endl
@ -457,6 +461,8 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
{ "octree", optional_argument, 0, 'O' },
{ "reflectance", no_argument, 0, 'R' },
{ "reflectivity", no_argument, 0, 'R' },
{ "temperature", no_argument, 0, 'D' },
{ "degree", no_argument, 0, 'D' },
{ "amplitude", no_argument, 0, 'a' },
{ "deviation", no_argument, 0, 'd' },
{ "height", no_argument, 0, 'h' },
@ -471,7 +477,7 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRadhTcb", longopts, NULL)) != -1) {
while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRDadhTcb", longopts, NULL)) != -1) {
switch (c) {
case 's':
w_start = atoi(optarg);
@ -515,6 +521,9 @@ int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxD
case 'R':
types |= PointType::USE_REFLECTANCE;
break;
case 'D':
types |= PointType::USE_TEMPERATURE;
break;
case 'a':
types |= PointType::USE_AMPLITUDE;
break;

13
src/show/show_gl.cc
View file

@ -1905,15 +1905,18 @@ void mapColorToValue(int dummy) {
cm->setCurrentType(PointType::USE_REFLECTANCE);
break;
case 2:
cm->setCurrentType(PointType::USE_AMPLITUDE);
cm->setCurrentType(PointType::USE_TEMPERATURE);
break;
case 3:
cm->setCurrentType(PointType::USE_DEVIATION);
cm->setCurrentType(PointType::USE_AMPLITUDE);
break;
case 4:
cm->setCurrentType(PointType::USE_TYPE);
cm->setCurrentType(PointType::USE_DEVIATION);
break;
case 5:
cm->setCurrentType(PointType::USE_TYPE);
break;
case 6:
cm->setCurrentType(PointType::USE_COLOR);
break;
default:
@ -1930,6 +1933,7 @@ void changeColorMap(int dummy) {
JetMap jm;
HotMap hot;
DiffMap diff;
TempMap temp;
switch (listboxColorMapVal) {
case 0:
@ -1954,6 +1958,9 @@ void changeColorMap(int dummy) {
case 6:
cm->setColorMap(shsv);
break;
case 7:
cm->setColorMap(temp);
break;
default:
break;
}

3
src/show/show_menu.cc
View file

@ -259,6 +259,7 @@ void newMenu()
GLUI_RadioGroup *color_rog = glui1->add_radiogroup_to_panel( color_ro, &listboxColorVal, 0, &mapColorToValue );
glui1->add_radiobutton_to_group( color_rog, "height");
GLUI_RadioButton *rbrefl = glui1->add_radiobutton_to_group( color_rog, "reflectance");
GLUI_RadioButton *rbtemp = glui1->add_radiobutton_to_group( color_rog, "temperature");
GLUI_RadioButton *rbampl = glui1->add_radiobutton_to_group( color_rog, "amplitude");
GLUI_RadioButton *rbdevi = glui1->add_radiobutton_to_group( color_rog, "deviation");
GLUI_RadioButton *rbtype = glui1->add_radiobutton_to_group( color_rog, "type");
@ -267,6 +268,7 @@ void newMenu()
//if (!(types & PointType::USE_DEVIATION)) rbdevi->disable();
//if (!(types & PointType::USE_TYPE)) rbtype->disable();
if (!(pointtype.hasReflectance())) rbrefl->disable();
if (!(pointtype.hasTemperature())) rbtemp->disable();
if (!(pointtype.hasAmplitude())) rbampl->disable();
if (!(pointtype.hasDeviation())) rbdevi->disable();
if (!(pointtype.hasType())) rbtype->disable();
@ -282,6 +284,7 @@ void newMenu()
glui1->add_radiobutton_to_group(colorm_rog, "Hot");
glui1->add_radiobutton_to_group(colorm_rog, "Rand");
glui1->add_radiobutton_to_group(colorm_rog, "SHSV");
glui1->add_radiobutton_to_group(colorm_rog, "TEMP");
GLUI_Panel *scans_color = glui1->add_rollout_to_panel(color_panel, "Color type:", false);
scans_color->set_alignment(GLUI_ALIGN_LEFT);

10
src/show/wxshow.cc
View file

@ -21,6 +21,7 @@ class SelectionImpl : public Selection {
SelectionImpl( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Selection"), bool advanced_controls = false, const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( -1,-1 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL ) : Selection(parent, id, title, pos, size, style, advanced_controls) {
if (pointtype.hasReflectance()) m_choice11->Append(wxT("reflectance"));
if (pointtype.hasTemperature()) m_choice11->Append(wxT("temperature"));
if (pointtype.hasAmplitude()) m_choice11->Append(wxT("amplitude"));
if (pointtype.hasDeviation()) m_choice11->Append(wxT("deviation"));
if (pointtype.hasType()) m_choice11->Append(wxT("type"));
@ -89,16 +90,21 @@ class SelectionImpl : public Selection {
break;
}
case 2:
if (pointtype.hasTemperature()) {
cm->setCurrentType(PointType::USE_TEMPERATURE);
break;
}
case 3:
if (pointtype.hasAmplitude()) {
cm->setCurrentType(PointType::USE_AMPLITUDE);
break;
}
case 3:
case 4:
if (pointtype.hasDeviation()) {
cm->setCurrentType(PointType::USE_DEVIATION);
break;
}
case 4:
case 5:
if (pointtype.hasType()) {
cm->setCurrentType(PointType::USE_TYPE);
break;

24
src/slam6d/CMakeLists.txt
View file

@ -1,36 +1,32 @@
### SCAN_RED
### TOOLS
IF(WITH_SCAN_REDUCTION)
add_executable(scan_red scan_red.cc)
IF(WITH_TOOLS)
FIND_PACKAGE(OpenCV REQUIRED)
### SCAN_RED
add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc)
IF(UNIX)
target_link_libraries(scan_red scan dl ANN)
target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS})
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_red scan ANN XGetopt)
ENDIF(WIN32)
ENDIF(WITH_SCAN_REDUCTION)
### SCAN_DIFF
IF(WITH_SCAN_DIFF)
### SCAN_DIFF
add_executable(scan_diff scan_diff.cc)
add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc)
# add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc)
IF(UNIX)
target_link_libraries(scan_diff scan dl ANN)
target_link_libraries(scan_diff2d scan dl ANN)
# target_link_libraries(scan_diff2d scan dl ANN)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_diff scan ANN XGetopt)
target_link_libraries(scan_diff2d scan ANN XGetopt)
# target_link_libraries(scan_diff2d scan ANN XGetopt)
ENDIF(WIN32)
ENDIF(WITH_SCAN_DIFF)
### TOOLS
IF(WITH_TOOLS)
add_executable(frame_to_graph frame_to_graph.cc)
add_executable(convergence convergence.cc)
add_executable(graph_balancer graph_balancer.cc)

7
src/slam6d/basicScan.cc
View file

@ -137,6 +137,7 @@ void BasicScan::get(unsigned int types)
vector<double> xyz;
vector<unsigned char> rgb;
vector<float> reflectance;
vector<float> temperature;
vector<float> amplitude;
vector<int> type;
vector<float> deviation;
@ -155,6 +156,7 @@ void BasicScan::get(unsigned int types)
&xyz,
&rgb,
&reflectance,
&temperature,
&amplitude,
&type,
&deviation);
@ -172,6 +174,10 @@ void BasicScan::get(unsigned int types)
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_TEMPERATURE && !temperature.empty()) {
float* data = reinterpret_cast<float*>(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer());
for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i];
}
if(types & DATA_AMPLITUDE && !amplitude.empty()) {
int* data = reinterpret_cast<int*>(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer());
for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i];
@ -197,6 +203,7 @@ DataPointer BasicScan::get(const std::string& identifier)
if(identifier == "xyz") get(DATA_XYZ); else
if(identifier == "rgb") get(DATA_RGB); else
if(identifier == "reflectance") get(DATA_REFLECTANCE); else
if(identifier == "temperature") get(DATA_TEMPERATURE); else
if(identifier == "amplitude") get(DATA_AMPLITUDE); else
if(identifier == "type") get(DATA_TYPE); else
if(identifier == "deviation") get(DATA_DEVIATION); else

156
src/slam6d/fbr/panorama.cc
View file

@ -554,6 +554,155 @@ namespace fbr{
}
}
void panorama::recoverPointCloud(const cv::Mat& range_image, const string& file ) {
std::ofstream scan_file (file.c_str());
//recover from EQUIRECTANGULAR projection
if(pMethod == EQUIRECTANGULAR) {
double xFactor = (double) range_image.size().width / 2 / M_PI;
//int widthMax = range_image.size().width - 1;
double yFactor = (double) range_image.size().height / ((MAX_ANGLE - MIN_ANGLE) / 360 * 2 * M_PI);
double heightLow = (0 - MIN_ANGLE) / 360 * 2 * M_PI;
int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float theta = (heightMax - row) / yFactor - heightLow;
float phi = col / xFactor;
phi *= 180.0 / M_PI;
phi = 360.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
//recover from CYLINDRICAL projection
if(pMethod == CYLINDRICAL) {
double xFactor = (double) range_image.size().width / 2 / M_PI;
//int widthMax = range_image.size().width - 1;
double yFactor = (double) range_image.size().height / (tan(MAX_ANGLE / 360 * 2 * M_PI) - tan(MIN_ANGLE / 360 * 2 * M_PI));
double heightLow = (MIN_ANGLE) / 360 * 2 * M_PI;
//int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float theta = atan2(row + yFactor * tan(heightLow), yFactor);
float phi = col / xFactor;
phi *= 180.0 / M_PI;
phi = 360.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
//recover from MERCATOR projection
if(pMethod == MERCATOR) {
double xFactor = (double) range_image.size().width / 2 / M_PI;
double yFactor = (double) range_image.size().height / ( log( tan( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan( MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI) ) ) );
double heightLow = log(tan(MIN_ANGLE / 360 * 2 * M_PI) + (1/cos(MIN_ANGLE / 360 * 2 * M_PI)));
int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float theta = 2 * atan2(exp((heightMax - row) / yFactor + heightLow), 1.) - M_PI_2;
float phi = col / xFactor;
phi *= 180.0 / M_PI;
phi = 180.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
//recover from CONIC projection
if(pMethod == CONIC) {
// set up maximum latitude and longitude angles of the robot
double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0, MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0,
MIN_HORIZ_ANGLE = -M_PI, MAX_HORIZ_ANGLE = M_PI;
// set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html
double Lat0 = 0., Long0 = 0.;
double Phi1 = -40. * M_PI / 180.0, Phi2 = 60 * M_PI / 180.0;
double n = (sin(Phi1) + sin(Phi2)) / 2.;
double C = sqr(cos(Phi1)) + 2 * n * sin(Phi1);
double Rho0 = sqrt(C - 2 * n * sin(Lat0)) / n;
// set up max values for x and y and add the longitude to x axis and latitude to y axis
double xmax = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MAX_HORIZ_ANGLE - Long0));
double xmin = (1./n * sqrt(C - 2*n*sin( MIN_VERT_ANGLE )) ) * sin(n * (MIN_HORIZ_ANGLE - Long0));
double xFactor = (double) range_image.size().width / ( xmax - xmin );
double ymin = Rho0 - (1./n * sqrt(C - 2*n*sin(MIN_VERT_ANGLE)) ) * cos(n * ( 0. - Long0 ));
double ymax = Rho0 - (1./n * sqrt(C - 2*n*sin(MAX_VERT_ANGLE)) ) * cos(n * (MAX_HORIZ_ANGLE - Long0 ));
double yFactor = (double) range_image.size().height / ( ymax - ymin );
int heightMax = range_image.size().height - 1;
bool first_seen = true;
for (int row = 0; row < range_image.size().height; ++row) {
for (int col = 0; col < range_image.size().width; ++col) {
float range = range_image.at<float>(row, col);
float x = col * 1. / xFactor - fabs(xmin);
float y = (heightMax - row) * 1. / yFactor - fabs(ymin);
float theta = asin((C - (x*x + (Rho0 - y) * (Rho0 - y)) * n * n) / (2 * n));
float phi = Long0 + (1./n) * atan2(x, Rho0 - y);
phi *= 180.0 / M_PI;
phi = 360.0 - phi;
phi *= M_PI / 180.0;
theta *= 180.0 / M_PI;
theta *= -1;
theta += 90.0;
theta *= M_PI / 180.0;
double polar[3] = { theta, phi, range }, cartesian[3] = {0., 0., 0.};
toKartesian(polar, cartesian);
//if ( std::isnan(cartesian[0]) || std::isnan(cartesian[1]) || std::isnan(cartesian[2]) ) continue;
if( fabs(cartesian[0]) < 1e-5 && fabs(cartesian[1]) < 1e-5 && fabs(cartesian[2]) < 1e-5) {
if (first_seen) first_seen = false;
else continue;
}
scan_file << -100. * cartesian[1] << " " << 100. * cartesian[2] << " " << 100. * cartesian[0] << endl;
}
}
}
scan_file.close();
}
unsigned int panorama::getImageWidth(){
return iWidth;
}
@ -595,8 +744,11 @@ namespace fbr{
}
void panorama::getDescription(){
cout<<"panorama created with width: "<<iWidth<<", and height: "<<iHeight<<", and projection method: "<<projectionMethodToString(pMethod)<<", number of images: "<<nImages<<", projection param: "<<pParam<<"."<<endl;
cout<<endl;
cout << "panorama created with width: " << iWidth << ", and height: "
<< iHeight << ", and projection method: " << projectionMethodToString(pMethod)
<< ", number of images: " << nImages << ", projection param: " << pParam << "."
<< endl;
cout << endl;
}
}

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