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

update svn to r733

Browse source
This commit is contained in:
Razvan Mihalyi 2012-10-24 11:22:27 +02:00
parent 9bf8e054d0
commit ab0dc46ed6
41 changed files with 1679 additions and 1182 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
.svn/wc.db
View file

Binary file not shown.

6
CMakeLists.txt
View file

@ -1,4 +1,4 @@
cmake_minimum_required (VERSION 2.6.1)
cmake_minimum_required (VERSION 2.8.2)
SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH})
project (Slam6D)
@ -254,7 +254,7 @@ 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")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${NUMBER_OF_CPUS} -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")
@ -395,7 +395,7 @@ 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)
target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s)
ENDIF(EXPORT_SHARED_LIBS)
MESSAGE (STATUS "Build environment is set up!")

5
CONTRIBUTORS
View file

@ -25,12 +25,13 @@ Mohammad Faisal Abdullah m.faisal@jacobs-university.de
Li Ming liming751218@whu.edu.cn
Li Wei xpaulee@gmail.com
Shams Feyzabadi sh.feyzabadi@gmail.co
Vladislav Perelmann v.perelman@jacobs-university.de
Vladislav Perelmann v.perelman@jacobs-university.de
Chen Long lchen.whu@gmail.com
Remuas Dumitru r.dumitru@jaocbs-university.de
Remus Dumitru r.dumitru@jaocbs-university.de
Billy Okal okal.billy@googlemail.com
Razvan-George Mihalyi r.mihalyi@jacobs-university.de
Johannes Schauer j.schauer@jacobs-university.de
Corneliu-Claudiu Prodescu c.prodescu@jacobs-university.de
Further contributors

1
README.scanserver
View file

@ -45,6 +45,7 @@ If a great portion of the RAM is used for the cache data, swapping will usually
* soft memlock unlimited
* hard memlock unlimited
After adding these lines and rebooting the system the scanserver can be started without superuser rights.
5. Using the octtree serialization feature in show with scanserver

1
include/shapes/ransac.h
View file

@ -16,7 +16,6 @@ void Ransac(CollisionShape<T> &shape, Scan *scan, vector<T*> *best_points = 0) {
// create octree from the points
DataXYZ xyz(scan->get("xyz reduced"));
RansacOctTree<T> *oct = new RansacOctTree<T>(PointerArray<double>(xyz).get(), xyz.size(), 50.0 );
cout << "start 5000 iterations" << endl;
for(int i = 0; i < 5000; i++) {
ps.clear();

12
include/shapes/ransac_Boctree.h
View file

@ -108,7 +108,7 @@ void showbits(char a)
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
// check if leaf contains shape
if ( shape.isInCube(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
pointrep *points = children->points;
pointrep *points = children->getPointreps();
unsigned int length = points[0].length;
T *point = &(points[1].v); // first point
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
@ -121,6 +121,7 @@ void showbits(char a)
result += PointsOnShape( children->node, ccenter, size/2.0, shape);
}
++children; // next child
//r++;
}
}
@ -142,7 +143,7 @@ void showbits(char a)
if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center
// check if leaf contains shape
if ( shape.isInCube(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
pointrep *points = children->points;
pointrep *points = children->getPointreps();
unsigned int length = points[0].length;
T *point = &(points[1].v); // first point
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
@ -169,7 +170,6 @@ void showbits(char a)
void DrawPoints(vector<T *> &p, bitoct &node, unsigned char nrp) {
bitunion<T> *children;
bitoct::getChildren(node, children);
unsigned char n_children = POPCOUNT(node.valid);
unsigned char r = randUC(n_children);
if (r == n_children) r--;
@ -217,8 +217,9 @@ void showbits(char a)
if (leaf) {
/* cout << "STOPPED" << endl;
return;*/
pointrep *points = children[r].points;
return;*/
pointrep *points = children[r].getPointreps();
unsigned int length = points[0].length;
if (length < nrp) return;
if (length == nrp) {
@ -227,7 +228,6 @@ void showbits(char a)
}
return;
}
// randomly get nrp points, we will not check if this succeeds in getting nrp distinct points
for (char c = 0; c < nrp; c++) {
int tmp = rand(points[0].length);

7
include/show/show.icc
View file

@ -5,11 +5,13 @@
@author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __SHOW_ICC__
#define __SHOW_ICC__
/**
sets the OpenGL point,
(z axis is inverted in OpenGL)
*/
void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ)
inline void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ)
{
// pZ *= -1.0;
glVertex3d(pX, pY, pZ);
@ -20,9 +22,10 @@ void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ)
sets the OpenGL point,
(z axis is inverted in OpenGL)
*/
void setGLPoint(GLdouble* p)
inline void setGLPoint(GLdouble* p)
{
GLdouble pZ = 1.0 * p[2];
glVertex3d(p[0], p[1], pZ);
}
#endif

3
include/show/show1.icc
View file

@ -4,6 +4,8 @@
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __SHOW1_ICC__
#define __SHOW1_ICC__
#include "slam6d/globals.icc"
@ -119,3 +121,4 @@ inline void QuaternionToAxisAngle(const double *quat, double *axis, double &angl
axis[1] = normalized_quat[1] / sin_a;
axis[2] = -1.0 * normalized_quat[2] / sin_a;
}
#endif

2
include/show/show_Boctree.h
View file

@ -15,7 +15,7 @@
#include "show/colordisplay.h"
#include "slam6d/scan.h"
using namespace show;
/**
* @brief Octree for show

11
include/show/viewcull.h
View file

@ -7,14 +7,19 @@
#include <windows.h>
#include <GL/glu.h>
#include <GL/glut.h>
#elif __APPLE__
#include <GLUT/glut.h>
#include <OpenGl/glu.h>
#include <stdbool.h>
#else
#include <glut.h>
#include <glu.h>
#include <GL/glut.h>
#include <GL/glu.h>
#include <stdbool.h>
#endif
#ifndef __VIEWCULL_H__
#define __VIEWCULL_H__
namespace show{
///////////////////////// Variable declarations...
/** The 6 planes of the viewing frustum */
@ -161,5 +166,5 @@ void remViewport();
bool LOD(float x, float y, float z, float size);
int LOD2(float x, float y, float z, float size);
}
#endif

2
include/slam6d/ann_kd.h
View file

@ -30,7 +30,7 @@ public:
/**
* Constructor using the point set pts and the num_of_pts n
*/
ANNtree(double **_pts, int n);
ANNtree(PointerArray<double>&_pts, int n);
/**
* destructor

6
include/slam6d/basicScan.h
View file

@ -10,6 +10,8 @@
class BasicScan : public Scan {
public:
BasicScan() {};
static void openDirectory(const std::string& path, IOType type, int start, int end);
static void closeDirectory();
/*
@ -29,12 +31,14 @@ public:
virtual void get(unsigned int types);
virtual DataPointer create(const std::string& identifier, unsigned int size);
virtual void clear(const std::string& identifier);
virtual unsigned int readFrames();
virtual void saveFrames();
virtual unsigned int getFrameCount();
virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type);
//! Constructor for creation of Scans without openDirectory
BasicScan(double * rPos, double * rPosTheta, std::vector<double*> points);
protected:
virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate();

15
include/slam6d/globals.icc
View file

@ -25,6 +25,21 @@ inline int gettimeofday (struct timeval* tp, void* tzp)
#include <sys/time.h>
#endif
#define _USE_MATH_DEFINES
#include <math.h>
#if defined(__CYGWIN__)
# ifndef M_PI
# define M_PI 3.14159265358979323846
# define M_PI_2 1.57079632679489661923
# define M_PI_4 0.78539816339744830962
# define M_1_PI 0.31830988618379067154
# define M_2_PI 0.63661977236758134308
# define M_SQRT2 1.41421356237309504880
# define M_SQRT1_2 0.70710678118654752440
# endif
#endif
#include <algorithm>
using std::min;
using std::max;

66
include/slam6d/kd.h
View file

@ -10,6 +10,7 @@
#include "slam6d/kdparams.h"
#include "slam6d/searchTree.h"
#include "slam6d/kdTreeImpl.h"
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
@ -21,6 +22,15 @@
#include <omp.h>
#endif
struct Void { };
struct PtrAccessor {
inline double *operator() (Void, double* indices) {
return indices;
}
};
/**
* @brief The optimized k-d tree.
*
@ -28,67 +38,13 @@
* capabilities (find nearest point to
* a given point, or to a ray).
**/
class KDtree : public SearchTree {
class KDtree : public SearchTree, private KDTreeImpl<Void, double*, PtrAccessor> {
public:
KDtree(double **pts, int n);
virtual ~KDtree();
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
protected:
/**
* storing the parameters of the k-d tree, i.e., the current closest point,
* the distance to the current closest point and the point itself.
* These global variable are needed in this search.
*
* Padded in the parallel case.
*/
#ifdef _OPENMP
#ifdef __INTEL_COMPILER
__declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS];
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif //__INTEL_COMPILER
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif
/**
* number of points. If this is 0: intermediate node. If nonzero: leaf.
*/
int npts;
/**
* Cue the standard rant about anon unions but not structs in C++
*/
union {
/**
* in case of internal node...
*/
struct {
double center[3]; ///< storing the center of the voxel (R^3)
double dx, ///< defining the voxel itself
dy, ///< defining the voxel itself
dz, ///< defining the voxel itself
r2; ///< defining the voxel itself
int splitaxis; ///< defining the kind of splitaxis
KDtree *child1; ///< pointers to the childs
KDtree *child2; ///< pointers to the childs
} node;
/**
* in case of leaf node ...
*/
struct {
/**
* store the value itself
* Here we store just a pointer to the data
*/
double **p;
} leaf;
};
void _FindClosest(int threadNum) const;
};
#endif

75
include/slam6d/kdManaged.h
View file

@ -11,6 +11,7 @@
#include "slam6d/kdparams.h"
#include "slam6d/searchTree.h"
#include "slam6d/data_types.h"
#include "slam6d/kdTreeImpl.h"
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
@ -25,6 +26,14 @@
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
class Scan;
struct ArrayAccessor {
inline double *operator() (const DataXYZ& pts, unsigned int i) {
return pts[i];
}
};
/**
* @brief The optimized k-d tree.
*
@ -32,69 +41,11 @@
* capabilities (find nearest point to
* a given point, or to a ray).
**/
class KDtreeManagedBase : public SearchTree {
public:
KDtreeManagedBase(const DataXYZ& pts, unsigned int* indices, unsigned int n);
class KDtreeManaged :
public SearchTree,
private KDTreeImpl<const DataXYZ&, unsigned int, ArrayAccessor>
virtual ~KDtreeManagedBase();
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const { return 0; }
protected:
/**
* storing the parameters of the k-d tree, i.e., the current closest point,
* the distance to the current closest point and the point itself.
* These global variable are needed in this search.
*
* Padded in the parallel case.
*/
#ifdef _OPENMP
#ifdef __INTEL_COMPILER
__declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS];
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif //__INTEL_COMPILER
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif
/**
* number of points. If this is 0: intermediate node. If nonzero: leaf.
*/
int npts;
/**
* Cue the standard rant about anon unions but not structs in C++
*/
union {
/**
* in case of internal node...
*/
struct {
double center[3]; ///< storing the center of the voxel (R^3)
double dx, ///< defining the voxel itself
dy, ///< defining the voxel itself
dz, ///< defining the voxel itself
r2; ///< defining the voxel itself
int splitaxis; ///< defining the kind of splitaxis
KDtreeManagedBase *child1; ///< pointers to the childs
KDtreeManagedBase *child2; ///< pointers to the childs
} node;
/**
* in case of leaf node ...
*/
struct {
//! Content is an array of indices to be put into the dynamically aquired data array
unsigned int* p;
} leaf;
};
void _FindClosest(const DataXYZ& pts, int threadNum) const;
};
class Scan;
class KDtreeManaged : public KDtreeManagedBase {
{
public:
KDtreeManaged(Scan* scan);
virtual ~KDtreeManaged() {}

87
include/slam6d/kdMeta.h
View file

@ -11,6 +11,7 @@
#include "kdparams.h"
#include "searchTree.h"
#include "data_types.h"
#include "kdTreeImpl.h"
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
@ -30,6 +31,16 @@
class Scan;
struct Index {
unsigned int s, i;
inline void set(unsigned int _s, unsigned int _i) { s = _s; i = _i; }
};
struct IndexAccessor {
inline double* operator() (const DataXYZ* const* pts, const Index& i) const {
return (*pts[i.s])[i.i];
}
};
/**
* @brief The optimized k-d tree.
@ -38,78 +49,10 @@ class Scan;
* capabilities (find nearest point to
* a given point, or to a ray).
**/
class KDtreeMeta : public SearchTree {
protected:
class Index {
public:
unsigned int s, i;
inline void set(unsigned int _s, unsigned int _i) { s = _s; i = _i; }
};
public:
KDtreeMeta();
virtual ~KDtreeMeta();
void create(const DataXYZ* const* pts, Index* indices, unsigned int n);
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const { return 0; }
protected:
/**
* storing the parameters of the k-d tree, i.e., the current closest point,
* the distance to the current closest point and the point itself.
* These global variable are needed in this search.
*
* Padded in the parallel case.
*/
#ifdef _OPENMP
#ifdef __INTEL_COMPILER
__declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS];
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif //__INTEL_COMPILER
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif
/**
* number of points. If this is 0: intermediate node. If nonzero: leaf.
*/
int npts;
/**
* Cue the standard rant about anon unions but not structs in C++
*/
union {
/**
* in case of internal node...
*/
struct {
double center[3]; ///< storing the center of the voxel (R^3)
double dx, ///< defining the voxel itself
dy, ///< defining the voxel itself
dz, ///< defining the voxel itself
r2; ///< defining the voxel itself
int splitaxis; ///< defining the kind of splitaxis
KDtreeMeta *child1; ///< pointers to the childs
KDtreeMeta *child2; ///< pointers to the childs
} node;
/**
* in case of leaf node ...
*/
struct {
//! Content is an array of indices to be put into the dynamically aquired data array
Index* p;
} leaf;
};
inline double* point(const DataXYZ* const* pts, const Index& i) const {
return (*pts[i.s])[i.i];
}
void _FindClosest(const DataXYZ* const* pts, int threadNum) const;
};
class KDtreeMetaManaged : public KDtreeMeta {
class KDtreeMetaManaged :
public SearchTree,
private KDTreeImpl<const DataXYZ* const*, Index, IndexAccessor>
{
public:
KDtreeMetaManaged(const vector<Scan*>& scans);
virtual ~KDtreeMetaManaged();

11
include/slam6d/scan.h
View file

@ -103,8 +103,8 @@ public:
};
// delete copy-ctor and assignment, scans shouldn't be copied by basic class
// Scan(const Scan& other) = delete;
// Scan& operator=(const Scan& other) = delete;
Scan(const Scan& other) = delete;
Scan& operator=(const Scan& other) = delete;
virtual ~Scan();
@ -256,10 +256,8 @@ protected:
public:
/* Direct creation of reduced points and search tree */
//! Apply reduction and initial transMatOrg transformation
void toGlobal();
@ -406,9 +404,10 @@ private:
//! flag for openDirectory and closeDirectory to distinguish the scans
static bool scanserver;
public:
//! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment
// it can not be compiled in wein32 use boost 1.48, therefore we remeove it temporarily
// boost::mutex m_mutex_reduction, m_mutex_create_tree;
// it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily
boost::mutex m_mutex_reduction, m_mutex_create_tree;
};
#include "scan.icc"

9
include/thermo/thermo.h
View file

@ -6,14 +6,15 @@
#include <string>
#include <slam6d/scan.h>
using namespace std;
typedef vector<vector<float> > Float2D[1200][1600];
//typedef vector<vector<float> > Float2D[1200][1600];
typedef vector<vector<float> > Float2D[2592][3888];
void calcBoard(double point_array[][2], int board_n, double &x, double &y, double &cx, double &cy, bool pc);
void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, bool quiet);
IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]);
void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color=false);
IplImage* resizeImage(IplImage *source, int scale);
IplImage* detectCorners(IplImage *orgimage, int corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale=1);
IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale=1);
void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1);
void writeCalibParam(int images, int corner_exp, int board_w, CvMat* image_points, CvSize size, string dir);
@ -27,8 +28,8 @@ void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat *
points3D, CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, CvMat
* distortions, CvMat * instrinsics, int corners, int successes, string dir, bool quiet=true, string substring = "") ;
void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, int successes, string dir, bool quiet=true, string substring = "") ;
void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size);
void clusterSearch(float ** points, int size, double thresh1, double thresh2, fstream &outfile);
void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical);
void clusterSearch(float ** points, int size, double thresh1, double thresh2, fstream &outfile, bool optical);
void sortDistances(float ** points, int size);
void ExtrCalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1);

4
include/veloslam/veloscan.h
View file

@ -18,7 +18,7 @@
#include <vector>
#include <map>
#include "slam6d/scan.h"
#include "slam6d/basicScan.h"
#include "veloslam/gridcell.h"
#include "veloslam/gridcluster.h"
@ -36,7 +36,7 @@ class Trajectory
/**
* @brief 3D scan representation and implementation of dynamic velodyne scan matching
*/
class VeloScan : public Scan {
class VeloScan : public BasicScan {
public:
VeloScan();

15
src/scanio/scan_io_velodyne.cc
View file

@ -365,8 +365,12 @@ int read_one_packet (
{
#ifdef _MSC_VER
fseek(fp , BLOCK_OFFSET, SEEK_CUR);
#else
#ifdef __CYGWIN__
fseek(fp , BLOCK_OFFSET, SEEK_CUR);
#else
fseeko(fp , BLOCK_OFFSET, SEEK_CUR);
#endif
#endif
size=fread ( buf, 1, BLOCK_SIZE, fp );
@ -533,8 +537,12 @@ void ScanIO_velodyne::readScan(
#ifdef _MSC_VER
scan_in = fopen(filename,"rb");
#else
#ifdef __CYGWIN__
scan_in = fopen(filename,"rb");
#else
scan_in = fopen64(filename,"rb");
#endif
#endif
if(scan_in == NULL)
@ -552,12 +560,17 @@ void ScanIO_velodyne::readScan(
fileCounter= atoi(identifier);
#ifdef _MSC_VER
#ifdef _MSC_VER
fseek(scan_in, 24, SEEK_SET);
fseek(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR);
#else
#ifdef __CYGWIN__
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
#endif
read_one_packet(

6
src/segmentation/CMakeLists.txt
View file

@ -1,7 +1,11 @@
IF(WITH_SEGMENTATION)
add_executable(scan2segments scan2segments.cc)
add_executable(scan2segments scan2segments.cc ../slam6d/fbr/fbr_global.cc)
add_executable(fhsegmentation fhsegmentation.cc FHGraph.cc disjoint-set.cc segment-graph.cc)
FIND_PACKAGE(OpenCV REQUIRED)
target_link_libraries(scan2segments scan ANN fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${Boost_LIBRARIES} ${OpenCV_LIBS})
target_link_libraries(fhsegmentation scan ANN ${Boost_LIBRARIES} ${OpenCV_LIBS})
ENDIF(WITH_SEGMENTATION)

159
src/segmentation/scan2segments.cc
View file

@ -24,12 +24,15 @@ namespace po = boost::program_options;
#include "slam6d/fbr/panorama.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <sys/stat.h>
#include <sys/types.h>
enum image_type {M_RANGE, M_INTENSITY};
enum segment_method {THRESHOLD, ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, WATERSHED};
enum segment_method {THRESHOLD, ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, PYR_SEGMENTATION, WATERSHED};
/* Function used to check that 'opt1' and 'opt2' are not specified
at the same time. */
@ -87,6 +90,7 @@ void validate(boost::any& v, const std::vector<std::string>& values,
if(strcasecmp(arg.c_str(), "THRESHOLD") == 0) v = THRESHOLD;
else if(strcasecmp(arg.c_str(), "ADAPTIVE_THRESHOLD") == 0) v = ADAPTIVE_THRESHOLD;
else if(strcasecmp(arg.c_str(), "PYR_MEAN_SHIFT") == 0) v = PYR_MEAN_SHIFT;
else if(strcasecmp(arg.c_str(), "PYR_SEGMENTATION") == 0) v = PYR_SEGMENTATION;
else if(strcasecmp(arg.c_str(), "WATERSHED") == 0) v = WATERSHED;
else throw std::runtime_error(std::string("segmentation method ") + arg + std::string(" is unknown"));
}
@ -106,6 +110,23 @@ void validate(boost::any& v, const std::vector<std::string>& values,
}
}
void segmentation_option_dependency(const po::variables_map & vm, segment_method stype, const char *option)
{
if (vm.count("segment") && vm["segment"].as<segment_method>() == stype) {
if (!vm.count(option)) {
throw std::logic_error (string("this segmentation option needs ")+option+" to be set");
}
}
}
void segmentation_option_conflict(const po::variables_map & vm, segment_method stype, const char *option)
{
if (vm.count("segment") && vm["segment"].as<segment_method>() == stype) {
if (vm.count(option)) {
throw std::logic_error (string("this segmentation option is incompatible with ")+option);
}
}
}
/*
* parse commandline options, fill arguments
*/
@ -114,7 +135,8 @@ void parse_options(int argc, char **argv, int &start, int &end,
fbr::projection_method &ptype, string &dir, IOType &iotype,
int &maxDist, int &minDist, int &nImages, int &pParam, bool &logarithm,
float &cutoff, segment_method &stype, string &marker, bool &dump_pano,
bool &dump_seg, double &thresh, int &maxlevel, int &radius)
bool &dump_seg, double &thresh, int &maxlevel, int &radius,
double &pyrlinks, double &pyrcluster, int &pyrlevels)
{
po::options_description generic("Generic options");
generic.add_options()
@ -169,7 +191,7 @@ void parse_options(int argc, char **argv, int &start, int &end,
segment.add_options()
("segment,g", po::value<segment_method>(&stype)->
default_value(PYR_MEAN_SHIFT), "segmentation method (THRESHOLD, "
"ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, WATERSHED)")
"ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, PYR_SEGMENTATION, WATERSHED)")
("marker,K", po::value<string>(&marker),
"marker mask for watershed segmentation")
("thresh,T", po::value<double>(&thresh),
@ -178,6 +200,13 @@ void parse_options(int argc, char **argv, int &start, int &end,
"maximum level for meanshift segmentation")
("radius,R", po::value<int>(&radius),
"radius for meanshift segmentation")
("links,l", po::value<double>(&pyrlinks),
"error threshold for establishing the links for pyramid segmentation")
("clustering,c", po::value<double>(&pyrcluster),
"error threshold for the segments clustering for pyramid "
"segmentation")
("levels,E", po::value<int>(&pyrlevels)->default_value(4),
"levels of pyramid segmentation")
("dump-seg,D", po::bool_switch(&dump_seg),
"output segmentation image (for debugging)");
@ -206,6 +235,9 @@ void parse_options(int argc, char **argv, int &start, int &end,
// display help
if (vm.count("help")) {
cout << cmdline_options;
cout << "\nExample usage:\n"
<< "bin/scan2segments -s 0 -e 0 -f riegl_txt --segment PYR_SEGMENTATION -l 50 -c 50 -E 4 -D -i ~/path/to/bremen_city\n"
<< "bin/scan2segments -s 0 -e 0 -f riegl_txt --segment PYR_SEGMENTATION -l 255 -c 30 -E 2 -D -i ~/path/to/bremen_city\n";
exit(0);
}
@ -220,37 +252,36 @@ void parse_options(int argc, char **argv, int &start, int &end,
else
itype = M_INTENSITY;
// option dependencies on segmentation types
if (stype == WATERSHED) {
if (!vm.count("marker"))
throw std::logic_error("watershed segmentation requires --marker to be set");
if (vm.count("thresh"))
throw std::logic_error("watershed segmentation cannot be used with --thresh");
if (vm.count("maxlevel"))
throw std::logic_error("watershed segmentation cannot be used with --maxlevel");
if (vm.count("radius"))
throw std::logic_error("watershed segmentation cannot be used with --radius");
}
if (stype == THRESHOLD) {
if (!vm.count("thresh"))
throw std::logic_error("threshold segmentation requires --thresh to be set");
if (vm.count("marker"))
throw std::logic_error("threshold segmentation cannot be used with --marker");
if (vm.count("maxlevel"))
throw std::logic_error("threshold segmentation cannot be used with --maxlevel");
if (vm.count("radius"))
throw std::logic_error("threshold segmentation cannot be used with --radius");
}
if (stype == PYR_MEAN_SHIFT) {
if (!vm.count("maxlevel"))
throw std::logic_error("mean shift segmentation requires --maxlevel to be set");
if (!vm.count("radius"))
throw std::logic_error("mean shift segmentation requires --radius to be set");
if (vm.count("thresh"))
throw std::logic_error("mean shift segmentation cannot be used with --thresh");
if (vm.count("marker"))
throw std::logic_error("mean shift segmentation cannot be used with --marker");
}
segmentation_option_dependency(vm, WATERSHED, "marker");
segmentation_option_conflict(vm, WATERSHED, "thresh");
segmentation_option_conflict(vm, WATERSHED, "maxlevel");
segmentation_option_conflict(vm, WATERSHED, "radius");
segmentation_option_conflict(vm, WATERSHED, "links");
segmentation_option_conflict(vm, WATERSHED, "clustering");
segmentation_option_conflict(vm, WATERSHED, "levels");
segmentation_option_conflict(vm, THRESHOLD, "marker");
segmentation_option_dependency(vm, THRESHOLD, "thresh");
segmentation_option_conflict(vm, THRESHOLD, "maxlevel");
segmentation_option_conflict(vm, THRESHOLD, "radius");
segmentation_option_conflict(vm, THRESHOLD, "links");
segmentation_option_conflict(vm, THRESHOLD, "clustering");
segmentation_option_conflict(vm, THRESHOLD, "levels");
segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "marker");
segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "thresh");
segmentation_option_dependency(vm, PYR_MEAN_SHIFT, "maxlevel");
segmentation_option_dependency(vm, PYR_MEAN_SHIFT, "radius");
segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "links");
segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "clustering");
segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "levels");
segmentation_option_conflict(vm, PYR_SEGMENTATION, "marker");
segmentation_option_conflict(vm, PYR_SEGMENTATION, "thresh");
segmentation_option_conflict(vm, PYR_SEGMENTATION, "maxlevel");
segmentation_option_conflict(vm, PYR_SEGMENTATION, "radius");
segmentation_option_dependency(vm, PYR_SEGMENTATION, "links");
segmentation_option_dependency(vm, PYR_SEGMENTATION, "clustering");
// correct pParam and nImages for certain panorama types
if (ptype == fbr::PANNINI && pParam == 0) {
@ -430,6 +461,60 @@ cv::Mat calculatePyrMeanShift(vector<vector<cv::Vec3f>> &segmented_points,
return res;
}
///TODO: need to pass *two* thresh params, see: http://bit.ly/WmFeub
cv::Mat calculatePyrSegmentation(vector<vector<cv::Vec3f>> &segmented_points,
cv::Mat &img, vector<vector<vector<cv::Vec3f> > > extendedMap,
double thresh1, double thresh2, int pyrlevels)
{
int i, j, idx;
int block_size = 1000;
IplImage ipl_img = img;
IplImage* ipl_original = &ipl_img;
IplImage* ipl_segmented = 0;
CvMemStorage* storage = cvCreateMemStorage(block_size);
CvSeq* comp = NULL;
// the following lines are required because the level must not be more
// than log2(min(width, height))
ipl_original->width &= -(1<<pyrlevels);
ipl_original->height &= -(1<<pyrlevels);
ipl_segmented = cvCloneImage( ipl_original );
// apply the pyramid segmentation algorithm
cvPyrSegmentation(ipl_original, ipl_segmented, storage, &comp, pyrlevels, thresh1+1, thresh2+1);
// mapping of color value to component id
map<int, int> mapping;
unsigned int segments = comp->total;
for (unsigned int cur_seg = 0; cur_seg < segments; ++cur_seg) {
CvConnectedComp* cc = (CvConnectedComp*) cvGetSeqElem(comp, cur_seg);
// since images are single-channel grayscale, only the first value is
// of interest
mapping.insert(pair<int, int>(cc->value.val[0], cur_seg));
}
segmented_points.resize(segments);
uchar *data = (uchar *)ipl_segmented->imageData;
int step = ipl_segmented->widthStep;
for (i = 0; i < ipl_segmented->height; i++) {
for (j = 0; j < ipl_segmented->width; j++) {
idx = mapping[data[i*step+j]];
segmented_points[idx].insert(segmented_points[idx].end(),
extendedMap[i][j].begin(),
extendedMap[i][j].end());
}
}
// clearing memory
cvReleaseMemStorage(&storage);
cv::Mat res(ipl_segmented);
return res;
}
/*
* calculate the adaptive threshold
*/
@ -572,11 +657,13 @@ int main(int argc, char **argv)
bool dump_pano, dump_seg;
double thresh;
int maxlevel, radius;
double pyrlinks, pyrcluster;
int pyrlevels;
parse_options(argc, argv, start, end, scanserver, itype, width, height,
ptype, dir, iotype, maxDist, minDist, nImages, pParam, logarithm,
cutoff, stype, marker, dump_pano, dump_seg, thresh, maxlevel,
radius);
radius, pyrlinks, pyrcluster, pyrlevels);
Scan::openDirectory(scanserver, dir, iotype, start, end);
@ -623,6 +710,8 @@ int main(int argc, char **argv)
} else if (stype == PYR_MEAN_SHIFT) {
res = calculatePyrMeanShift(segmented_points, img, fPanorama.getExtendedMap(),
maxlevel, radius);
} else if (stype == PYR_SEGMENTATION) {
res = calculatePyrSegmentation(segmented_points, img, fPanorama.getExtendedMap(), pyrlinks, pyrcluster, pyrlevels);
} else if (stype == ADAPTIVE_THRESHOLD) {
res = calculateAdaptiveThreshold(segmented_points, img, fPanorama.getExtendedMap());
} else if (stype == WATERSHED) {

2
src/shapes/plane.cc
View file

@ -276,7 +276,7 @@ int main(int argc, char **argv)
if(!quiet) cout << nx << " " << ny << " " << nz << " " << d << endl;
/**
/*
for (unsigned int i = 0; i < points.size(); i++) {
cerr << points[i][0] << " " << points[i][1] << " " << points[i][2] << endl;
}

9
src/show/CMakeLists.txt
View file

@ -1,4 +1,4 @@
SET(SHOW_LIBS ${OPENGL_LIBRARIES} glui scan ANN)
SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES})
IF(WIN32)
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt)
@ -7,7 +7,7 @@ IF(WIN32)
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
ENDIF(WIN32)
IF (UNIX)
SET(SHOW_LIBS newmat dl ${GLUT_LIBRARIES} ${SHOW_LIBS})
SET(SHOW_LIBS newmat dl ${SHOW_LIBS} ${GLUT_LIBRARIES})
ENDIF(UNIX)
IF(WITH_GLEE)
@ -26,3 +26,8 @@ IF(WITH_WXSHOW)
target_link_libraries(wxshow ${wxWidgets_LIBRARIES} wxthings ${SHOW_LIBS})
ENDIF(WITH_WXSHOW)
### EXPORT SHARED LIBS
IF(EXPORT_SHARED_LIBS)
add_library(show_s SHARED ${SHOW_SRCS})
target_link_libraries(show_s ${SHOW_LIBS})
ENDIF(EXPORT_SHARED_LIBS)

1
src/show/compacttree.cc
View file

@ -38,6 +38,7 @@ using std::list;
#include "show/scancolormanager.h"
#include "show/viewcull.h"
using namespace show;
compactTree::~compactTree(){
delete alloc;

1
src/show/show_gl.cc
View file

@ -12,6 +12,7 @@
#include "show/viewcull.h"
#include "show/scancolormanager.h"
using namespace show;
bool fullydisplayed = true; // true if all points have been drawn to the screen
bool mousemoving = false; // true iff a mouse button has been pressed inside a window,
// but hs not been released

2
src/show/viewcull.cc
View file

@ -27,6 +27,7 @@
#endif
#include "slam6d/globals.icc"
namespace show{
/** The 6 planes of the viewing frustum */
float frustum[6][4];
@ -843,3 +844,4 @@ bool CubeInFrustum( float x, float y, float z, float size )
float minB[NUMDIM], maxB[NUMDIM]; /*box */
float coord[NUMDIM]; /* hit point */
}

1
src/slam6d/CMakeLists.txt
View file

@ -90,6 +90,7 @@ ENDIF(UNIX)
IF(EXPORT_SHARED_LIBS)
add_library(scan_s SHARED ${SCANLIB_SRCS})
#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat)
target_link_libraries(scan_s newmat sparse ANN scanclient pointfilter scanio)
ENDIF(EXPORT_SHARED_LIBS)
### SLAM6D

24
src/slam6d/ann_kd.cc
View file

@ -39,9 +39,26 @@ using std::swap;
* @param pts 3D array of points
* @param n number of points
*/
ANNtree::ANNtree(double **_pts, int n)
ANNtree::ANNtree(PointerArray<double>&_pts, int n)
{
pts = _pts;
/*
pts = new double*[n];
for(unsigned int i = 0; i < n; i++) {
pts[i] = new double[3];
pts[i][0] = _pts.get()[i][0];
pts[i][1] = _pts.get()[i][1];
pts[i][2] = _pts.get()[i][2];
}
*/
pts = new double*[n];
double* tpts = new double[3*n];
for(unsigned int i = 0, j = 0; i < n; i++) {
pts[i] = &tpts[j];
tpts[j++] = _pts.get()[i][0];
tpts[j++] = _pts.get()[i][1];
tpts[j++] = _pts.get()[i][2];
}
annkd = new ANNkd_tree(pts, n, 3, 1, ANN_KD_SUGGEST); // links to the constructor of ANNkd_tree
cout << "ANNkd_tree was generated with " << n << " points" << endl;
nn = new ANNdist[1];
@ -60,6 +77,8 @@ ANNtree::~ANNtree()
delete annkd; //links to the destructor of ANNkd_tree
delete [] nn;
delete [] nn_idx;
delete [] pts[0];
delete [] pts;
}
@ -76,6 +95,7 @@ double *ANNtree::FindClosest(double *_p, double maxdist2, int threadNum) const
#pragma omp critical
annkd->annkSearch(_p, 1, nn_idx, nn, 0.0);
int idx = nn_idx[0];
if (Dist2(_p, pts[idx]) > maxdist2) return 0;

34
src/slam6d/basicScan.cc
View file

@ -66,6 +66,38 @@ void BasicScan::closeDirectory()
Scan::allScans.clear();
}
BasicScan::BasicScan(double *_rPos, double *_rPosTheta, vector<double*> points) {
init();
for(int i = 0; i < 3; i++) {
rPos[i] = _rPos[i];
rPosTheta[i] = _rPosTheta[i];
}
// write original pose matrix
EulerToMatrix4(rPos, rPosTheta, 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);
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);
double* data = reinterpret_cast<double*>(create("xyz", sizeof(double) * 3 * points.size()).get_raw_pointer());
int tmp = 0;
for(unsigned int i = 0; i < points.size(); ++i) {
for(unsigned int j = 0; j < 3; j++) {
data[tmp++] = points[i][j];
}
}
}
BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOType type) :
m_path(path), m_identifier(identifier), m_type(type)
{
@ -274,7 +306,7 @@ void BasicScan::createSearchTreePrivate()
kd = new KDtree(ar.get(), xyz_orig.size());
break;
case ANNTree:
kd = new ANNtree(ar.get(), xyz_orig.size());
kd = new ANNtree(ar, xyz_orig.size());
break;
case BOCTree:
kd = new BOctTree<double>(ar.get(), xyz_orig.size(), 10.0, PointType(), true);

14
src/slam6d/fbr/CMakeLists.txt
View file

@ -4,8 +4,8 @@ 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})
#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc)
#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC})
add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc)
SET(FBR_FEATURE_SRC feature.cc)
@ -17,7 +17,8 @@ add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC})
SET(FBR_REGISTRATION_SRC registration.cc)
add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC})
add_library(fbr STATIC ${FBR_IO_SRC} ${FBR_PANORAMA_SRC} ${FBR_FEATURE_SRC} ${FBR_FEATURE_MATCHER_SRC} ${FBR_REGISTRATION_SRC} fbr_global.cc)
SET(FBR_SRC scan_cv.cc panorama.cc feature.cc feature_matcher.cc registration.cc fbr_global.cc)
add_library(fbr STATIC ${FBR_SRC})
IF(WITH_FBR)
SET(FBR_LIBS scan ANN ${OpenCV_LIBS})
@ -26,3 +27,10 @@ add_executable(featurebasedregistration feature_based_registration.cc fbr_global
#target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS})
target_link_libraries(featurebasedregistration fbr ${FBR_LIBS})
ENDIF(WITH_FBR)
### EXPORT SHARED LIBS
IF(EXPORT_SHARED_LIBS)
add_library(fbr_s SHARED ${FBR_SRC})
target_link_libraries(fbr_s scan ANN ${OpenCV_LIBS})
ENDIF(EXPORT_SHARED_LIBS)

3
src/slam6d/icp6D.cc
View file

@ -144,6 +144,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan)
#pragma omp parallel
{
int thread_num = omp_get_thread_num();
Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
thread_num, step,
rnd, max_dist_match2,
@ -183,7 +184,6 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan)
}
} // end parallel
// do we have enough point pairs?
unsigned int pairssize = 0;
for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
@ -330,6 +330,7 @@ void icp6D::doICP(vector <Scan *> allScans)
vector < Scan* > meta_scans;
Scan* my_MetaScan = 0;
for(unsigned int i = 0; i < allScans.size(); i++) {
cout << i << "*" << endl;

135
src/slam6d/kd.cc
View file

@ -31,7 +31,8 @@ using std::swap;
#include <cstring>
// KDtree class static variables
KDParams KDtree::params[MAX_OPENMP_NUM_THREADS];
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
/**
* Constructor
@ -43,100 +44,11 @@ KDParams KDtree::params[MAX_OPENMP_NUM_THREADS];
*/
KDtree::KDtree(double **pts, int n)
{
// Find bbox
double xmin = pts[0][0], xmax = pts[0][0];
double ymin = pts[0][1], ymax = pts[0][1];
double zmin = pts[0][2], zmax = pts[0][2];
for (int i = 1; i < n; i++) {
xmin = min(xmin, pts[i][0]);
xmax = max(xmax, pts[i][0]);
ymin = min(ymin, pts[i][1]);
ymax = max(ymax, pts[i][1]);
zmin = min(zmin, pts[i][2]);
zmax = max(zmax, pts[i][2]);
}
// Leaf nodes
if ((n > 0) && (n <= 10)) {
npts = n;
leaf.p = new double*[n];
memcpy(leaf.p, pts, n * sizeof(double *));
return;
}
// Else, interior nodes
npts = 0;
node.center[0] = 0.5 * (xmin+xmax);
node.center[1] = 0.5 * (ymin+ymax);
node.center[2] = 0.5 * (zmin+zmax);
node.dx = 0.5 * (xmax-xmin);
node.dy = 0.5 * (ymax-ymin);
node.dz = 0.5 * (zmax-zmin);
node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz);
// Find longest axis
if (node.dx > node.dy) {
if (node.dx > node.dz) {
node.splitaxis = 0;
} else {
node.splitaxis = 2;
}
} else {
if (node.dy > node.dz) {
node.splitaxis = 1;
} else {
node.splitaxis = 2;
}
}
// Partition
double splitval = node.center[node.splitaxis];
if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) {
npts = n;
leaf.p = new double*[n];
memcpy(leaf.p, pts, n * sizeof(double *));
return;
}
double **left = pts, **right = pts + n - 1;
while (1) {
while ((*left)[node.splitaxis] < splitval)
left++;
while ((*right)[node.splitaxis] >= splitval)
right--;
if (right < left)
break;
swap(*left, *right);
}
// Build subtrees
int i;
#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (i = 0; i < 2; i++) {
if (i == 0) node.child1 = new KDtree(pts, left-pts);
if (i == 1) node.child2 = new KDtree(left, n-(left-pts));
}
create(Void(), pts, n);
}
KDtree::~KDtree()
{
if (!npts) {
#ifdef WITH_OPENMP_KD
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (int i = 0; i < 2; i++) {
if (i == 0 && node.child1) delete node.child1;
if (i == 1 && node.child2) delete node.child2;
}
} else {
if (leaf.p) delete [] leaf.p;
}
}
/**
@ -152,45 +64,6 @@ double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const
params[threadNum].closest = 0;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
_FindClosest(threadNum);
_FindClosest(Void(), threadNum);
return params[threadNum].closest;
}
/**
* Wrapped function
*/
void KDtree::_FindClosest(int threadNum) const
{
// Leaf nodes
if (npts) {
for (int i = 0; i < npts; i++) {
double myd2 = Dist2(params[threadNum].p, leaf.p[i]);
if (myd2 < params[threadNum].closest_d2) {
params[threadNum].closest_d2 = myd2;
params[threadNum].closest = leaf.p[i];
}
}
return;
}
// Quick check of whether to abort
double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx,
fabs(params[threadNum].p[1]-node.center[1])-node.dy),
fabs(params[threadNum].p[2]-node.center[2])-node.dz);
if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2)
return;
// Recursive case
double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis];
if (myd >= 0.0) {
node.child1->_FindClosest(threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child2->_FindClosest(threadNum);
}
} else {
node.child2->_FindClosest(threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child1->_FindClosest(threadNum);
}
}
}

156
src/slam6d/kdManaged.cc
View file

@ -32,163 +32,13 @@ using std::swap;
#include <cstring>
// KDtree class static variables
KDParams KDtreeManagedBase::params[MAX_OPENMP_NUM_THREADS];
/**
* Constructor
*
* Create a KD tree from the points pointed to by the array pts
*
* @param pts 3D array of points
* @param n number of points
*/
KDtreeManagedBase::KDtreeManagedBase(const DataXYZ& pts, unsigned int* indices, unsigned int n)
{
// Find bbox
double xmin = pts[indices[0]][0], xmax = pts[indices[0]][0];
double ymin = pts[indices[0]][1], ymax = pts[indices[0]][1];
double zmin = pts[indices[0]][2], zmax = pts[indices[0]][2];
for(unsigned int i = 1; i < n; i++) {
xmin = min(xmin, pts[indices[i]][0]);
xmax = max(xmax, pts[indices[i]][0]);
ymin = min(ymin, pts[indices[i]][1]);
ymax = max(ymax, pts[indices[i]][1]);
zmin = min(zmin, pts[indices[i]][2]);
zmax = max(zmax, pts[indices[i]][2]);
}
// Leaf nodes
if ((n > 0) && (n <= 10)) {
npts = n;
leaf.p = new unsigned int[n];
// fill leaf index array with indices
for(unsigned int i = 0; i < n; ++i) {
leaf.p[i] = indices[i];
}
return;
}
// Else, interior nodes
npts = 0;
node.center[0] = 0.5 * (xmin+xmax);
node.center[1] = 0.5 * (ymin+ymax);
node.center[2] = 0.5 * (zmin+zmax);
node.dx = 0.5 * (xmax-xmin);
node.dy = 0.5 * (ymax-ymin);
node.dz = 0.5 * (zmax-zmin);
node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz);
// Find longest axis
if (node.dx > node.dy) {
if (node.dx > node.dz) {
node.splitaxis = 0;
} else {
node.splitaxis = 2;
}
} else {
if (node.dy > node.dz) {
node.splitaxis = 1;
} else {
node.splitaxis = 2;
}
}
// Partition
double splitval = node.center[node.splitaxis];
if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) {
npts = n;
leaf.p = new unsigned int[n];
// fill leaf index array with indices
for(unsigned int i = 0; i < n; ++i) {
leaf.p[i] = indices[i];
}
return;
}
unsigned int* left = indices, * right = indices + n - 1;
while(true) {
while(pts[*left][node.splitaxis] < splitval)
left++;
while(pts[*right][node.splitaxis] >= splitval)
right--;
if(right < left)
break;
swap(*left, *right);
}
// Build subtrees
int i;
#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (i = 0; i < 2; i++) {
if (i == 0) node.child1 = new KDtreeManagedBase(pts, indices, left - indices);
if (i == 1) node.child2 = new KDtreeManagedBase(pts, left, n - (left - indices));
}
}
KDtreeManagedBase::~KDtreeManagedBase()
{
if (!npts) {
#ifdef WITH_OPENMP_KD
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (int i = 0; i < 2; i++) {
if (i == 0 && node.child1) delete node.child1;
if (i == 1 && node.child2) delete node.child2;
}
} else {
if (leaf.p) delete [] leaf.p;
}
}
/**
* Wrapped function
*/
void KDtreeManagedBase::_FindClosest(const DataXYZ& pts, int threadNum) const
{
// Leaf nodes
if (npts) {
for (int i = 0; i < npts; i++) {
double myd2 = Dist2(params[threadNum].p, pts[leaf.p[i]]);
if (myd2 < params[threadNum].closest_d2) {
params[threadNum].closest_d2 = myd2;
params[threadNum].closest = pts[leaf.p[i]];
}
}
return;
}
// Quick check of whether to abort
double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx,
fabs(params[threadNum].p[1]-node.center[1])-node.dy),
fabs(params[threadNum].p[2]-node.center[2])-node.dz);
if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2)
return;
// Recursive case
double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis];
if (myd >= 0.0) {
node.child1->_FindClosest(pts, threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child2->_FindClosest(pts, threadNum);
}
} else {
node.child2->_FindClosest(pts, threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child1->_FindClosest(pts, threadNum);
}
}
}
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
KDtreeManaged::KDtreeManaged(Scan* scan) :
KDtreeManagedBase(scan->get("xyz reduced original"), prepareTempIndices(scan->size<DataXYZ>("xyz reduced original")), scan->size<DataXYZ>("xyz reduced original")),
m_scan(scan), m_data(0), m_count_locking(0)
{
create(scan->get("xyz reduced original"), prepareTempIndices(scan->size<DataXYZ>("xyz reduced original")), scan->size<DataXYZ>("xyz reduced original"));
// allocate in prepareTempIndices, deleted here
delete[] m_temp_indices;
}

164
src/slam6d/kdMeta.cc
View file

@ -32,166 +32,8 @@ using std::swap;
#include <cstring>
// KDtree class static variables
KDParams KDtreeMeta::params[MAX_OPENMP_NUM_THREADS];
KDtreeMeta::KDtreeMeta()
{
}
/**
* Create a KD tree from the points pointed to by the array pts
*
* @param pts 3D array of points
* @param n number of points
*/
void KDtreeMeta::create(const DataXYZ* const* pts, Index* indices, unsigned int n)
{
// Find bbox
double xmin = point(pts, indices[0])[0], xmax = point(pts, indices[0])[0];
double ymin = point(pts, indices[0])[1], ymax = point(pts, indices[0])[1];
double zmin = point(pts, indices[0])[2], zmax = point(pts, indices[0])[2];
for(unsigned int i = 1; i < n; i++) {
xmin = min(xmin, point(pts, indices[i])[0]);
xmax = max(xmax, point(pts, indices[i])[0]);
ymin = min(ymin, point(pts, indices[i])[1]);
ymax = max(ymax, point(pts, indices[i])[1]);
zmin = min(zmin, point(pts, indices[i])[2]);
zmax = max(zmax, point(pts, indices[i])[2]);
}
// Leaf nodes
if ((n > 0) && (n <= 10)) {
npts = n;
leaf.p = new Index[n];
// fill leaf index array with indices
for(unsigned int i = 0; i < n; ++i) {
leaf.p[i] = indices[i];
}
return;
}
// Else, interior nodes
npts = 0;
node.center[0] = 0.5 * (xmin+xmax);
node.center[1] = 0.5 * (ymin+ymax);
node.center[2] = 0.5 * (zmin+zmax);
node.dx = 0.5 * (xmax-xmin);
node.dy = 0.5 * (ymax-ymin);
node.dz = 0.5 * (zmax-zmin);
node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz);
// Find longest axis
if (node.dx > node.dy) {
if (node.dx > node.dz) {
node.splitaxis = 0;
} else {
node.splitaxis = 2;
}
} else {
if (node.dy > node.dz) {
node.splitaxis = 1;
} else {
node.splitaxis = 2;
}
}
// Partition
double splitval = node.center[node.splitaxis];
if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) {
npts = n;
leaf.p = new Index[n];
// fill leaf index array with indices
for(unsigned int i = 0; i < n; ++i) {
leaf.p[i] = indices[i];
}
return;
}
Index* left = indices, * right = indices + n - 1;
while(true) {
while(point(pts, *left)[node.splitaxis] < splitval)
left++;
while(point(pts, *right)[node.splitaxis] >= splitval)
right--;
if(right < left)
break;
swap(*left, *right);
}
// Build subtrees
int i;
#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (i = 0; i < 2; i++) {
if (i == 0) {
node.child1 = new KDtreeMeta();
node.child1->create(pts, indices, left - indices);
}
if (i == 1) {
node.child2 = new KDtreeMeta();
node.child2->create(pts, left, n - (left - indices));
}
}
}
KDtreeMeta::~KDtreeMeta()
{
if (!npts) {
#ifdef WITH_OPENMP_KD
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (int i = 0; i < 2; i++) {
if (i == 0 && node.child1) delete node.child1;
if (i == 1 && node.child2) delete node.child2;
}
} else {
if (leaf.p) delete [] leaf.p;
}
}
/**
* Wrapped function
*/
void KDtreeMeta::_FindClosest(const DataXYZ* const* pts, int threadNum) const
{
// Leaf nodes
if (npts) {
for (int i = 0; i < npts; i++) {
double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i]));
if (myd2 < params[threadNum].closest_d2) {
params[threadNum].closest_d2 = myd2;
params[threadNum].closest = point(pts, leaf.p[i]);
}
}
return;
}
// Quick check of whether to abort
double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx,
fabs(params[threadNum].p[1]-node.center[1])-node.dy),
fabs(params[threadNum].p[2]-node.center[2])-node.dz);
if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2)
return;
// Recursive case
double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis];
if (myd >= 0.0) {
node.child1->_FindClosest(pts, threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child2->_FindClosest(pts, threadNum);
}
} else {
node.child2->_FindClosest(pts, threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child1->_FindClosest(pts, threadNum);
}
}
}
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
KDtreeMetaManaged::KDtreeMetaManaged(const vector<Scan*>& scans) :
m_count_locking(0)
@ -217,7 +59,7 @@ KDtreeMetaManaged::~KDtreeMetaManaged()
delete[] m_data;
}
KDtreeMeta::Index* KDtreeMetaManaged::prepareTempIndices(const vector<Scan*>& scans)
Index* KDtreeMetaManaged::prepareTempIndices(const vector<Scan*>& scans)
{
unsigned int n = getPointsSize(scans);

4
src/slam6d/scan.cc
View file

@ -146,7 +146,7 @@ void Scan::toGlobal() {
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);
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
@ -166,7 +166,7 @@ void Scan::createSearchTree()
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);
boost::lock_guard<boost::mutex> lock(m_mutex_reduction);
if(m_has_reduced) return;
#ifdef WITH_METRICS

27
src/thermo/CMakeLists.txt
View file

@ -2,47 +2,24 @@ IF (WITH_THERMO)
find_package(OpenCV REQUIRED)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/gocr-0.48/src)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/gocr-0.48/include)
include_directories(${CMAKE_SOURCE_DIR}/include/shapes/)
include_directories(${CMAKE_SOURCE_DIR}/include/thermo/)
include_directories(/usr/include/)
include_directories(/usr/include/opencv)
# # Compile gocr library
# SET(GOCR_DIR ${CMAKE_SOURCE_DIR}/3rdparty/gocr-0.48/src/)
# add_library(Pgm2asc SHARED ${GOCR_DIR}gocr.c ${GOCR_DIR}pgm2asc.c ${GOCR_DIR}box.c ${GOCR_DIR}database.c
# ${GOCR_DIR}detect.c ${GOCR_DIR}barcode.c ${GOCR_DIR}lines.c ${GOCR_DIR}list.c
# ${GOCR_DIR}ocr0.c ${GOCR_DIR}ocr0n.c ${GOCR_DIR}ocr1.c ${GOCR_DIR}otsu.c
# ${GOCR_DIR}output.c ${GOCR_DIR}pixel.c ${GOCR_DIR}unicode.c ${GOCR_DIR}remove.c
# ${GOCR_DIR}pnm.c ${GOCR_DIR}pcx.c ${GOCR_DIR}progress.c ${GOCR_DIR}job.c)
# add_executable(shapes shapes.cc geom_math.cc numberrec.cc scan_ransac.cc )
#
# IF(WIN32)
# target_link_libraries(shapes scan XGetopt netpbm Pgm2asc)
# ENDIF(WIN32)
#
# IF (UNIX)
# target_link_libraries(shapes scan newmat dl netpbm Pgm2asc )
# ENDIF(UNIX)
#
add_executable(caliboard caliboard.cc)
# add_executable(thermo thermo.cc)
# add_executable(thermo thermo.cc src/cvaux.cpp src/cvblob.cpp src/cvcolor.cpp src/cvcontour.cpp src/cvlabel.cpp src/cvtrack.cpp)
add_executable(thermo thermo.cc)
# add_executable(thermo thermo.cc src/cvaux.cpp src/cvblob.cpp src/cvcolor.cpp src/cvcontour.cpp src/cvlabel.cpp src/cvtrack.cpp)
IF(UNIX)
target_link_libraries(caliboard scan shape newmat dl ANN)
target_link_libraries(thermo scan shape newmat dl ANN)
# target_link_libraries(thermo scan shape newmat dl ANN)
target_link_libraries(thermo cv cvaux cxcore GL GLU highgui cvblob scan ANN)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(caliboard scan shape newmat XGetopt ANN)
# target_link_libraries(thermo scan shape newmat XGetopt ANN)
target_link_libraries(thermo scan shape newmat XGetopt ANN)
target_link_libraries(thermo cv cvaux cxcore GL GLU highgui cvblob scan ANN)
ENDIF(WIN32)

237
src/thermo/caliboard.cc
View file

@ -42,6 +42,7 @@ using std::endl;
#endif
#include "shapes/hough.h"
#include "slam6d/basicScan.h"
#include "shapes/shape.h"
#include "shapes/ransac.h"
@ -114,6 +115,14 @@ void usage(char* prog) {
}
void writeFalse(string output) {
ofstream caliout(output.c_str());
caliout << "failed" << endl;
caliout.close();
caliout.clear();
}
bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, string output) {
double rPos[3] = {0.0,0.0,0.0};
double rPosTheta[3] = {0.0,0.0,0.0};
@ -139,6 +148,18 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
halfwidth = 19.0;
halfheight = 38.0;
break;
case 4: //Ostia
/*
halfwidth = 14.85;
halfheight = 21;
*/
halfwidth = 22.5;
halfheight = 30.5;
break;
case 5: // Ostia large
halfwidth = 22.5;
halfheight = 30.5;
break;
}
for(double i = -halfwidth; i <= halfwidth; i+=w_step) {
@ -147,16 +168,28 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
p[0] = i;
p[1] = j;
p[2] = 0.0;
//cout << p[0] << " " << p[1] << " " << p[2] << endl;
//cout << p[0] << " " << p[1] << " " << p[2] << " 1" << endl;
boardpoints.push_back(p);
}
}
int nr_points = boardpoints.size();
int nr_points2 = points.size();
Scan * plane = new Scan(rPos, rPosTheta, points);
Scan * board = new Scan(rPos, rPosTheta, boardpoints);
board->transform(alignxf, Scan::INVALID, 0);
Scan * plane = new BasicScan(rPos, rPosTheta, points);
Scan * board = new BasicScan(rPos, rPosTheta, boardpoints);
for(int i = 0; i < boardpoints.size(); i++) {
delete[] boardpoints[i];
}
plane->setRangeFilter(-1, -1);
plane->setReductionParameter(-1, 0);
plane->setSearchTreeParameter(simpleKD, false);
board->setRangeFilter(-1, -1);
board->setReductionParameter(-1, 0);
board->setSearchTreeParameter(simpleKD, false);
board->transform(alignxf, Scan::ICP, 0);
bool quiet = true;
icp6Dminimizer *my_icp6Dminimizer = 0;
@ -166,24 +199,21 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
double mdm = 50;
int mni = 50;
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, false, -1, false, 1, 0.00, false, false);
plane->createTree(false,false);
board->createTree(false,false);
my_icp->match(plane, board);
delete my_icp;
mdm = 2;
mni = 300;
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, false, -1, false, 1, 0.00, false, false);
my_icp->match(plane, board);
delete my_icp;
delete my_icp6Dminimizer;
double sum;
double centroid_s[3] = {0.0, 0.0, 0.0};
double centroid_t[3] = {0.0, 0.0, 0.0};
vector<PtPair> pairs_out;
Scan::getPtPairs(&pairs_out, plane, board, 1, 0, 2.0, sum, centroid_s, centroid_t);
Scan::getPtPairs(&pairs_out, plane, board, 1, 0, 3.0, sum, centroid_s, centroid_t);
int nr_matches = pairs_out.size();
cout << "Result " << nr_matches << " " << nr_points << " " << nr_points2 << endl;
@ -195,7 +225,7 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
}
cout << endl << endl;
cout << "Transform new: " << endl;
for(int i = 0; i < 3; i++) {
cout << pos[i] << " ";
}
@ -204,7 +234,6 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
cout << deg(postheta[i]) << " ";
}
cout << endl;
vector<double *> * result = new vector<double *>();
cout << "Calipoints Start" << endl;
ofstream caliout(output.c_str());
@ -215,19 +244,48 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
caliout << "Calibration" << endl;
}
/**
* write FRAMES
*/
/*
string filename = "tmp.frames";
ofstream fout(filename.c_str());
if (!fout.good()) {
cerr << "ERROR: Cannot open file " << filename << endl;
exit(1);
}
// write into file
//fout << "frames for scan" << endl;
//fout << plane->sout.str() << endl;
//fout << "frames for lightbulbs" << endl;
fout << board ->sout.str() << endl;
fout.close();
fout.clear();
*/
/*
board->saveFrames();
*/
/**
* end write frames
*/
switch(pattern) {
// lightbulb
case 0:
for(double y = -25; y < 30; y+=10.0) {
//for(double x = -20; x < 25; x+=10.0) {
for(double x = 20; x > -25; x-=10.0) {
double * p = new double[3];
p[0] = x;
p[1] = y;
p[2] = 0.0;
transform3(transMat, p);
result->push_back(p);
//result->push_back(p);
caliout << p[0] << " " << p[1] << " " << p[2] << endl;
delete[] p;
}
}
break;
@ -240,23 +298,22 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
p[1] = y;
p[2] = 0.0;
transform3(transMat, p);
result->push_back(p);
caliout << p[0] << " " << p[1] << " " << p[2] << endl;
delete[] p;
}
}
break;
// chessboard on wooden board pattern bottom
case 3:
for(double y = -4.1; y > -33.0; y-=5.2) {
//for(double y = -30.1; y < -0.0; y+=5.2) {
// for(double x = 7.8; x > -10; x-=5.2) {
for(double x = -8.1; x < 10; x+=5.2) {
double * p = new double[3];
p[0] = x;
p[1] = y;
p[2] = 0.0;
transform3(transMat, p);
result->push_back(p);
caliout << p[0] << " " << p[1] << " " << p[2] << endl;
delete[] p;
}
}
break;
@ -269,25 +326,61 @@ bool matchPlaneToBoard(vector<double *> &points, double *alignxf, int pattern, s
p[1] = y;
p[2] = 0.0;
transform3(transMat, p);
result->push_back(p);
caliout << p[0] << " " << p[1] << " " << p[2] << endl;
delete[] p;
}
}
break;
// Ostia
case 4:
for(double x = -12; x < 16; x+=8.0) {
for(double y = -20; y < 24; y+=8.0) {
double * p = new double[3];
p[0] = x;
p[1] = y;
p[2] = 0.0;
transform3(transMat, p);
caliout << p[0] << " " << p[1] << " " << p[2] << endl;
delete[] p;
}
}
break;
break;
//_|_|_|_|_|_|_
//4: 1.5*8 = 12
//6: 2.5*8 = 20
// Universum
case 5:
for(double y = 20; y > -24; y-=8.0) {
for(double x = -12; x < 16; x+=8.0) {
double * p = new double[3];
p[0] = x;
p[1] = y;
p[2] = 0.0;
transform3(transMat, p);
caliout << p[0] << " " << p[1] << " " << p[2] << endl;
delete[] p;
}
}
break;
}
caliout.close();
caliout.clear();
cout << "Calipoints End" << endl;
delete board;
delete plane;
return !(nr_matches < nr_points);
}
int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int &end, int &pattern, int &maxDist, int &minDist, double &top, double &bottom, int &octree, IOType &type, bool
&quiet) {
int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int
&end, int &pattern, int &maxDist, int &minDist, double &top, double &bottom, int
&octree, IOType &type, bool &quiet) {
bool reduced = false;
int c;
// from unistd.h:
extern char *optarg;
extern int optind;
@ -335,7 +428,7 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int &
break;
case 'p':
pattern = atoi(optarg);
if(pattern < 0 || pattern > 3) { cerr << "Error: choose pattern between 0 and 3!\n"; exit(1); }
if(pattern < 0 || pattern > 5) { cerr << "Error: choose pattern between 0 and 3!\n"; exit(1); }
break;
case 'q':
quiet = true;
@ -412,7 +505,6 @@ int main(int argc, char **argv)
cout << "Parse args" << endl;
parseArgs(argc, argv, dir, red, start, end, pattern, maxDist, minDist, top, bottom, octree, type, quiet);
Scan::dir = dir;
int fileNr = start;
string calidir = dir + "/cali";
@ -447,21 +539,26 @@ int main(int argc, char **argv)
int failures = 0;
long calitime = GetCurrentTimeInMilliSec();
#ifndef WITH_SCANSERVER
//#ifndef WITH_SCANSERVER
while (fileNr <= end) {
Scan::readScans(type, fileNr, fileNr, dir, maxDist, minDist, 0);
Scan::openDirectory(false, dir, type, fileNr, fileNr);
Scan::allScans[0]->setRangeFilter(maxDist, minDist);
Scan::allScans[0]->setHeightFilter(top, bottom);
Scan::allScans[0]->setSearchTreeParameter(simpleKD, false);
string output = calidir + "/scan" + to_string(fileNr,3) + ".3d";
cout << "Top: " << top << " Bottom: " << bottom << endl;
Scan::allScans[0]->trim(top, bottom);
Scan::allScans[0]->toGlobal(red, octree);
Scan::allScans[0]->toGlobal();
double id[16];
M4identity(id);
for(int i = 0; i < 10; i++) {
Scan::allScans[0]->transform(id, Scan::ICP, 0); // write end pose
}
/*
#else //WITH_SCANSERVER
Scan::readScansRedSearch(type, start, end, dir, filter, red, octree);
for(std::vector<Scan*>::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
{
@ -477,16 +574,22 @@ int main(int argc, char **argv)
scan->transform(id, Scan::ICP, 0); // write end pose
}
#endif //WITH_SCANSERVER
*/
cout << "start plane detection" << endl;
long starttime = GetCurrentTimeInMilliSec();
vector<double *> points;
CollisionPlane<double> * plane;
plane = new LightBulbPlane<double>(50,120);
#ifndef WITH_SCANSERVER
//#ifndef WITH_SCANSERVER
Ransac(*plane, Scan::allScans[0], &points);
/*
#else //WITH_SCANSERVER
cout << "S" << endl;
Ransac(*plane, scan, &points);
#endif //WITH_SCANSERVER
cout << "T" << endl;
//#endif //WITH_SCANSERVER
*/
starttime = (GetCurrentTimeInMilliSec() - starttime);
cout << "nr points " << points.size() << endl;
@ -495,31 +598,73 @@ int main(int argc, char **argv)
cout << "DONE " << endl;
cout << nx << " " << ny << " " << nz << " " << d << endl;
double rPos[3];
double rPosTheta[3];
for(int i = 0; i < 3; i++) {
rPosTheta[i] = 0.0;
}
((LightBulbPlane<double> *)plane)->getCenter(rPos[0], rPos[1], rPos[2]);
double alignxf[16];
EulerToMatrix4(rPos, rPosTheta, alignxf);
if(matchPlaneToBoard(points, alignxf, pattern, output)) {
successes++;
} else {
if(std::isnan(d)) {
writeFalse(output);
failures++;
} else {
if(d < 0) {
nx = -nx;
ny = -ny;
nz = -nz;
d = -d;
}
double tx, ty, tz;
tz = 0;
ty = asin(nx);
tx = asin(-ny/cos(ty));
double rPos[3];
double rPosTheta[3];
for(int i = 0; i < 3; i++) {
rPosTheta[i] = 0.0;
}
rPosTheta[0] = tx;
rPosTheta[1] = ty;
rPosTheta[2] = tz;
//rPosTheta[1] = acos(nz);
// rotate plane model to make parallel with detected plane
// transform plane model to center of detected plane
((LightBulbPlane<double> *)plane)->getCenter(rPos[0], rPos[1], rPos[2]);
cout << "Angle: " << deg(acos(nz)) << endl;
for(int i = 0; i < 3; i++) {
cout << rPos[i] << " ";
}
for(int i = 0; i < 3; i++) {
cout << deg(rPosTheta[i]) << " ";
}
cout << endl;
double alignxf[16];
EulerToMatrix4(rPos, rPosTheta, alignxf);
if(matchPlaneToBoard(points, alignxf, pattern, output)) {
successes++;
} else {
failures++;
}
}
for(int i = points.size() - 1; i > -1; i++) {
for(int i = points.size() - 1; i > -1; i--) {
delete[] points[i];
}
delete plane;
cout << "Time for Plane Detection " << starttime << endl;
#ifndef WITH_SCANSERVER
//#ifndef WITH_SCANSERVER
delete Scan::allScans[0];
Scan::allScans.clear();
fileNr++;
#endif //WITH_SCANSERVER
//#endif //WITH_SCANSERVER
}
calitime = (GetCurrentTimeInMilliSec() - calitime);
@ -527,9 +672,11 @@ int main(int argc, char **argv)
<< " failures!" << endl;
cout << "Time for Calibration " << calitime << endl;
/*
#ifdef WITH_SCANSERVER
Scan::clearScans();
ClientInterface::destroy();
#endif //WITH_SCANSERVER
*/
}

1506
src/thermo/thermo.cc
View file

File diff suppressed because it is too large Load diff

6
src/veloslam/veloscan.cc
View file

@ -109,7 +109,7 @@ Trajectory::Trajectory()
* default Constructor
*/
VeloScan::VeloScan()
: Scan()
// : BasicScan()
{
isTrackerHandled =false;
}
@ -131,9 +131,11 @@ int VeloScan::DeletePoints()
/**
* Copy constructor
*/
/*
VeloScan::VeloScan(const VeloScan& s)
: Scan(s)
: BasicScan(s)
{ }
*/
int VeloScan::TransferToCellArray(int maxDist, int minDist)
{

4
src/veloslam/veloslam.cc
View file

@ -1018,7 +1018,7 @@ void MatchTwoScan(icp6D *my_icp, VeloScan* currentScan, int scanCount, bool eP )
//////////////////////ICP//////////////////////
if (scanCount > 0)
{
PreviousScan =Scan::allScans[scanCount-1];
PreviousScan = Scan::allScans[scanCount-1];
// extrapolate odometry // 以前一帧的坐标为基准
if (eP)
currentScan->mergeCoordinatesWithRoboterPosition(PreviousScan);
@ -1111,7 +1111,7 @@ int main(int argc, char **argv)
exit(0);
}
VeloScan::openDirectory(scanserver, dir, type, start, end);
Scan::openDirectory(scanserver, dir, type, start, end);
if(VeloScan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;