update svn to r733

CMakeLists.txt

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

CONTRIBUTORS

@@ -27,10 +27,11 @@ Li Wei                    xpaulee@gmail.com
 Shams Feyzabadi           sh.feyzabadi@gmail.co
 Vladislav Perelmann		    v.perelman@jacobs-university.de
 Chen Long                 lchen.whu@gmail.com
-Remuas Dumitru            r.dumitru@jaocbs-university.de
+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
 

README.scanserver

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

include/shapes/ransac.h

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

include/shapes/ransac_Boctree.h

@@ -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--;
@@ -218,7 +218,8 @@ void showbits(char a)
     if (leaf) {
 /*      cout << "STOPPED" << endl;
         return;*/
-      pointrep *points = children[r].points;
+     
+      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);

include/show/show.icc

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

include/show/show1.icc

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

include/show/show_Boctree.h

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

include/show/viewcull.h

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

include/slam6d/ann_kd.h

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

include/slam6d/basicScan.h

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

include/slam6d/globals.icc

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

include/slam6d/kd.h

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

include/slam6d/kdManaged.h

@@ -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 {
+class KDtreeManaged :
-public:
+    public SearchTree,
-  KDtreeManagedBase(const DataXYZ& pts, unsigned int* indices, unsigned int n);
+    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() {}

include/slam6d/kdMeta.h

@@ -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 {
+class KDtreeMetaManaged : 
-protected:
+    public SearchTree,
-  class Index {
+    private KDTreeImpl<const DataXYZ* const*, Index, IndexAccessor>
-  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 {
 public:
   KDtreeMetaManaged(const vector<Scan*>& scans);
   virtual ~KDtreeMetaManaged();

include/slam6d/scan.h

@@ -103,8 +103,8 @@ public:
   };
   
   // delete copy-ctor and assignment, scans shouldn't be copied by basic class
-  //  Scan(const Scan& other) = delete;
+  Scan(const Scan& other) = delete;
-  //  Scan& operator=(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
+  // 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;
+  boost::mutex m_mutex_reduction, m_mutex_create_tree;
 };
 
 #include "scan.icc"

include/thermo/thermo.h

@@ -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 CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical);
-void clusterSearch(float ** points, int size, double thresh1, double thresh2, fstream &outfile);
+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);
 

include/veloslam/veloscan.h

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

src/scanio/scan_io_velodyne.cc

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

src/segmentation/CMakeLists.txt

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

src/segmentation/scan2segments.cc

@@ -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
+    segmentation_option_dependency(vm, WATERSHED, "marker");
-    if (stype == WATERSHED) {
+    segmentation_option_conflict(vm, WATERSHED, "thresh");
-        if (!vm.count("marker"))
+    segmentation_option_conflict(vm, WATERSHED, "maxlevel");
-            throw std::logic_error("watershed segmentation requires --marker to be set");
+    segmentation_option_conflict(vm, WATERSHED, "radius");
-        if (vm.count("thresh"))
+    segmentation_option_conflict(vm, WATERSHED, "links");
-            throw std::logic_error("watershed segmentation cannot be used with --thresh");
+    segmentation_option_conflict(vm, WATERSHED, "clustering");
-        if (vm.count("maxlevel"))
+    segmentation_option_conflict(vm, WATERSHED, "levels");
-            throw std::logic_error("watershed segmentation cannot be used with --maxlevel");
+
-        if (vm.count("radius"))
+    segmentation_option_conflict(vm, THRESHOLD, "marker");
-            throw std::logic_error("watershed segmentation cannot be used with --radius");
+    segmentation_option_dependency(vm, THRESHOLD, "thresh");
-    }
+    segmentation_option_conflict(vm, THRESHOLD, "maxlevel");
-    if (stype == THRESHOLD) {
+    segmentation_option_conflict(vm, THRESHOLD, "radius");
-        if (!vm.count("thresh"))
+    segmentation_option_conflict(vm, THRESHOLD, "links");
-            throw std::logic_error("threshold segmentation requires --thresh to be set");
+    segmentation_option_conflict(vm, THRESHOLD, "clustering");
-        if (vm.count("marker"))
+    segmentation_option_conflict(vm, THRESHOLD, "levels");
-            throw std::logic_error("threshold segmentation cannot be used with --marker");
+
-        if (vm.count("maxlevel"))
+    segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "marker");
-            throw std::logic_error("threshold segmentation cannot be used with --maxlevel");
+    segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "thresh");
-        if (vm.count("radius"))
+    segmentation_option_dependency(vm, PYR_MEAN_SHIFT, "maxlevel");
-            throw std::logic_error("threshold segmentation cannot be used with --radius");
+    segmentation_option_dependency(vm, PYR_MEAN_SHIFT, "radius");
-    }
+    segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "links");
-    if (stype == PYR_MEAN_SHIFT) {
+    segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "clustering");
-        if (!vm.count("maxlevel"))
+    segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "levels");
-            throw std::logic_error("mean shift segmentation requires --maxlevel to be set");
+
-        if (!vm.count("radius"))
+    segmentation_option_conflict(vm, PYR_SEGMENTATION, "marker");
-            throw std::logic_error("mean shift segmentation requires --radius to be set");
+    segmentation_option_conflict(vm, PYR_SEGMENTATION, "thresh");
-        if (vm.count("thresh"))
+    segmentation_option_conflict(vm, PYR_SEGMENTATION, "maxlevel");
-            throw std::logic_error("mean shift segmentation cannot be used with --thresh");
+    segmentation_option_conflict(vm, PYR_SEGMENTATION, "radius");
-        if (vm.count("marker"))
+    segmentation_option_dependency(vm, PYR_SEGMENTATION, "links");
-            throw std::logic_error("mean shift segmentation cannot be used with --marker");
+    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) {

src/shapes/plane.cc

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

src/show/CMakeLists.txt

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

src/show/compacttree.cc

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

src/show/show_gl.cc

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

src/show/viewcull.cc

@@ -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 */
+}

src/slam6d/CMakeLists.txt

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

src/slam6d/ann_kd.cc

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

src/slam6d/basicScan.cc

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

src/slam6d/fbr/CMakeLists.txt

@@ -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})
-#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc)
+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)

src/slam6d/icp6D.cc

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

src/slam6d/kd.cc

@@ -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
+    create(Void(), pts, n);
-  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));
-  }
 }
 
 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);
-    }
-  }
-}

src/slam6d/kdManaged.cc

@@ -32,163 +32,13 @@ using std::swap;
 #include <cstring>
 
 // KDtree class static variables
-KDParams KDtreeManagedBase::params[MAX_OPENMP_NUM_THREADS];
+template<class PointData, class AccessorData, class AccessorFunc>
-
+KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::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);
-    }
-  }
-}
 
 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;
 }

src/slam6d/kdMeta.cc

@@ -32,166 +32,8 @@ using std::swap;
 #include <cstring>
 
 // KDtree class static variables
-KDParams KDtreeMeta::params[MAX_OPENMP_NUM_THREADS];
+template<class PointData, class AccessorData, class AccessorFunc>
-
+KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::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);
-    }
-  }
-}
 
 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);
 

src/slam6d/scan.cc

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

src/thermo/CMakeLists.txt

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

src/thermo/caliboard.cc

@@ -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 * plane = new BasicScan(rPos, rPosTheta, points);
-  Scan * board = new Scan(rPos, rPosTheta, boardpoints);
+  Scan * board = new BasicScan(rPos, rPosTheta, boardpoints);
-  board->transform(alignxf, Scan::INVALID, 0);
+  
+  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
+int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int
-&quiet) {
+&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,12 +598,49 @@ int main(int argc, char **argv)
     cout << "DONE " << endl;
 
     cout << nx << " " << ny << " " << nz << " " << d << endl;
+    
+    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)) {
@@ -508,18 +648,23 @@ int main(int argc, char **argv)
       } 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
+*/
 }
 

src/veloslam/veloscan.cc

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

src/veloslam/veloslam.cc

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