update to r733 (again)

.svn/pristine/02/0266ada4f3e0365a9d424292ae86bd3186b49387.svn-base

@@ -0,0 +1,849 @@
+/*
+ * compacttree implementation
+ *
+ * Copyright (C) Jan Elseberg, Kai Lingemann, Jan Elseberg
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file 
+ * @brief Efficient representation of an octree
+ * @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany. 
+ * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+
+#include <stdio.h>
+
+#include <vector>
+using std::vector;
+#include <deque>
+using std::deque;
+#include <set>
+using std::set;
+#include <list>
+using std::list;
+#include <iostream>
+#include <fstream>
+#include <string>
+
+#include "slam6d/globals.icc"
+#include "slam6d/point_type.h"
+
+#include "slam6d/Boctree.h"
+#include "show/compacttree.h"
+#include "show/colormanager.h"
+#include "show/scancolormanager.h"
+#include "show/viewcull.h"
+
+using namespace show;
+compactTree::~compactTree(){
+  delete alloc;
+
+  delete[] mins;
+  delete[] maxs;
+} 
+
+
+void compactTree::AllPoints( cbitoct &node, vector<double*> &vp, double center[3], double size) {
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        tshort *point = children->getPoints();
+        lint length = children->getLength();
+
+        for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+          double *p = new double[3];
+          //cout << point[0] << " " << point[1] << " " << point[2] << endl; 
+          for (unsigned int k = 0; k < 3; k++){
+            p[k] = point[k] * precision + ccenter[k];
+          }
+
+          vp.push_back(p);
+          point+=POINTDIM;
+        }
+      } else { // recurse
+        AllPoints( children->node, vp, ccenter, size/2.0);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+
+
+void compactTree::GetOctTreeCenter(vector<double*>&c, cbitoct &node, double *center, double size) {
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (unsigned char i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        double * cp = new double[POINTDIM];
+        for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) {
+          cp[iterator] = ccenter[iterator];
+        }
+        c.push_back(cp);
+      } else { // recurse
+        GetOctTreeCenter(c, children->node, ccenter, size/2.0);
+      }
+      ++children; // next child
+    }
+  }
+}
+long compactTree::countNodes(cbitoct &node) {
+  long result = 0;
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
+        //   ++result;
+      } else { // recurse
+        result += countNodes(children->node) + 1;
+      }
+      ++children; // next child
+    }
+  }
+  return result;
+}
+
+long compactTree::countLeaves(cbitoct &node) {
+  long result = 0;
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
+        lint nrpts = children->getLength();
+        result += POINTDIM*nrpts + 1;
+      } else { // recurse
+        result += countLeaves(children->node);
+      }
+      ++children; // next child
+    }
+  }
+  return result;
+}
+
+
+void compactTree::deletetNodes(cbitoct &node) {
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+  bool haschildren = false;
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
+        tshort *points = children->getPoints();
+        delete [] points;
+        //          delete [] children->points;
+      } else { // recurse
+        deletetNodes(children->node);
+      }
+      ++children; // next child
+      haschildren = true;
+    }
+  }
+  // delete children
+  if (haschildren) {
+    cbitoct::getChildren(node, children);
+    delete[] children;
+  }
+}
+
+
+unsigned long compactTree::maxTargetPoints( cbitoct &node ) {
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  unsigned long max = 0;
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        lint length = children->getLength();
+        if (length > max) max = length;
+      } else { // recurse
+        unsigned long tp = maxTargetPoints( children->node);
+        if (tp > max) max = tp;
+      }
+      ++children; // next child
+    }
+  }
+
+  return max*POPCOUNT(node.valid);
+}
+
+void compactTree::displayOctTreeAll( cbitoct &node, double *center, double size) {
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        tshort *point = children->getPoints();
+        lint length = children->getLength();
+        glBegin(GL_POINTS);
+        for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+          if(cm) cm->setColor(point);
+          //cout << "C " << point[1] << " " << cm << endl;
+          //glVertex3f( point[0], point[1], point[2]);
+          glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+          point+=POINTDIM;
+        }
+        glEnd();
+      } else { // recurse
+        displayOctTreeAll( children->node, ccenter, size/2.0);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::displayOctTreeAllCulled( cbitoct &node, double *center, double size ) {
+  int res = CubeInFrustum2(center[0], center[1], center[2], size);
+  if (res==0) return;  // culled do not continue with this branch of the tree
+
+  if (res == 2) { // if entirely within frustrum discontinue culling
+    displayOctTreeAll(node, center, size);
+    return;
+  }
+
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        // check if leaf is visible
+        if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+          tshort *point = children->getPoints();
+          lint length = children->getLength();
+          glBegin(GL_POINTS);
+          for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+            if(cm) cm->setColor(point);
+            //glVertex3f( point[0], point[1], point[2]);
+            glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+            point+=POINTDIM;
+          }
+          glEnd();
+        }
+      } else { // recurse
+        displayOctTreeAllCulled( children->node, ccenter, size/2.0);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::displayOctTreeCulledLOD(long targetpts, cbitoct &node, double *center, double size ) {
+  if (targetpts <= 0) return; // no need to display anything
+
+  int res = CubeInFrustum2(center[0], center[1], center[2], size);
+  if (res==0) return;  // culled do not continue with this branch of the tree
+
+  if (res == 2) { // if entirely within frustrum discontinue culling
+    displayOctTreeLOD(targetpts, node, center, size);
+    return;
+  }
+
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  unsigned short nc = POPCOUNT(node.valid);
+  long newtargetpts = targetpts;
+  if (nc > 0) {
+    newtargetpts = newtargetpts/nc;
+    if (newtargetpts <= 0 ) return;
+  }
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        // check if leaf is visible
+        if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+          tshort *point = children->getPoints();
+          lint length = children->getLength();
+          glBegin(GL_POINTS);
+          if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {  // only a single pixel on screen only paint one point
+            if(cm) cm->setColor(point);
+            //glVertex3f( point[0], point[1], point[2]);
+            glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+          } else if (length <= newtargetpts) {        // more points requested than possible, plot all
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+              if(cm) cm->setColor(point);
+              //glVertex3f( point[0], point[1], point[2]);
+              glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+              point+=POINTDIM;
+            }
+          } else {                         // select points to show
+            // TODO smarter subselection of points here
+            double each = (double)POINTDIM * (double)((double)length/(double)newtargetpts);
+            tshort *p;
+            int index;
+            for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
+              index = (double)iterator * each;
+              p = point + index - index%POINTDIM;
+              if(cm) cm->setColor(p);
+              //glVertex3f( p[0], p[1], p[2]);
+              glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
+              //point += each;
+            }
+          }
+          glEnd();
+        }
+
+      } else { // recurse
+        displayOctTreeCulledLOD(newtargetpts, children->node, ccenter, size/2.0);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::displayOctTreeLOD(long targetpts, cbitoct &node, double *center, double size ) {
+  if (targetpts <= 0) return; // no need to display anything
+
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  unsigned short nc = POPCOUNT(node.valid);
+  long newtargetpts = targetpts;
+  if (nc > 0) {
+    newtargetpts = newtargetpts/nc;
+    if (newtargetpts <= 0 ) return;
+  }
+
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        tshort *point = children->getPoints();
+        lint length = children->getLength();
+        glBegin(GL_POINTS);
+        /*          if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {  // only a single pixel on screen only paint one point
+                    if(cm) cm->setColor(point);
+        //glVertex3f( point[0], point[1], point[2]);
+        glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+        } else*/ if (length <= newtargetpts) {        // more points requested than possible, plot all
+          for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+            if(cm) cm->setColor(point);
+            //glVertex3f( point[0], point[1], point[2]);
+            glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+            point+=POINTDIM;
+          }
+        } else {                         // select points to show
+          // TODO smarter subselection of points here
+          double each = (double)POINTDIM * (double)((double)length/(double)newtargetpts);
+          tshort *p;
+          int index;
+          for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
+            index = (double)iterator * each;
+            p = point + index - index%POINTDIM;
+            if(cm) cm->setColor(p);
+            //glVertex3f( p[0], p[1], p[2]);
+            glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
+            //point += each;
+          }
+        }
+      glEnd();
+      } else { // recurse
+        displayOctTreeLOD(newtargetpts, children->node, ccenter, size/2.0);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::displayOctTreeCulledLOD2(float ratio, cbitoct &node, double *center, double size ) {
+  int res = CubeInFrustum2(center[0], center[1], center[2], size);
+  if (res==0) return;  // culled do not continue with this branch of the tree
+
+  if (res == 2) { // if entirely within frustrum discontinue culling
+    displayOctTreeLOD2(ratio, node, center, size);
+    return;
+  }
+
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        // check if leaf is visible
+        if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+          tshort *point = children->getPoints();
+          lint length = children->getLength();
+            
+          int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+          l = max((int)(l*l*ratio), 0);
+          if (l != 0) {
+            if ((int)length > l ) {
+              double each = (double)POINTDIM * (double)((double)length/(double)l);
+              tshort *p;
+              int index;
+              for(int iterator = 0; iterator < l; iterator++ ) {
+                index = (double)iterator * each;
+                p = point + index - index%POINTDIM;
+                if(cm) cm->setColor(p);
+                glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
+              }
+            } else if ((int)length <= l) { 
+              for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+                if(cm) cm->setColor(point);
+                glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+                point+=POINTDIM;
+              }
+            } else if (l == 1) {
+                if(cm) cm->setColor(point);
+                glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+            }
+          }
+        }
+      } else { // recurse
+        int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+        l = max((int)(l*l*ratio), 0);
+        if (l > 0) {
+          displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0);
+        }
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::displayOctTreeLOD2(float ratio, cbitoct &node, double *center, double size ) {
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+        tshort *point = children->getPoints();
+        lint length = children->getLength();
+            
+        int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+        l = max((int)(l*l*ratio), 0);
+        if (l > 1) {
+          if ((int)length > l ) {
+            double each = (double)POINTDIM * (double)((double)length/(double)l);
+            tshort *p;
+            int index;
+            for(int iterator = 0; iterator < l; iterator++ ) {
+              index = (double)iterator * each;
+              p = point + index - index%POINTDIM;
+              if(cm) cm->setColor(p);
+              glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
+            }
+          } else if ((int)length <= l) { 
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+              point+=POINTDIM;
+            }
+          }
+        } else {
+          if(cm) cm->setColor(point);
+          glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
+        }
+      } else { // recurse
+        int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+        l = max((int)(l*l*ratio), 0);
+        if (l > 0) {
+          displayOctTreeLOD2(ratio, children->node, ccenter, size/2.0);
+        }
+      }
+      ++children; // next child
+    }
+  }
+}
+
+
+void compactTree::displayOctTreeCAllCulled( cbitoct &node, double *center, double size, double minsize ) {
+  int res = CubeInFrustum2(center[0], center[1], center[2], size);
+  if (res==0) return;  // culled do not continue with this branch of the tree
+
+  if (res == 2) { // if entirely within frustrum discontinue culling
+    displayOctTreeCAll(node, center, size, minsize);
+    return;
+  }
+
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf || minsize > size ) {   // if ith node is leaf get center
+        // check if leaf is visible
+        if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+          showCube(ccenter, size/2.0);
+        }
+      } else { // recurse
+        displayOctTreeCAllCulled( children->node, ccenter, size/2.0, minsize);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::displayOctTreeCAll( cbitoct &node, double *center, double size, double minsize ) {
+  double ccenter[3];
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      childcenter(center, ccenter, size, i);  // childrens center
+      if (  ( 1 << i ) & node.leaf || minsize > size ) {   // if ith node is leaf get center
+        showCube(ccenter, size/2.0);
+      } else { // recurse
+        displayOctTreeCAll( children->node, ccenter, size/2.0, minsize);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+void compactTree::showCube(double *center, double size) {
+  glLineWidth(1.0);
+  glBegin(GL_QUADS);      // draw a cube with 6 quads
+  glColor3f(0.0f,1.0f,0.0f);      // Set The Color To Green
+  glVertex3f(center[0] + size, center[1] + size, center[2] - size);
+  glVertex3f(center[0] - size, center[1] + size, center[2] - size);
+  glVertex3f(center[0] - size, center[1] + size, center[2] + size);
+  glVertex3f(center[0] + size, center[1] + size, center[2] + size);
+  glColor3f(1.0f,0.5f,0.0f);      // Set The Color To Orange
+
+  glVertex3f(center[0] + size, center[1] - size, center[2] + size); 
+  glVertex3f(center[0] - size, center[1] - size, center[2] + size);
+  glVertex3f(center[0] - size, center[1] - size, center[2] - size);
+  glVertex3f(center[0] + size, center[1] - size, center[2] - size);
+
+  glColor3f(1.0f,0.0f,0.0f);      // Set The Color To Red
+  glVertex3f(center[0] + size, center[1] + size, center[2] + size); 
+  glVertex3f(center[0] - size, center[1] + size, center[2] + size);
+  glVertex3f(center[0] - size, center[1] - size, center[2] + size);
+  glVertex3f(center[0] + size, center[1] - size, center[2] + size);
+
+  glColor3f(1.0f,1.0f,0.0f);      // Set The Color To Yellow
+  glVertex3f(center[0] + size, center[1] - size, center[2] - size); 
+  glVertex3f(center[0] - size, center[1] - size, center[2] - size);
+  glVertex3f(center[0] - size, center[1] + size, center[2] - size);
+  glVertex3f(center[0] + size, center[1] + size, center[2] - size);
+
+  glColor3f(0.0f,0.0f,1.0f);      // Set The Color To Blue
+  glVertex3f(center[0] - size, center[1] + size, center[2] + size); 
+  glVertex3f(center[0] - size, center[1] + size, center[2] - size);
+  glVertex3f(center[0] - size, center[1] - size, center[2] - size);
+  glVertex3f(center[0] - size, center[1] - size, center[2] + size);
+
+  glColor3f(1.0f,0.0f,1.0f);      // Set The Color To Violet
+  glVertex3f(center[0] + size, center[1] + size, center[2] - size); 
+  glVertex3f(center[0] + size, center[1] + size, center[2] + size);
+  glVertex3f(center[0] + size, center[1] - size, center[2] + size);
+  glVertex3f(center[0] + size, center[1] - size, center[2] - size);
+
+
+  glEnd();
+
+}
+
+
+
+
+template <class T>
+void compactTree::selectRay(vector<T *> &points) { 
+  //selectRay(points, *root, center, size); 
+}
+
+
+void compactTree::childcenter(double *pcenter, double *ccenter, double size, unsigned char i) {
+  switch (i) {
+    case 0:
+      ccenter[0] = pcenter[0] - size / 2.0;
+      ccenter[1] = pcenter[1] - size / 2.0;
+      ccenter[2] = pcenter[2] - size / 2.0;
+      break;
+    case 1:
+      ccenter[0] = pcenter[0] + size / 2.0;
+      ccenter[1] = pcenter[1] - size / 2.0;
+      ccenter[2] = pcenter[2] - size / 2.0;
+      break;
+    case 2:
+      ccenter[0] = pcenter[0] - size / 2.0;
+      ccenter[1] = pcenter[1] + size / 2.0;
+      ccenter[2] = pcenter[2] - size / 2.0;
+      break;
+    case 3:
+      ccenter[0] = pcenter[0] + size / 2.0;
+      ccenter[1] = pcenter[1] + size / 2.0;
+      ccenter[2] = pcenter[2] - size / 2.0;
+      break;
+    case 4:
+      ccenter[0] = pcenter[0] - size / 2.0;
+      ccenter[1] = pcenter[1] - size / 2.0;
+      ccenter[2] = pcenter[2] + size / 2.0;
+      break;
+    case 5:
+      ccenter[0] = pcenter[0] + size / 2.0;
+      ccenter[1] = pcenter[1] - size / 2.0;
+      ccenter[2] = pcenter[2] + size / 2.0;
+      break;
+    case 6:
+      ccenter[0] = pcenter[0] - size / 2.0;
+      ccenter[1] = pcenter[1] + size / 2.0;
+      ccenter[2] = pcenter[2] + size / 2.0;
+      break;
+    case 7:
+      ccenter[0] = pcenter[0] + size / 2.0;
+      ccenter[1] = pcenter[1] + size / 2.0;
+      ccenter[2] = pcenter[2] + size / 2.0;
+      break;
+    default:
+      break;
+  }
+}
+
+
+void compactTree::GetOctTreeCenter(vector<double*>&c) { GetOctTreeCenter(c, *root, center, size); }
+void compactTree::AllPoints(vector<double *> &vp) { AllPoints(*compactTree::root, vp, center, size); }
+
+long compactTree::countNodes() { return 1 + countNodes(*root); }
+long compactTree::countLeaves() { return 1 + countLeaves(*root); }
+
+void compactTree::setColorManager(ColorManager *_cm) { cm = _cm; }
+
+void compactTree::drawLOD(float ratio) { 
+    switch (current_lod_mode) {
+      case 1:
+        glBegin(GL_POINTS);
+        displayOctTreeCulledLOD2(ratio , *root, center, size);
+        glEnd();
+        break;
+      case 2:
+        /*
+#ifdef WITH_GLEE
+        if (GLEE_ARB_point_parameters) {
+          glPointParameterfARB(GL_POINT_SIZE_MIN_ARB, 1.0);
+          glPointParameterfARB(GL_POINT_SIZE_MAX_ARB, 100000.0);
+          GLfloat p[3] = {0.0, 0.0000, 0.0000005};
+          glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
+          displayOctTreeCPAllCulled(*BOctTree<T>::root, BOctTree<T>::center, BOctTree<T>::size,  BOctTree<T>::size/  pow(2, min( (int)(ratio * BOctTree<T>::max_depth ), BOctTree<T>::max_depth - 3) ) ); 
+          p[0] = 1.0;
+          p[2] = 0.0;
+          glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
+        }
+#endif
+*/
+      //break;
+      case 0:
+        glBegin(GL_POINTS);
+        displayOctTreeCulledLOD(maxtargetpoints * ratio, *root, center, size); 
+        glEnd();
+        break;
+      default:
+      break;
+    }
+}
+
+void compactTree::draw() { 
+  displayOctTreeAllCulled(*root, center, size); 
+}
+
+void compactTree::displayOctTree(double minsize ) { 
+  displayOctTreeCAllCulled(*root, center, size, minsize); 
+}
+
+shortpointrep* compactTree::createPoints(lint length) {
+  //shortpointrep *points = new shortpointrep[POINTDIM*length];
+  shortpointrep *points = alloc->allocate<shortpointrep> (POINTDIM*length);
+  return points;
+}
+void compactTree::deserialize(std::string filename ) {
+  char buffer[sizeof(float) * 20];
+  float *p = reinterpret_cast<float*>(buffer);
+
+  std::ifstream file;
+  file.open (filename.c_str(), std::ios::in | std::ios::binary);
+
+  // read magic bits
+  file.read(buffer, 2);
+  if ( buffer[0] != 'X' || buffer[1] != 'T') {
+    std::cerr << "Not an octree file!!" << endl;
+    file.close();
+    return;
+  }
+
+  // read header
+  pointtype = PointType::deserialize(file);
+
+  file.read(buffer, 5 * sizeof(float));
+  voxelSize = p[0];
+  center[0] = p[1];
+  center[1] = p[2];
+  center[2] = p[3];
+  size = p[4];
+
+  file.read(buffer, sizeof(int));
+  int *ip = reinterpret_cast<int*>(buffer);
+  POINTDIM = *ip;
+
+  float *fmins = new float[POINTDIM];
+  float *fmaxs = new float[POINTDIM];
+  mins = new double[POINTDIM];
+  maxs = new double[POINTDIM];
+
+  file.read(reinterpret_cast<char*>(fmins), POINTDIM * sizeof(float));
+  file.read(reinterpret_cast<char*>(fmaxs), POINTDIM * sizeof(float));
+
+  for (unsigned int i = 0; i < POINTDIM; i++) {
+    mins[i] = fmins[i];
+    maxs[i] = fmaxs[i];
+  }
+
+  double vs = size;
+  while (vs > voxelSize) {
+    vs = vs * 0.5;
+  }
+//  precision = vs / 32768;  // 2^15
+  precision = vs / TSHORT_MAXP1;  // 2^15
+
+
+  // read root node
+  //root = new cbitoct();
+  root = alloc->allocate<cbitoct>();    
+  deserialize(file, *root );
+  file.close();
+}
+
+
+
+
+void compactTree::deserialize(std::ifstream &f, cbitoct &node) {
+  char buffer[2];
+  f.read(buffer, 2);
+  node.valid = buffer[0];
+  node.leaf = buffer[1];
+
+  unsigned short n_children = POPCOUNT(node.valid);
+
+  // create children
+  //cbitunion<tshort> *children = new cbitunion<tshort>[n_children];
+  cbitunion<tshort> *children = alloc->allocate<cbitunion<tshort> >(n_children);    
+  cbitoct::link(node, children);
+
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf read points 
+        lint length;
+        f.read(reinterpret_cast<char*>(&length), sizeof(lint));
+        shortpointrep *points = createPoints(length); 
+
+        f.read(reinterpret_cast<char*>(points), sizeof(shortpointrep) * length * POINTDIM); // read the points
+        children->linkPoints(points, length); 
+      } else {  // write child 
+        deserialize(f, children->node);
+      }
+      ++children; // next child
+    }
+  }
+}
+
+
+
+void compactTree::serialize(std::string filename) {
+  char buffer[sizeof(float) * 20];
+  float *p = reinterpret_cast<float*>(buffer);
+
+  std::ofstream file;
+  file.open (filename.c_str(), std::ios::out | std::ios::binary);
+
+  // write magic bits
+  buffer[0] = 'X';
+  buffer[1] = 'T';
+  file.write(buffer, 2);
+
+  // write header
+  pointtype.serialize(file);
+
+  p[0] = voxelSize;
+  p[1] = center[0]; 
+  p[2] = center[1]; 
+  p[3] = center[2];
+  p[4] = size;
+
+  int *ip = reinterpret_cast<int*>(&(buffer[5 * sizeof(float)]));
+  *ip = POINTDIM;
+
+  file.write(buffer, 5 * sizeof(float) + sizeof(int));
+
+
+  for (unsigned int i = 0; i < POINTDIM; i++) {
+    p[i] = mins[i];
+  }
+  for (unsigned int i = 0; i < POINTDIM; i++) {
+    p[i+POINTDIM] = maxs[i];
+  }
+
+  file.write(buffer, 2*POINTDIM * sizeof(float));
+
+  // write root node
+  serialize(file, *root);
+
+  file.close();
+}
+
+
+void compactTree::serialize(std::ofstream &of, cbitoct &node) {
+  char buffer[2];
+  buffer[0] = node.valid;
+  buffer[1] = node.leaf;
+  of.write(buffer, 2);
+
+  // write children
+  cbitunion<tshort> *children;
+  cbitoct::getChildren(node, children);
+  for (short i = 0; i < 8; i++) {
+    if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+      if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf write points 
+        tshort *points = children->getPoints();
+        lint length = children->getLength();
+
+        of.write(reinterpret_cast<char*>(&length), sizeof(lint) );
+
+        of.write(reinterpret_cast<char*>(points), POINTDIM*length*sizeof(tshort) );
+
+      } else {  // write child 
+        serialize(of, children->node);
+      }
+      ++children; // next child
+    }
+  }
+}

.svn/pristine/04/048d5ca5d56fbbc8f62b96fad293aa3647b1b764.svn-base

@@ -0,0 +1,269 @@
+/**
+  * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm
+  *
+  * Copyright (C) Jacobs University Bremen
+  *
+  * Released under the GPL version 3.
+  *
+  * @author Mihai-Cotizo Sima
+  */
+
+#include <segmentation/FHGraph.h>
+#include <map>
+#include <omp.h>
+#include <algorithm>
+
+using namespace std;
+
+
+
+FHGraph::FHGraph(std::vector< Point >& ps, double weight(Point, Point), double sigma, double eps, int neighbors, float radius) :
+    points( ps ), V( ps.size() )
+{
+    /*
+     * 1. create adjency list using a map<int, vector<half_edge> >
+     * 2. use get_neighbors(e, max_dist) to get all the edges e' that are at a distance smaller than max_dist than e
+     * 3. using all these edges, compute the gaussian smoothed weight
+     * 4. insert the edges in a new list
+     */
+    nr_neighbors = neighbors;
+    this->radius = radius;
+
+    compute_neighbors(weight, eps);
+
+
+    if ( sigma > 0.01 )
+    {
+        do_gauss(sigma);
+    }
+    else
+    {
+        without_gauss();
+    }
+
+    adjency_list.clear();
+}
+
+void FHGraph::compute_neighbors(double weight(Point, Point), double eps)
+{
+
+    adjency_list.reserve(points.size());
+    adjency_list.resize(points.size());
+
+    ANNpointArray pa = annAllocPts(points.size(), 3);
+    for (size_t i=0; i<points.size(); ++i)
+    {
+        pa[i] = new ANNcoord[3];
+        pa[i][0] = points[i].x;
+        pa[i][1] = points[i].y;
+        pa[i][2] = points[i].z;
+    }
+
+    ANNkd_tree t(pa, points.size(), 3);
+
+    if ( radius < 0 ) // Using knn search
+    {
+        nr_neighbors++;
+        ANNidxArray n = new ANNidx[nr_neighbors];
+        ANNdistArray d = new ANNdist[nr_neighbors];
+
+        for (size_t i=0; i<points.size(); ++i)
+        {
+            ANNpoint p = pa[i];
+
+            t.annkSearch(p, nr_neighbors, n, d, eps);
+
+            for (int j=0; j<nr_neighbors; ++j)
+            {
+                if ( n[j] == (int)i ) continue;
+
+                he e;
+                e.x = n[j];
+                e.w = weight(points[i], points[n[j]]);
+
+                adjency_list[i].push_back(e);
+            }
+
+        }
+
+        delete[] n;
+        delete[] d;
+    }
+    else // Using radius search
+    {
+        float sqradius = radius*radius;
+
+
+        ANNidxArray n;
+        ANNdistArray d;
+        int nret;
+        int total = 0;
+
+        const int MOD = 1000;
+        int TMP = MOD;
+
+        for (size_t i=0; i<points.size(); ++i)
+        {
+            ANNpoint p = pa[i];
+
+            nret = t.annkFRSearch(p, sqradius, 0, NULL, NULL, eps);
+            total += nret;
+
+            n = new ANNidx[nret];
+            d = new ANNdist[nret];
+            t.annkFRSearch(p, sqradius, nret, n, d, eps);
+
+            if ( nr_neighbors > 0 && nr_neighbors < nret )
+            {
+                random_shuffle(n, n+nret);
+                nret = nr_neighbors;
+            }
+
+            for (int j=0; j<nret; ++j)
+            {
+                if ( n[j] == (int)i ) continue;
+
+                he e;
+                e.x = n[j];
+                e.w = weight(points[i], points[n[j]]);
+
+                adjency_list[i].push_back(e);
+            }
+
+            delete[] n;
+            delete[] d;
+            if ( TMP==0 )
+            {
+                TMP = MOD;
+                cout << "Point " << i << "/" << V << ", or "<< (i*100.0 / V) << "%\r"; cout.flush();
+            }
+            TMP --;
+        }
+        cout << "Average nr of neighbors: " << (float) total / points.size() << endl;
+
+    }
+
+    annDeallocPts(pa);
+}
+
+static double gauss(double x, double miu, double sigma)
+{
+    double tmp = ((x-miu)/sigma);
+    return exp(- .5 * tmp * tmp);
+}
+
+static void normalize(std::vector<double>& v)
+{
+    double s = 0;
+    for (size_t i=0; i<v.size(); ++i)
+        s += v[i];
+    for (size_t i=0; i<v.size(); ++i)
+        v[i] /= s;
+}
+
+string tostr(int x)
+{
+    stringstream ss;
+    ss << x;
+    return ss.str();
+}
+
+void FHGraph::do_gauss(double sigma)
+{
+    edges.reserve( V * nr_neighbors);
+    list<he>::iterator k, j;
+    vector<double> gauss_weight, edge_weight;
+    edge e;
+#pragma omp parallel for private(j, k, e, gauss_weight, edge_weight) schedule(dynamic)
+    for (int i=0; i<V; ++i)
+    {
+        for (j=adjency_list[i].begin();
+             j!=adjency_list[i].end();
+             j++)
+        {
+
+            gauss_weight.clear();
+            edge_weight.clear();
+
+            for (k=adjency_list[i].begin();
+                 k!=adjency_list[i].end();
+                 ++k)
+            {
+                gauss_weight.push_back(gauss(k->w, j->w, sigma));
+                edge_weight.push_back(k->w);
+            }
+            for (k=adjency_list[j->x].begin();
+                 k!=adjency_list[j->x].end();
+                 ++k)
+            {
+                gauss_weight.push_back(gauss(k->w, j->w, sigma));
+                edge_weight.push_back(k->w);
+            }
+            normalize(gauss_weight);
+
+            e.a = i; e.b = j->x;
+            e.w = 0;
+            for (size_t k=0; k<edge_weight.size(); ++k)
+                e.w += gauss_weight[k] * edge_weight[k];
+
+#pragma omp critical
+            {
+                edges.push_back(e);
+            }
+        }
+    }
+}
+
+void FHGraph::without_gauss()
+{
+    edges.reserve( V * nr_neighbors);
+    list<he>::iterator j;
+    edge e;
+
+    for (int i=0; i<V; ++i)
+    {
+        for (j=adjency_list[i].begin();
+             j!=adjency_list[i].end();
+             j++)
+        {
+            e.a = i; e.b = j->x; e.w = j->w;
+            edges.push_back(e);
+        }
+    }
+}
+
+edge* FHGraph::getGraph()
+{
+    edge* ret = new edge[edges.size()];
+    for (size_t i=0; i<edges.size(); ++i)
+        ret[i] = edges[i];
+    return ret;
+}
+
+Point FHGraph::operator[](int index)
+{
+    return points[index];
+}
+
+int FHGraph::getNumPoints()
+{
+    return V;
+}
+
+int FHGraph::getNumEdges()
+{
+    return edges.size();
+}
+
+template<typename T>
+void vectorFree(T& t) {
+    T tmp;
+    t.swap(tmp);
+}
+
+void FHGraph::dispose() {
+    vectorFree(edges);
+    vectorFree(points);
+    vectorFree(adjency_list);
+}
+

.svn/pristine/0b/0b791769e2124bae1b4af44cbb37f799db11c8d1.svn-base

@@ -0,0 +1,27 @@
+IF (WITH_THERMO)
+  find_package(OpenCV REQUIRED)
+
+  include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
+  include_directories(${CMAKE_SOURCE_DIR}/include/shapes/)
+  include_directories(${CMAKE_SOURCE_DIR}/include/thermo/)
+  include_directories(/usr/include/)
+  include_directories(/usr/include/opencv)
+  
+  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)
+  
+  IF(UNIX)
+    target_link_libraries(caliboard 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 cv cvaux cxcore GL GLU highgui cvblob scan ANN)
+  ENDIF(WIN32)
+  
+
+ENDIF(WITH_THERMO)

.svn/pristine/13/137cef42436c2c4ae9351f5632a48c0ed06ca884.svn-base

@@ -0,0 +1,322 @@
+/*
+ * plane implementation
+ *
+ * Copyright (C) Dorit Borrmann
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file 
+ * @author Dorit Borrmann. Institute of Computer Science, University of Osnabrueck, Germany.
+*/
+
+#include <cfloat>
+#include <fstream>
+#ifndef _MSC_VER
+#include <getopt.h>
+#else
+#include "XGetopt.h"
+#endif
+
+#include <iostream>
+using std::ofstream;
+using std::flush;
+using std::cout;
+using std::string;
+using std::cerr;
+using std::endl;
+#include <errno.h>
+
+#ifdef _MSC_VER
+#define strcasecmp _stricmp
+#define strncasecmp _strnicmp
+#include <windows.h>
+#include <direct.h>
+#else
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <strings.h>
+#include <dlfcn.h>
+#endif
+
+#include "shapes/hough.h"
+#include "shapes/shape.h"
+#include "shapes/ransac.h"
+
+enum plane_alg { 
+  RHT, SHT, PHT, PPHT, APHT, RANSAC
+};
+
+void usage(char* prog) {
+#ifndef _MSC_VER
+  const string bold("\033[1m");
+  const string normal("\033[m");
+#else
+  const string bold("");
+  const string normal("");
+#endif
+  cout << endl
+	  << bold << "USAGE " << normal << endl
+	  << "   " << prog << " [options] directory" << endl << endl;
+  cout << bold << "OPTIONS" << normal << endl
+
+	  << bold << "  -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
+	  << "         using shared library F for input" << endl
+	  << "         (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl
+	  << endl
+	  << bold << "  -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
+	  << "         neglegt all data points with a distance larger than NR 'units'" << endl
+	  << endl
+	  << bold << "  -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
+	  << "         neglegt all data points with a distance smaller than NR 'units'" << endl
+	  << endl
+	  << bold << "  -p" << normal << " P, " << bold << "--plane=" << normal << "P" << endl
+	  << "         using algorithm P for plane detection" << endl
+	  << "         (chose P from {rht, sht, pht, ppht, apht, ran})" << endl
+	  << endl
+	  << bold << "  -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
+	  << "         turns on octree based point reduction (voxel size=<NR>)" << endl
+	  << endl
+	  << bold << "  -O" << normal << " NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl
+	  << "         use randomized octree based point reduction (pts per voxel=<NR>)" << endl
+	  << "         requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl
+	  << endl
+	  << bold << "  -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
+	  << "         start at scan NR (i.e., neglects the first NR scans)" << endl
+	  << "         [ATTENTION: counting naturally starts with 0]" << endl
+	  << endl
+	  << bold << "  -S, --scanserver" << normal << endl
+	  << "         Use the scanserver as an input method and handling of scan data" << endl
+	  << endl
+    	  << endl << endl;
+  
+  cout << bold << "EXAMPLES " << normal << endl
+	  << "   " << prog << " -m 500 -r 5 dat" << endl
+	  << "   " << prog << " --max=5000 -r 10.2 dat" << endl
+	  << "   " << prog << " -s 2 -e 10 -r dat" << endl << endl;
+  exit(1);
+
+}
+
+/**
+  * Parses command line arguments needed for plane detection. For details about
+  * the argument see usage().
+  */
+
+int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int
+		    &maxDist, int&minDist, int &octree, IOType &type, plane_alg &alg, bool
+		    &quiet, bool& scanserver)
+{
+  bool reduced = false;
+  int  c;
+  // from unistd.h:
+  extern char *optarg;
+  extern int optind;
+
+  /* options descriptor */
+  // 0: no arguments, 1: required argument, 2: optional argument
+  static struct option longopts[] = {
+    { "format",          required_argument,   0,  'f' },  
+    { "max",             required_argument,   0,  'm' },
+    { "min",             required_argument,   0,  'M' },
+    { "start",           required_argument,   0,  's' },
+    { "reduce",          required_argument,   0,  'r' },
+    { "plane",           required_argument,   0,  'p' },
+    { "quiet",            no_argument,         0,  'q' },
+    { "octree",          optional_argument,   0,  'O' },
+    { "scanserver",      no_argument,         0,  'S' },
+    { 0,           0,   0,   0}                    // needed, cf. getopt.h
+  };
+
+  cout << endl;
+  while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:p:O:q", longopts, NULL)) != -1) 
+    switch (c)
+	 {
+	 case 'r':
+	   red = atof(optarg);
+     reduced = true;
+	   break;
+	 case 's':
+	   start = atoi(optarg);
+	   if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
+	   break;
+	 case 'f':
+     try {
+       type = formatname_to_io_type(optarg);
+     } catch (...) { // runtime_error
+       cerr << "Format " << optarg << " unknown." << endl;
+       abort();
+     }
+     break;
+   case 'p': 
+      if(strcasecmp(optarg, "rht") == 0) alg = RHT;
+      else if(strcasecmp(optarg, "sht") == 0) alg = SHT;
+      else if(strcasecmp(optarg, "pht") == 0) alg = PHT;
+      else if(strcasecmp(optarg, "ppht") == 0) alg = PPHT;
+      else if(strcasecmp(optarg, "apht") == 0) alg = APHT;
+      else if(strcasecmp(optarg, "ran") == 0) alg = RANSAC;
+      else abort();
+      break;
+	 case 'q':
+	   quiet = true;
+     break;
+	 case 'm':
+	   maxDist = atoi(optarg);
+	   break;
+	 case 'O':
+	   if (optarg) {
+		octree = atoi(optarg);
+	   } else {
+		octree = 1;
+	   }
+	   break;
+	 case 'M':
+	   minDist = atoi(optarg);
+	   break;
+	 case 'S':
+        scanserver = true;
+        break;
+	 case '?':
+	   usage(argv[0]);
+	   return 1;
+      default:
+	   abort ();
+      }
+
+  if (optind != argc-1) {
+    cerr << "\n*** Directory missing ***" << endl;
+    usage(argv[0]);
+  }
+  dir = argv[optind];
+
+#ifndef _MSC_VER
+  if (dir[dir.length()-1] != '/') dir = dir + "/";
+#else
+  if (dir[dir.length()-1] != '\\') dir = dir + "\\";
+#endif
+
+  return 0;
+}
+
+/**
+ * Main function. The Hough Transform or RANSAC are called for the scan indicated as
+ * argument.
+ *
+ */
+int main(int argc, char **argv) 
+{
+
+  cout << "(c) Jacobs University Bremen, gGmbH, 2010" << endl << endl;
+  
+  if (argc <= 1) {
+    usage(argv[0]);
+  }
+  // parsing the command line parameters
+  // init, default values if not specified
+  string dir;
+  double red   = -1.0;
+  int    start = 0;
+  int    maxDist    = -1;
+  int    minDist    = -1;
+  int    octree     = 0;
+  bool   quiet = false;
+  IOType type    = UOS;
+  plane_alg alg    = RHT;
+  bool   scanserver = false;
+  
+  cout << "Parse args" << endl;
+  parseArgs(argc, argv, dir, red, start, maxDist, minDist, octree, type, alg, quiet, scanserver);
+  int fileNr = start;
+  string planedir = dir + "planes"; 
+
+#ifdef _MSC_VER
+  int success = mkdir(planedir.c_str());
+#else
+  int success = mkdir(planedir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
+#endif
+  if(success == 0) { 
+    if(!quiet) {
+      cout << "Writing planes to " << planedir << endl;
+    }
+  } else if(errno == EEXIST) {
+    cout << "Directory " << planedir << " exists already.  CONTINUE" << endl; 
+  } else {
+    cerr << "Creating directory " << planedir << " failed" << endl;
+    exit(1);
+  }
+
+  Scan::openDirectory(scanserver, dir, type, fileNr, fileNr);
+  Scan* scan = Scan::allScans.front();
+  scan->setRangeFilter(maxDist, minDist);
+  scan->setReductionParameter(red, octree);
+  //    scan->setSearchTreeParameter(nns_method, use_cuda);
+  scan->toGlobal();
+
+  double id[16];
+  M4identity(id);
+  for(int i = 0; i < 10; i++) {
+    scan->transform(id, Scan::ICP, 0);  // write end pose
+  }
+
+  if (!quiet) cout << "start plane detection" << endl;
+  long starttime = GetCurrentTimeInMilliSec(); 
+  if(alg >= RANSAC) {
+    vector<double *> points;
+    CollisionPlane<double> * plane;
+    plane = new CollisionPlane<double>(1.0); // 1.0 cm maxdist
+    Ransac(*plane, Scan::allScans[0], &points);
+    starttime = (GetCurrentTimeInMilliSec() - starttime);
+
+    cout << "nr points " << points.size() << endl;
+    double nx,ny,nz,d;
+    plane->getPlane(nx,ny,nz,d);
+    if(!quiet) cout << "DONE " << endl;
+
+    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;
+    }
+    */
+    for(int i = points.size() - 1; i > -1; i--) {
+      delete[] points[i];
+    }
+    
+    delete plane;
+  } else {
+    Hough hough(Scan::allScans[0], quiet);
+    starttime = (GetCurrentTimeInMilliSec() - starttime);
+    cout << "Time for Constructor call: " << starttime << endl;
+
+    starttime = GetCurrentTimeInMilliSec(); 
+    if (!quiet) cout << "algorithm: " << alg << endl;
+    // choose Hough method here
+    switch(alg) {
+      case RHT: hough.RHT();
+                break;
+      case SHT: hough.SHT();
+                break;
+      case PHT: hough.PHT();
+                break;
+      case PPHT:  hough.PPHT();
+                  break;
+      case APHT:  hough.APHT();
+                  break;
+      default:  usage(argv[0]);
+                exit(1);
+                break;
+    }
+
+    hough.writePlanes(0);
+    cout << "Write Planes done" << endl;
+    starttime = (GetCurrentTimeInMilliSec() - starttime);
+  }
+
+  cout << "Time for Plane Detection " << starttime << endl;
+  delete Scan::allScans[0];
+  Scan::allScans.clear();
+}
+

.svn/pristine/1a/1a4914ee1edbcfa2063c6bbb823444a5fec55e6b.svn-base

@@ -0,0 +1,69 @@
+/*
+ * kd implementation
+ *
+ * Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/** @file 
+ *  @brief An optimized k-d tree implementation
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+
+#ifdef _MSC_VER
+#define  _USE_MATH_DEFINES
+#endif
+
+#include "slam6d/kd.h"
+#include "slam6d/globals.icc"
+
+#include <iostream>
+using std::cout;
+using std::cerr;
+using std::endl;
+#include <algorithm>
+using std::swap;
+#include <cmath>
+#include <cstring>
+
+// KDtree class static variables
+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
+ */
+KDtree::KDtree(double **pts, int n)
+{
+    create(Void(), pts, n);
+}
+
+KDtree::~KDtree()
+{
+}
+
+/**
+ * Finds the closest point within the tree,
+ * wrt. the point given as first parameter.
+ * @param _p point
+ * @param maxdist2 maximal search distance.
+ * @param threadNum Thread number, for parallelization
+ * @return Pointer to the closest point
+ */
+double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const
+{
+  params[threadNum].closest = 0;
+  params[threadNum].closest_d2 = maxdist2;
+  params[threadNum].p = _p;
+  _FindClosest(Void(), threadNum);
+  return params[threadNum].closest;
+}

.svn/pristine/1b/1bc82d9ef314c49f5f3477cfcd5b20f21d500a62.svn-base

@@ -0,0 +1,80 @@
+/** @file 
+ *  @brief Representation of the optimized k-d tree. MetaScan variant.
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher
+ */
+
+#ifndef __KD_META_H__
+#define __KD_META_H__
+
+#include "kdparams.h"
+#include "searchTree.h"
+#include "data_types.h"
+#include "kdTreeImpl.h"
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+#ifdef _MSC_VER
+#if !defined _OPENMP && defined OPENMP 
+#define _OPENMP
+#endif
+#endif
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+
+
+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. 
+ * 
+ * A kD tree for points, with limited
+ * capabilities (find nearest point to
+ * a given point, or to a ray).
+ **/
+class KDtreeMetaManaged : 
+    public SearchTree,
+    private KDTreeImpl<const DataXYZ* const*, Index, IndexAccessor>
+{
+public:
+  KDtreeMetaManaged(const vector<Scan*>& scans);
+  virtual ~KDtreeMetaManaged();
+  
+  virtual void lock();
+  virtual void unlock();
+
+  //! Aquires cached data first to pass on to the usual KDtree to process
+  virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
+private:
+  Scan** m_scans;
+  DataXYZ** m_data;
+  unsigned int m_size;
+
+  //! Mutex for safely reducing points just once in a multithreaded environment
+  boost::mutex m_mutex_locking;
+  volatile unsigned int m_count_locking;
+
+  // constructor initializer list hacks
+  Index* m_temp_indices;
+  Index* prepareTempIndices(const vector<Scan*>& scans);
+  unsigned int getPointsSize(const vector<Scan*>& scans);
+};
+
+#endif

.svn/pristine/1f/1ff694340f907a92134585612918621390a04042.svn-base

@@ -0,0 +1,297 @@
+/**
+  * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm
+  *
+  * Copyright (C) Jacobs University Bremen
+  *
+  * Released under the GPL version 3.
+  *
+  * @author Billy Okal <b.okal@jacobs-university.de>
+  * @author Mihai-Cotizo Sima
+  * @file fhsegmentation.cc
+  */
+
+#include <iostream>
+#include <string>
+#include <fstream>
+#include <errno.h>
+
+#include <boost/program_options.hpp>
+
+#include <slam6d/io_types.h>
+#include <slam6d/globals.icc>
+#include <slam6d/scan.h>
+#include <scanserver/clientInterface.h>
+
+#include <segmentation/FHGraph.h>
+
+#ifdef _MSC_VER
+#define strcasecmp _stricmp
+#define strncasecmp _strnicmp
+#else
+#include <strings.h>
+#endif
+
+namespace po = boost::program_options;
+using namespace std;
+
+/// validate IO types
+void validate(boost::any& v, const std::vector<std::string>& values,
+    IOType*, int) {
+    if (values.size() == 0)
+        throw std::runtime_error("Invalid model specification");
+    string arg = values.at(0);
+    try {
+        v = formatname_to_io_type(arg.c_str());
+    } catch (...) { // runtime_error
+        throw std::runtime_error("Format " + arg + " unknown.");
+    }
+}
+
+/// Parse commandline options
+void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir,
+                   IOType &iotype, float &sigma, int &k, int &neighbors, float &eps, float &radius, int &min_size)
+{
+    /// ----------------------------------
+    /// set up program commandline options
+    /// ----------------------------------
+    po::options_description cmd_options("Usage: fhsegmentation <options> where options are (default values in brackets)");
+    cmd_options.add_options()
+            ("help,?", "Display this help message")
+            ("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
+            ("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
+            ("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
+            ("format,f", po::value<IOType>(&iotype)->default_value(UOS),
+             "using shared library <arg> for input. (chose format from [uos|uosr|uos_map|"
+             "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|"
+             "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])")
+            ("max,M", po::value<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
+            ("min,m", po::value<int>(&min_dist)->default_value(-1), "neglegt all data points with a distance smaller than <arg> 'units")
+            ("K,k", po::value<int>(&k)->default_value(1), "<arg> value of K value used in the FH segmentation")
+            ("neighbors,N", po::value<int>(&neighbors)->default_value(1), "use approximate <arg>-nearest neighbors search or limit the number of points")
+            ("sigma,v", po::value<float>(&sigma)->default_value(1.0), "Set the Gaussian variance  for smoothing to <arg>")
+            ("radius,r", po::value<float>(&radius)->default_value(-1.0), "Set the range of radius search to <arg>")
+            ("eps,E", po::value<float>(&eps)->default_value(1.0), "Set error threshold used by the AKNN algorithm to <arg>")
+            ("minsize,z", po::value<int>(&min_size)->default_value(0), "Keep segments of size at least <arg>")
+            ;
+
+    po::options_description hidden("Hidden options");
+    hidden.add_options()
+        ("input-dir", po::value<string>(&dir), "input dir");
+
+    po::positional_options_description pd;
+    pd.add("input-dir", 1);
+
+    po::options_description all;
+    all.add(cmd_options).add(hidden);
+
+    po::variables_map vmap;
+    po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap);
+    po::notify(vmap);
+
+    if (vmap.count("help")) {
+        cout << cmd_options << endl;
+        exit(-1);
+    }
+
+    // read scan path
+    if (dir[dir.length()-1] != '/') dir = dir + "/";
+
+}
+
+/// distance measures
+double weight1(Point a, Point b)
+{
+    return a.distance(b);
+}
+
+double weight2(Point a, Point b)
+{
+    return a.distance(b) * .5 + fabs(a.reflectance-b.reflectance) * .5;
+}
+
+
+/// Write a pose file with the specofied name
+void writePoseFiles(string dir, const double* rPos, const double* rPosTheta, int num, int outnum)
+{
+    for (int i = outnum; i < num; i++) {
+        string poseFileName = dir + "segments/scan" + to_string(i, 3) + ".pose";
+        ofstream posout(poseFileName.c_str());
+
+        posout << rPos[0] << " "
+               << rPos[1] << " "
+               << rPos[2] << endl
+               << deg(rPosTheta[0]) << " "
+               << deg(rPosTheta[1]) << " "
+               << deg(rPosTheta[2]) << endl;
+        posout.clear();
+        posout.close();
+    }
+}
+
+/// write scan files for all segments
+void writeScanFiles(string dir, int outnum, const vector<vector<Point>* > cloud)
+{
+    for (int i = outnum, j = 0; i < (int)cloud.size() && j < (int)cloud.size(); i++, j++) {
+        vector<Point>* segment = cloud[j];
+        string scanFileName = dir + "segments/scan" + to_string(i,3) + ".3d";
+        ofstream scanout(scanFileName.c_str());
+
+        for (int k = 0; k < (int)segment->size(); k++) {
+            Point p = segment->at(k);
+            scanout << p.x << " " << p.y << " " << p.z << endl;
+        }
+        scanout.close();
+    }
+}
+
+
+
+/// =============================================
+/// Main
+/// =============================================
+int main(int argc, char** argv)
+{
+    int start, end;
+    bool scanserver;
+    int max_dist, min_dist;
+    string dir;
+    IOType iotype;
+    float sigma;
+    int k, neighbors;
+    float eps;
+    float radius;
+    int min_size;
+
+    parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
+                  dir, iotype, sigma, k, neighbors, eps, radius, min_size);
+
+    /// ----------------------------------
+    /// Prepare and read scans
+    /// ----------------------------------
+    if (scanserver) {
+        try {
+            ClientInterface::create();
+        } catch(std::runtime_error& e) {
+            cerr << "ClientInterface could not be created: " << e.what() << endl;
+            cerr << "Start the scanserver first." << endl;
+            exit(-1);
+        }
+    }
+
+    /// Make directory for saving the scan segments
+    string segdir = dir + "segments";
+
+#ifdef _MSC_VER
+    int success = mkdir(segdir.c_str());
+#else
+    int success = mkdir(segdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
+#endif
+    if(success == 0) {
+        cout << "Writing segments to " << segdir << endl;
+    } else if(errno == EEXIST) {
+        cout << "WARN: Directory " << segdir << " exists already. Contents will be overwriten" << endl;
+    } else {
+        cerr << "Creating directory " << segdir << " failed" << endl;
+        exit(1);
+    }
+
+    /// Read the scans
+    Scan::openDirectory(scanserver, dir, iotype, start, end);
+    if(Scan::allScans.size() == 0) {
+        cerr << "No scans found. Did you use the correct format?" << endl;
+        exit(-1);
+    }
+
+    /// --------------------------------------------
+    /// Initialize and perform segmentation
+    /// --------------------------------------------
+    std::vector<Scan*>::iterator it = Scan::allScans.begin();
+    int outscan = start;
+
+    for( ; it != Scan::allScans.end(); ++it) {
+        Scan* scan = *it;
+        const double* rPos = scan->get_rPos();
+        const double* rPosTheta = scan->get_rPosTheta();
+
+        /// read scan into points
+        DataXYZ xyz(scan->get("xyz"));
+        vector<Point> points;
+        points.reserve(xyz.size());
+
+        for(unsigned int j = 0; j < xyz.size(); j++) {
+            Point p(xyz[j][0], xyz[j][1], xyz[j][2]);
+            points.push_back(p);
+        }
+
+        /// create the graph and get the segments
+        cout << "creating graph" << endl;
+        FHGraph sgraph(points, weight2, sigma, eps, neighbors, radius);
+
+        cout << "segmenting graph" << endl;
+        edge* sedges = sgraph.getGraph();
+        universe* segmented = segment_graph(sgraph.getNumPoints(), 
+                                            sgraph.getNumEdges(), 
+                                            sedges, k);
+
+        cout << "post processing" << endl;
+        for (int i = 0; i < sgraph.getNumEdges(); ++i)
+        {
+            int a = sedges[i].a;
+            int b = sedges[i].b;
+
+            int aa = segmented->find(a);
+            int bb = segmented->find(b);
+
+            if ( (aa!=bb) && 
+                 (segmented->size(aa) < min_size || 
+                  segmented->size(bb) < min_size) )
+                segmented->join(aa, bb);
+        }
+
+        delete[] sedges;
+
+        int nr = segmented->num_sets();
+        cout << "Obtained " << nr << " segment(s)" << endl;
+
+        /// write point clouds with segments
+        vector< vector<Point>* > clouds;
+        clouds.reserve(nr);
+        for (int i=0; i<nr; ++i)
+            clouds.push_back( new vector<Point> );
+
+        map<int, int> components2cloud;
+        int kk = 0;
+
+        for (int i = 0; i < sgraph.getNumPoints(); ++i)
+        {
+            int component = segmented->find(i);
+            if ( components2cloud.find(component)==components2cloud.end() )
+            {
+                components2cloud[component] = kk++;
+                clouds[components2cloud[component]]->reserve(segmented->size(component));
+            }
+            clouds[components2cloud[component]]->push_back(sgraph[i]);
+        }
+
+        // pose file (repeated for the number of segments
+        writePoseFiles(dir, rPos, rPosTheta, clouds.size(), outscan);
+        // scan files for all segments
+        writeScanFiles(dir, outscan, clouds);
+
+        outscan += clouds.size();
+
+        /// clean up
+        sgraph.dispose();
+    }
+
+    // shutdown everything
+    if (scanserver)
+        ClientInterface::destroy();
+    else
+        Scan::closeDirectory();
+
+    cout << "Normal program end" << endl;
+
+    return 0;
+}
+

.svn/pristine/2c/2ce0f84a30216373bcc91d1d3a176c840d15e404.svn-base

@@ -0,0 +1,982 @@
+/*
+ * veloscan implementation
+ *
+ * Copyright (C) Andreas Nuechter, Li Wei, Li Ming
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file
+ * @brief Implementation of a 3D scan and of 3D scan matching in all variants
+ * @author Li Wei, Wuhan University, China
+ * @author Li Ming, Wuhan University, China
+ * @author Andreas Nuechter. Jacobs University Bremen, Germany
+ */
+
+#ifdef _MSC_VER
+#ifdef OPENMP
+#define _OPENMP
+#endif
+#endif
+
+#include <fstream>
+using std::ifstream;
+using std::ofstream;
+
+#include <iostream>
+using std::cout;
+using std::cerr;
+using std::endl;
+
+#include <sstream>
+using std::stringstream;
+
+#include "veloslam/veloscan.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+#ifdef _MSC_VER
+#include <windows.h>
+#else
+#include <dlfcn.h>
+#endif
+
+#ifdef _MSC_VER
+#define strcasecmp _stricmp
+#define strncasecmp _strnicmp
+#else
+#include <strings.h>
+#endif
+
+#include <cstring>
+using std::flush;
+
+#include "slam6d/Boctree.h"
+#include "veloslam/veloscan.h"
+#include "veloslam/pcddump.h"
+#include "veloslam/trackermanager.h"
+#include "veloslam/clusterboundingbox.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+#ifdef _MSC_VER
+#include <windows.h>
+#else
+#include <dlfcn.h>
+#endif
+
+#ifdef _MSC_VER
+#define strcasecmp _stricmp
+#define strncasecmp _strnicmp
+#else
+#include <strings.h>
+#endif
+
+#include <cstring>
+using std::flush;
+
+#include <GL/gl.h>
+#include <GL/glu.h>
+
+#ifdef _MSC_VER
+#include <GL/glut.h>
+#else
+#include <GL/freeglut.h>
+#endif
+#include "veloslam/velodefs.h"
+#include "veloslam/color_util.h"
+
+int scanCount =0;
+TrackerManager trackMgr;
+float absf(float a)
+{
+	return a>0?a:-a;
+}
+
+Trajectory::Trajectory()
+{
+
+}
+
+
+/**
+ * default Constructor
+ */
+VeloScan::VeloScan()
+//    : BasicScan()
+{
+    isTrackerHandled =false;
+}
+
+/**
+ * Desctuctor
+ */
+VeloScan::~VeloScan()
+{
+	FreeAllCellAndCluterMemory();
+}
+
+int VeloScan::DeletePoints()
+{
+	FreeAllCellAndCluterMemory();
+	return 0;
+}
+
+/**
+ * Copy constructor
+ */
+/*
+VeloScan::VeloScan(const VeloScan& s)
+    : BasicScan(s)
+{ }
+*/
+
+int VeloScan::TransferToCellArray(int maxDist, int minDist)
+{
+#define  DefaultColumnSize 360
+    Point P;
+    DataXYZ xyz(get("xyz"));
+    int size= xyz.size();
+
+    int columnSize= 360;	//cfg.cfgPlaneDetect.ColumnSize;
+    int CellSize= 50;	    //cfg.cfgPlaneDetect.CellSize;
+
+    int MinRad=minDist;     //cfg.cfgPlaneDetect.MinRad;
+    int MaxRad=maxDist;      //cfg.cfgPlaneDetect.MaxRad
+
+    if((MaxRad-MinRad)%CellSize!=0)
+        CellSize=10;
+
+    int i,j,count=0;
+    int CellNumber=(MaxRad-MinRad)/CellSize;
+    float flag;
+    int offset;
+
+    if(columnSize==0)
+        return -1;
+
+    if(columnSize%360!=0)
+        columnSize=DefaultColumnSize;
+
+    int sectionSize=columnSize/8;
+
+    scanCellArray.resize(columnSize);
+    for(i=0; i<columnSize; ++i)
+        scanCellArray[i].resize(CellNumber);
+
+    float inc=(M_PI*2)/columnSize;
+
+    vector<float> tanv;
+    vector<float>::iterator result;
+
+    for(i=0; i<sectionSize; ++i)
+    {
+        tanv.push_back(tan(inc*i));
+    }
+
+    int diff;
+
+    for(i=0; i<size; ++i)
+    {
+            count++;
+            Point pt;
+            pt.x = xyz[i][0];
+            pt.y = xyz[i][1];
+            pt.z = xyz[i][2];
+
+			pt.point_id = i;  //important   for find point in  scans  ---raw points
+
+            pt.rad = sqrt(pt.x*pt.x + pt.z*pt.z);
+            pt.tan_theta = pt.z/pt.x ;
+
+            if(pt.rad <=MinRad || pt.rad>=MaxRad)
+                continue;
+
+            // some point losted which on the vline or hline
+            if(pt.x >=0 && pt.z>=0 )
+            {
+                if(pt.x > pt.z)
+                {
+                    flag= pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize-1;
+                    }
+                    else
+                    {
+                        offset=result-tanv.begin();
+                    }
+                }
+                else
+                {
+                    flag=1/pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize;
+                    }
+                    else
+                    {
+                        diff=result-tanv.begin();
+                        offset=sectionSize*2-1-(diff);
+                    }
+
+                }
+            }
+
+            else if(pt.x <= 0 && pt.z >=0)
+            {
+                if(-pt.x>pt.z)
+                {
+                    flag=-pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize*3;
+                    }
+                    else
+                    {
+                        offset=sectionSize*4-1-(result-tanv.begin());
+                    }
+
+                }
+                else
+                {
+                    flag=1/-pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize*3-1;
+                    }
+                    else
+                    {
+                        offset=sectionSize*2+(result-tanv.begin());
+                    }
+                }
+            }
+
+            else if(pt.x<=0 && pt.z<=0)
+            {
+                if(-pt.x>-pt.z)
+                {
+                    flag=pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize*5-1;
+                    }
+                    else
+                    {
+                        offset=sectionSize*4+(result-tanv.begin());
+                    }
+                }
+                else
+                {
+                    flag=1/pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize*5;
+                    }
+                    else
+                    {
+                        offset=sectionSize*6-1-(result-tanv.begin());
+                    }
+
+                }
+            }
+
+            else if(pt.x>=0&&pt.z<=0)
+            {
+                if(pt.x>-pt.z)
+                {
+                    flag=-pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize*7;
+                    }
+                    else
+                    {
+                        offset=sectionSize*8-1-(result-tanv.begin());
+                    }
+                }
+                else
+                {
+                    flag=1/-pt.tan_theta;
+                    result=upper_bound(tanv.begin(),tanv.end(),flag);
+                    if(result==tanv.end())
+                    {
+                        offset=sectionSize*7-1;
+                    }
+                    else
+                    {
+                        offset=sectionSize*6+(result-tanv.begin());
+                    }
+                }
+            }
+
+            else
+            {
+                continue;
+            }
+
+            int k= (int)((pt.rad-MinRad)/(CellSize*1.0));
+            scanCellArray[offset][k].push_back(&pt);
+    }
+	return 0;
+}
+
+
+
+int VeloScan::CalcCellFeature(cell& cellobj, cellFeature& f)
+{
+	   int outlier;
+	   float lastMaxY;
+	   f.size=cellobj.size();
+	   f.cellType=0;
+
+	   if(f.size==0)
+	   {
+		   f.cellType|=CELL_TYPE_INVALID;
+		   return 0;
+	   }
+
+	   f.ave_x= f.ave_y = f.ave_z=0.0;
+	   f.delta_y=0;
+
+	   int i=0;
+	   for(i=0; i<f.size; ++i)
+	   {
+		   f.ave_x+=cellobj[i]->x;
+		   f.ave_z+=cellobj[i]->z;
+		   f.ave_y+=cellobj[i]->y;
+
+		//   if(cellobj[i]->type & POINT_TYPE_BELOW_R)
+		//	   f.cellType |=CELL_TYPE_BELOW_R;
+
+		   if(i==0)
+		   {
+			   outlier=0;
+			   f.min_x=f.max_x=cellobj[i]->x;
+			   f.min_z=f.max_z=cellobj[i]->z;
+			   lastMaxY=f.min_y=f.max_y=cellobj[i]->y;
+		   }
+		   else
+		   {
+			   if(f.max_x<cellobj[i]->x)	f.max_x=cellobj[i]->x;
+
+			   if(f.min_x>cellobj[i]->x)	f.min_x=cellobj[i]->x;
+
+			   if(f.max_z<cellobj[i]->z)	f.max_z=cellobj[i]->z;
+
+			   if(f.min_z>cellobj[i]->z)    f.min_z=cellobj[i]->z;
+
+			   if(f.max_y<cellobj[i]->y)
+			   {
+				   lastMaxY=f.max_y;
+				   f.max_y=cellobj[i]->y;
+				   outlier=i;
+			   }
+
+			   if(f.min_y>cellobj[i]->y)
+				   f.min_y=cellobj[i]->y;
+
+		   }
+	   }
+
+	   if(f.size>1)
+	   {
+		   int y=f.ave_y-cellobj[outlier]->y;
+		   y/=(f.size-1)*1.0;
+
+		   if(cellobj[outlier]->y-y<50)
+		   {
+			   outlier=-1;
+			   f.ave_y/=f.size*1.0;
+		   }
+		   else
+		   {
+			   f.max_y=lastMaxY;
+			   f.ave_y=y;
+		   }
+	   }
+	   else
+	   {
+		   outlier=-1;
+		   f.ave_y/=f.size*1.0;
+	   }
+
+	   f.ave_x/=f.size*1.0;
+	   f.ave_z/=f.size*1.0;
+
+	   for(i=0; i<f.size; ++i)
+	   {
+		   if(i==outlier)
+			   continue;
+		   f.delta_y+= absf(cellobj[i]->y - f.ave_y);
+	   }
+
+	   float threshold;
+   	   threshold=f.delta_y;
+
+	   float GridThresholdGroundDetect =120;
+	   if( threshold >  GridThresholdGroundDetect)
+		   f.cellType =CELL_TYPE_STATIC;
+	   else
+		   f.cellType =CELL_TYPE_GROUND;
+
+    return 0;
+}
+
+
+int VeloScan::CalcScanCellFeature()
+{
+    int i,j;
+
+    if( scanCellArray.size()==0)
+        return -1;
+
+    int columnSize=scanCellArray.size();
+    int cellNumber=scanCellArray[0].size();
+
+    if( scanCellFeatureArray.size()==0)
+    {
+        scanCellFeatureArray.resize(columnSize);
+        for(i=0; i<columnSize; ++i)
+            scanCellFeatureArray[i].resize(cellNumber);
+    }
+
+    for(j=0; j <columnSize; j++)
+    {
+        cellColumn &column=scanCellArray[j];
+        cellNumber=column.size();
+        for( i=0; i<cellNumber; i++)
+        {
+            cell &cellObj=scanCellArray[j][i];
+            cellFeature &feature=scanCellFeatureArray[j][i];
+
+			feature.columnID=j;
+			feature.cellID=i;
+
+            feature.pCell=&cellObj;
+            CalcCellFeature(cellObj,feature);
+
+//	         if( feature.delta_y > 120)
+//             {
+//				 scanCellFeatureArray[j][i].cellType |= CELL_TYPE_STATIC;
+//				 for(int k=0;k <scanCellArray[j][i].size(); k++)
+//					 scanCellArray[j][i][k]->type |= POINT_TYPE_ABOVE_DELTA_Y;
+//
+//			 }
+
+        }
+    }
+
+    return 0;
+}
+
+int VeloScan::SearchNeigh(cluster& clu,charvv& flagvv,int i,int j)
+{
+    int columnSize=scanCellArray.size();
+    int cellNumber=scanCellArray[0].size();
+
+	if(i==-1)
+		i= columnSize-1;
+
+	if(i==columnSize)
+		i= 0;
+
+
+    if(i<0||i>=columnSize||j<0||j>=cellNumber)
+        return 0;
+
+    if(flagvv[i][j]==1)
+        return 0;
+    if(scanCellFeatureArray[i][j].size==0)
+    {
+        flagvv[i][j]=1;
+        return 0;
+    }
+
+    if(scanCellFeatureArray[i][j].cellType & CELL_TYPE_STATIC)
+    {
+        flagvv[i][j]=1;
+
+    	clu.push_back(&scanCellFeatureArray[i][j]);
+
+        SearchNeigh(clu,flagvv,i-1,j-1);
+        SearchNeigh(clu,flagvv,i-1,j);
+        SearchNeigh(clu,flagvv,i-1,j+1);
+        SearchNeigh(clu,flagvv,i,j-1);
+        SearchNeigh(clu,flagvv,i,j+1);
+        SearchNeigh(clu,flagvv,i+1,j-1);
+        SearchNeigh(clu,flagvv,i+1,j);
+        SearchNeigh(clu,flagvv,i+1,j+1);
+
+		SearchNeigh(clu,flagvv,i,j+2);
+		SearchNeigh(clu,flagvv,i,j-2);
+		SearchNeigh(clu,flagvv,i+2,j);
+		SearchNeigh(clu,flagvv,i-2,j);
+
+		//SearchNeigh(clu,flagvv,i,j+3);
+		//SearchNeigh(clu,flagvv,i,j-3);
+		//SearchNeigh(clu,flagvv,i+3,j);
+		//SearchNeigh(clu,flagvv,i-3,j);
+    }
+
+    return 0;
+}
+
+int VeloScan::CalcClusterFeature(cluster& clu, clusterFeature& f)
+{
+    f.size=clu.size();
+
+    if(f.size==0)
+    {
+        return 0;
+    }
+
+    f.clusterType=0;
+	f.pointNumber=0;
+
+	f.avg_x =0;  	f.avg_y=0;		f.avg_z=0;
+
+    int i=0;
+    for(i=0; i<f.size; ++i)
+    {
+		f.avg_x += clu[i]->ave_x;
+		f.avg_y += clu[i]->ave_y;
+		f.avg_z += clu[i]->ave_z;
+
+        if(i==0)
+        {
+            f.min_x=f.max_x=clu[i]->min_x;
+ 		    f.min_z=f.max_z=clu[i]->min_z;
+            f.min_y=f.max_y=clu[i]->min_y;
+        }
+        else
+        {
+            if(f.max_x<clu[i]->max_x)
+                f.max_x=clu[i]->max_x;
+
+            if(f.min_x>clu[i]->min_x)
+                f.min_x=clu[i]->min_x;
+
+            if(f.max_z<clu[i]->max_z)
+                f.max_z=clu[i]->max_z;
+
+            if(f.min_z>clu[i]->min_z)
+                f.min_z=clu[i]->min_z;
+
+            if(f.max_y<clu[i]->max_y)
+                f.max_y=clu[i]->max_y;
+
+            if(f.min_y>clu[i]->min_y)
+                f.min_y=clu[i]->min_y;
+
+        }
+		f.pointNumber+=clu[i]->size;
+		f.theta  += clu[i]->size*clu[i]->columnID;
+		f.radius  += clu[i]->size*clu[i]->cellID;
+    }
+
+    f.size_x=f.max_x-f.min_x;
+	f.size_z=f.max_z-f.min_z;
+	f.size_y=f.max_y-f.min_y;
+
+	f.avg_x /= f.size;
+	f.avg_y /= f.size;
+	f.avg_z /= f.size;
+
+	f.theta=f.theta / (f.pointNumber*1.0);
+	f.radius=f.radius / (f.pointNumber*1.0);
+
+    return 0;
+}
+
+int VeloScan::FindAndCalcScanClusterFeature()
+{
+    int i,j;
+
+    if( scanCellArray.size()==0)
+        return -1;
+
+    int columnSize=scanCellArray.size();
+    int cellNumber=scanCellArray[0].size();
+
+    charvv searchedFlag;
+    searchedFlag.resize(columnSize);
+    for(i=0; i<columnSize; ++i)
+        searchedFlag[i].resize(cellNumber,0);
+
+    for(i=0; i<columnSize; ++i)
+    {
+        for(j=0; j<cellNumber; ++j)
+        {
+            cluster clu;
+            SearchNeigh(clu,searchedFlag,i,j);
+            if(clu.size())
+                scanClusterArray.push_back(clu);
+        }
+    }
+
+	int clustersize=scanClusterArray.size();
+	if(scanClusterFeatureArray.size()==0)
+		scanClusterFeatureArray.resize(clustersize);
+
+	cout<<"clusterSize:"<<clustersize<<endl;
+	for(i=0; i<clustersize; ++i)
+	{
+		CalcClusterFeature(scanClusterArray[i],scanClusterFeatureArray[i]);
+//		BoundingBox clusterBox;
+//		clusterBox.CalBestRectangleBox(scanClusterArray[i],scanClusterFeatureArray[i]);
+	}
+	return 0;
+}
+
+void VeloScan::FreeAllCellAndCluterMemory()
+{
+    int i,j;
+    int columnSize=scanCellArray.size();
+    int cellNumber=scanCellArray[0].size();
+
+    for(j=0; j <columnSize; j++)
+    {
+        cellColumn &column=scanCellArray[j];
+        cellFeatureColumn &columnFeature=scanCellFeatureArray[j];
+
+        cellNumber=column.size();
+        for( i=0; i<cellNumber; i++)
+        {
+            cell &cellObj=scanCellArray[j][i];
+            cellFeature &feature=scanCellFeatureArray[j][i];
+            cellObj.clear();
+
+        }
+        column.clear();
+        columnFeature.clear();
+    }
+
+	scanCellArray.clear();
+	scanCellFeatureArray.clear();
+    int ClusterSize=scanClusterArray.size();
+    for(j=0; j <ClusterSize; j++)
+    {
+        cluster &cludata=scanClusterArray[j];
+        clusterFeature &clu=scanClusterFeatureArray[j];
+        cludata.clear();
+  //      clu.clear();
+    }
+	scanClusterArray.clear();
+	scanClusterFeatureArray.clear();
+}
+
+void VeloScan::calcReducedPoints_byClassifi(double voxelSize, int nrpts, PointType pointtype)
+{
+     // only copy the points marked POINT_TYPE_STATIC_OBJECT
+	int realCount =0;
+
+    // get xyz to start the scan load, separated here for time measurement
+    DataXYZ xyz(get("xyz"));
+    DataType Pt(get("type"));
+
+    // if the scan hasn't been loaded we can't calculate anything
+    if(xyz.size() == 0)
+       throw runtime_error("Could not calculate reduced points, XYZ data is empty");
+
+	for (int i = 0;  i < xyz.size(); i++)
+	{
+		if( Pt[i] &  POINT_TYPE_STATIC_OBJECT)
+		{
+			realCount++;
+		}
+    }
+
+	// load  only static part of scans in point_red for icp6D
+    DataXYZ xyz_t(create("xyz reduced", sizeof(double)*3*realCount));
+    realCount=0;
+	int j=0;
+	for (int i = 0;  i < xyz.size(); i++)
+	{
+		if( Pt[i] &  POINT_TYPE_STATIC_OBJECT)
+		{
+			xyz_t[j][0] = xyz[i][0];
+			xyz_t[j][1] = xyz[i][1];
+			xyz_t[j][2] = xyz[i][2];
+			j++;
+			realCount++;
+		}
+    }
+
+	// build octree-tree from CurrentScan
+    // put full data into the octtree
+    BOctTree<double> *oct = new BOctTree<double>(PointerArray<double>(xyz_t).get(),
+      xyz_t.size(), reduction_voxelSize, reduction_pointtype);
+
+    vector<double*> center;
+    center.clear();
+
+    if (reduction_nrpts > 0) {
+      if (reduction_nrpts == 1) {
+        oct->GetOctTreeRandom(center);
+      } else {
+        oct->GetOctTreeRandom(center, reduction_nrpts);
+      }
+    } else {
+      oct->GetOctTreeCenter(center);
+    }
+
+    // storing it as reduced scan
+    unsigned int size = center.size();
+    DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size));
+    for(unsigned int i = 0; i < size; ++i) {
+      for(unsigned int j = 0; j < 3; ++j) {
+        xyz_r[i][j] = center[i][j];
+      }
+    }
+
+    delete oct;
+}
+
+bool findBusCluster(clusterFeature &glu,  cluster &gluData)
+{
+  /*  double labelSVM;
+	for(int i=0;i<360;i++)
+	{
+		nod[i].index=i+1;
+		nod[i].value=intersectionFeature[i].slashLength/slashMaxLength;
+	}
+	nod[360].index=-1;
+	labelSVM= svm_predict(m,nod);
+
+	ofstream output;
+	output.open("intersection.txt");
+	output<<"labelSVM:"<<labelSVM<<endl;
+	for(int j=0;j<360;j++)
+		output<<j<<":"<<"  "<<nod[j].value;
+	output.close();
+
+	if(labelSVM>0.5)
+		cout<<"intersection"<<endl;
+	else
+		cout<<"segment"<<endl;
+	*/
+	return 0;
+}
+
+//long objcount =0;
+// In one scans find which the more like moving object  such as  pedestrian,  car,  bus.
+bool FilterNOMovingObjcets(clusterFeature &glu,  cluster &gluData)
+{
+	// small object do not use it!
+	if(glu.size <3)
+		return false;
+    if(glu.size_x > 800 ||  glu.size_z > 800 )
+   	{
+		return false;
+	}
+	return true;
+    // no filter
+
+	//char  filename[256];
+	//string file;
+	//sprintf(filename,"c:\\filename%d.txt", objcount);
+	//file =filename;
+	//DumpPointtoFile(gluData, file);
+	//DumpFeaturetoFile(glu, "c:\\feature");
+	//objcount++;
+}
+
+//long objcount =0;
+// In one scans find which the more like moving object  such as  pedestrian,  car,  bus.
+bool FindMovingObjcets(clusterFeature &glu,  cluster &gluData)
+{
+	// for debug moving object detections.
+
+	   if( glu.size_y > 200 && ((glu.size_x>glu.size_y?glu.size_x:glu.size_y))<360)
+	   {
+		   return false;
+	   }
+	   else if((glu.size_y>350 && glu.size_x<140)|| (glu.size_x>350 && glu.size_y<140))
+	   {
+		   return false;
+	   }
+	   else if(glu.size_y > 250 )
+	   {
+		   return false;
+	   }
+	   else if((glu.size_x>glu.size_y?glu.size_x:glu.size_y)>420 && glu.size_z<130)
+	   {
+		   return false;
+	   }
+	   else if((glu.size_x>glu.size_y?glu.size_x:glu.size_y)>3.5
+		   && ((glu.size_x>glu.size_y?glu.size_x:glu.size_y)/(glu.size_x<glu.size_y?glu.size_x:glu.size_y)>4))
+	   {
+		   return false;
+	   }
+	   else if(glu.size_x<700 && glu.size_z<700 &&  glu.size_y > 100  )
+	   {
+		   return true;
+	   }
+	   if(glu.size_x>1500 || glu.size_z>1500 || glu.size_x*glu.size_z >600*600 )
+	   {
+		   return false;
+	   }
+	   if (glu.size_x*glu.size_z > 500*500  &&  glu.size_x/glu.size_z < 1.5)
+	   {
+		   return false;
+	   }
+	   if(glu.size_y < 100)
+	   {
+		   return false;
+	   }
+	   if((glu.size_x + glu.size_y + glu.size_z)<1.5)
+	   {
+		   return false;
+	   }
+	   if(glu.size_z>700)
+	   {
+		   return false;
+	   }
+	   if(glu.size_x>700)
+	   {
+		   return false;
+	   }
+	   if(( glu.size_x  +  glu.size_z)  <4)
+	   {
+		   return false;
+	   }
+	   if( glu.size_x/glu.size_z> 3.0)
+	   {
+		   return false;
+	   }
+
+	   return true;
+}
+
+
+// bi classification for distigushed the moving or static
+void VeloScan::ClassifiAllObject()
+{
+    int i,j;
+    int clustersize=scanClusterArray.size();
+
+	//Find moving Ojbects
+	for(i=0; i<clustersize; ++i)
+	{
+     	clusterFeature &glu = scanClusterFeatureArray[i];
+		cluster &gluData=scanClusterArray[i];
+
+	    if( FilterNOMovingObjcets(glu,gluData))
+		{
+               if(FindMovingObjcets(glu,gluData))
+				   glu.clusterType  =CLUSTER_TYPE_MOVING_OBJECT;
+               else
+				   glu.clusterType  =CLUSTER_TYPE_STATIC_OBJECT;
+		}
+        else
+        {
+				   glu.clusterType  =CLUSTER_TYPE_STATIC_OBJECT;
+        }
+	}
+
+	//Mark No Moving Ojbects CELLS
+	int k;
+	for(i=0; i<clustersize; ++i)
+	{
+     	clusterFeature &glu = scanClusterFeatureArray[i];
+		if(glu.clusterType & CLUSTER_TYPE_MOVING_OBJECT)
+		{
+				cluster  &gclu = 	scanClusterArray[i];
+			    for(j =0; j< gclu.size() ; ++j)
+				{
+					cellFeature &gcF = *(gclu[j]);
+					gcF.cellType = CELL_TYPE_MOVING;
+				}
+		}
+        if(glu.clusterType & CLUSTER_TYPE_STATIC_OBJECT)
+	    {
+				cluster  &gclu = 	scanClusterArray[i];
+				for(j =0; j< gclu.size() ; ++j)
+				{
+					cellFeature &gcF = *(gclu[j]);
+					gcF.cellType = CELL_TYPE_STATIC;
+				}
+		}
+
+
+	}
+}
+
+void VeloScan::ClassifibyTrackingAllObject(int currentNO ,int windowsize )
+{
+   trackMgr.ClassifiyTrackersObjects(Scan::allScans, currentNO, windowsize) ;
+}
+
+void VeloScan::MarkStaticorMovingPointCloud()
+{
+	int i,j,k;
+    DataXYZ xyz(get("xyz"));
+	DataType Pt(get("type"));
+
+	int startofpoint  = 0;
+	int colMax=  scanCellFeatureArray.size();
+	for(i=0; i<colMax; ++i)
+	{
+		cellFeatureColumn  &gFeatureCol = scanCellFeatureArray[i];
+		int rowMax= gFeatureCol.size();
+		for(j=0;j< rowMax; ++j)
+		{
+		   cellFeature &gcellFreature =  gFeatureCol[j];
+		   startofpoint += gcellFreature.pCell->size();
+		   cell &gCell =*( gcellFreature.pCell);
+
+		   for( k=0; k< gcellFreature.pCell->size();++k)
+		   {
+			        // find Point in scan raw points by point_id;
+					Point p = *(gCell[k]);
+				   if(gcellFreature.cellType & CELL_TYPE_STATIC)
+					   Pt[p.point_id] = POINT_TYPE_STATIC_OBJECT;
+    			   if(gcellFreature.cellType & CELL_TYPE_MOVING)
+					   Pt[p.point_id] = POINT_TYPE_MOVING_OBJECT;
+    			   if(gcellFreature.cellType & CELL_TYPE_GROUND)
+					   Pt[p.point_id] = POINT_TYPE_GROUND;
+		   }
+
+		}
+	}
+}
+
+void VeloScan::FindingAllofObject(int maxDist, int minDist)
+{
+	TransferToCellArray(maxDist, minDist);
+	CalcScanCellFeature();
+	FindAndCalcScanClusterFeature();
+
+    return;
+ }
+
+
+void VeloScan::TrackingAllofObject(int trackingAlgo)
+{
+	trackMgr.HandleScan(*this,trackingAlgo);
+ }
+
+void VeloScan::ClassifiAllofObject()
+{
+	 ClassifiAllObject();
+ }
+
+void VeloScan::ExchangePointCloud()
+{
+     MarkStaticorMovingPointCloud();
+     return;
+ }

.svn/pristine/2d/2d3538d03392fb9bafc74ab0394be867d69fa0dd.svn-base

@@ -0,0 +1,47 @@
+/**
+  * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm
+  *
+  * Copyright (C) Jacobs University Bremen
+  *
+  * Released under the GPL version 3.
+  *
+  * @author Mihai-Cotizo Sima
+  */
+
+
+#include <segmentation/disjoint-set.h>
+
+universe::universe(int elements) {
+    elts = new uni_elt[elements];
+    num = elements;
+    for (int i = 0; i < elements; i++) {
+        elts[i].rank = 0;
+        elts[i].size = 1;
+        elts[i].p = i;
+    }
+}
+
+universe::~universe() {
+    delete [] elts;
+}
+
+int universe::find(int x) {
+    int y = x;
+    while (y != elts[y].p)
+        y = elts[y].p;
+    elts[x].p = y;
+    return y;
+}
+
+void universe::join(int x, int y) {
+    if (elts[x].rank > elts[y].rank) {
+        elts[y].p = x;
+        elts[x].size += elts[y].size;
+    } else {
+        elts[x].p = y;
+        elts[y].size += elts[x].size;
+        if (elts[x].rank == elts[y].rank)
+            elts[y].rank++;
+    }
+    num--;
+}

.svn/pristine/2d/2db59ca156a4091e122b11689b22d2a62022b330.svn-base

@@ -0,0 +1,64 @@
+This file describes the scanserver functionality, the code changes and its behaviour.
+
+To run with the scanserver functionality, pass -S or --scanserver to the slam6D/show call. Start the scanserver with bin/scanserver & first. If you want to directly jump to usage examples, see the "USAGE" section below.
+
+The scanserver is a new method to load and manage scans for 'slam6D', 'show' and some few other tools (so far). It removes all the IO code from the clients and handles it in the server process. This separation offers persistence of scan data and avoids unneccessary reloads of full scans or even reduced versions thereof. By using a caching framework it also transparently handles the available memory given and enables (nearly) endless amounts of data. The client is only required to open the interface, load a directory and start working on those scans without having to alter its workflow (e.g., pre-reduce them) to accomodate huge data volumes.
+
+If you have questions or problems (or both), contact Thomas Escher <tescher@uos.de>.
+
+
+
+USAGE:
+
+1. General
+
+Start the scanserver once (in another terminal, or in the same one as a background process):
+  bin/scanserver &
+  
+Do all the normal work as you would normally do, adding the parameter -S:
+  bin/slam6D dat -S
+  bin/show dat -S
+
+2. Changing the available memory size
+
+Changing the cache memory size used by scan data (about half the system memory usually works):
+
+  bin/scanserver -c 3500   (for 8GB RAM)
+
+If you intend to not reload the full scans for different reduction parameters or don't have too much memory/disk space, disable binary scan caching. Binary scan caching saves the full scans as long as the range or height parameters aren't touched, which would cause a full reload:
+
+  bin/scanserver -b 0
+
+If your dataset contains many scans and loops (e.g., 'hannover' with 468 scans), the default data memory (150M) won't be enough to hold all the animation frames and you need to increase it:
+
+  bin/scanserver -d 250
+
+3. Altering the shared memory on your linux system (bus_error)
+
+If you receive a bus_error, the size of your shared memory is too small and the requested allocation was too big. This is resolved via remounting your shm device. Default is half of the available RAM. This limit can be increased to nearly 90% of the RAM if required.
+
+  sudo mount -o remount,size=7000M /dev/shm   (for 8GB RAM)
+
+4. Locking the memory to avoid swapping (Linux only)
+
+If a great portion of the RAM is used for the cache data, swapping will usually occur after 50% of usage. To avoid this, the scanserver tries to lock the whole memory in place. This will fail without superuser rights, as well as on a full shared memory (see 3.) even with rights. Add these two additional lines to '/etc/security/limits.conf':
+
+  * 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
+
+The octtree serialization behaves slightly different than before. Since the scanserver caches octtrees between calls of 'show', the loading of octtrees only becomes relevant if no octtrees are in the cache and have to be created from the scan itself. If this has been done once before and the octtrees have been saved via --saveOct before this can be used to speed up the octtree loading with --loadOct.
+
+  bin/show dat --loadOct --saveOct
+
+If octtrees are not cached, they are deserialized if available, created otherwise and then saved for future calls.
+
+IMPLEMENTATION STATUS:
+
+Currently only 'slam6D' and 'show' are working with the scanserver.
+
+Since the scanserver handles the disk IO now and the filtering has been optimized, not all ScanIOs are updated yet. Just copy and paste and change the minor parts about reading the input lines.
+
+Working: 'ls src/scanio' in a shell

.svn/pristine/2f/2fe18e6c7cf19c30fdfb8ff8d953bf7138a65c31.svn-base

@@ -0,0 +1,71 @@
+/** @file 
+ *  @brief Representation of the optimized k-d tree. 
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher
+ */
+
+#ifndef __KD_MANAGED_H__
+#define __KD_MANAGED_H__
+
+#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 
+#define _OPENMP
+#endif
+#endif
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+#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. 
+ * 
+ * A kD tree for points, with limited
+ * capabilities (find nearest point to
+ * a given point, or to a ray).
+ **/
+class KDtreeManaged :
+    public SearchTree,
+    private KDTreeImpl<const DataXYZ&, unsigned int, ArrayAccessor>
+
+{
+public:
+  KDtreeManaged(Scan* scan);
+  virtual ~KDtreeManaged() {}
+  
+  virtual void lock();
+  virtual void unlock();
+
+  //! Aquires cached data first to pass on to the usual KDtree to process
+  virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
+private:
+  Scan* m_scan;
+  DataXYZ* m_data;
+
+  //! Mutex for safely reducing points just once in a multithreaded environment
+  boost::mutex m_mutex_locking;
+  volatile unsigned int m_count_locking;
+
+  // constructor initializer list hacks
+  unsigned int* m_temp_indices;
+  unsigned int* prepareTempIndices(unsigned int n);
+};
+
+#endif

.svn/pristine/45/459a8ac2a115f52554eef1c357fdebd3c79bf177.svn-base

@@ -0,0 +1,49 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+*/
+
+#ifndef SEGMENT_GRAPH
+#define SEGMENT_GRAPH
+
+#include <algorithm>
+#include <cmath>
+#include "disjoint-set.h"
+
+// threshold function
+#define THRESHOLD(size, c) (c/size)
+
+typedef struct {
+    float w;
+    int a, b;
+} edge;
+
+bool operator<(const edge &a, const edge &b);
+
+/*
+ * Segment a graph
+ *
+ * Returns a disjoint-set forest representing the segmentation.
+ *
+ * num_vertices: number of vertices in graph.
+ * num_edges: number of edges in graph
+ * edges: array of edges.
+ * c: constant for treshold function.
+ */
+universe *segment_graph(int num_vertices, int num_edges, edge *edges,
+                        float c);
+
+#endif

.svn/pristine/49/494f2fcff752c79b0981b59126fa3ed5bd01acf7.svn-base

@@ -0,0 +1,31 @@
+/**
+   @file
+   @brief Displaying of a matched 3D scene
+   @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+   @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)
+*/
+inline void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ)
+{
+  //  pZ *= -1.0;
+  glVertex3d(pX, pY, pZ);
+}
+
+
+/**
+   sets the OpenGL point,
+   (z axis is inverted in OpenGL)
+*/
+inline void setGLPoint(GLdouble* p) 
+{
+  GLdouble pZ = 1.0 * p[2];
+
+  glVertex3d(p[0], p[1], pZ);
+}
+#endif

.svn/pristine/65/6566191d235553599f77174b9192fa4cb776e8ca.svn-base

@@ -0,0 +1,787 @@
+/*
+ * scan implementation
+ *
+ * Copyright (C) Andreas Nuechter, Kai Lingemann, Dorit Borrmann, Jan Elseberg, Thomas Escher
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/scan.h"
+
+#include "slam6d/basicScan.h"
+#include "slam6d/managedScan.h"
+#include "slam6d/metaScan.h"
+#include "slam6d/searchTree.h"
+#include "slam6d/kd.h"
+#include "slam6d/Boctree.h"
+#include "slam6d/globals.icc"
+
+#ifdef WITH_METRICS
+#include "slam6d/metrics.h"
+#endif
+
+#ifdef _MSC_VER
+#define _NO_PARALLEL_READ
+#endif
+
+#ifdef __APPLE__
+#define _NO_PARALLEL_READ
+#endif
+
+using std::vector;
+
+
+vector<Scan*> Scan::allScans;
+bool Scan::scanserver = false;
+
+
+void Scan::openDirectory(bool scanserver, const std::string& path, IOType type,
+  int start, int end)
+{
+  Scan::scanserver = scanserver;
+  if(scanserver)
+    ManagedScan::openDirectory(path, type, start, end);
+  else
+    BasicScan::openDirectory(path, type, start, end);
+}
+
+void Scan::closeDirectory()
+{
+  if(scanserver)
+    ManagedScan::closeDirectory();
+  else
+    BasicScan::closeDirectory();
+}
+
+Scan::Scan()
+{
+  unsigned int i;
+
+  // pose and transformations
+  for(i = 0; i < 3; ++i) rPos[i] = 0;
+  for(i = 0; i < 3; ++i) rPosTheta[i] = 0;
+  for(i = 0; i < 4; ++i) rQuat[i] = 0;
+  M4identity(transMat);
+  M4identity(transMatOrg);
+  M4identity(dalignxf);
+
+  // trees and reduction methods
+  cuda_enabled = false;
+  nns_method = -1;
+  kd = 0;
+  ann_kd_tree = 0;
+
+  // reduction on-demand
+  reduction_voxelSize = 0.0;
+  reduction_nrpts = 0;
+  reduction_pointtype = PointType();
+
+  // flags
+  m_has_reduced = false;
+
+  // octtree
+  octtree_reduction_voxelSize = 0.0;
+  octtree_voxelSize = 0.0;
+  octtree_pointtype = PointType();
+  octtree_loadOct = false;
+  octtree_saveOct = false;
+}
+
+Scan::~Scan()
+{
+  if(kd) delete kd;
+}
+
+void Scan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype)
+{
+  reduction_voxelSize = voxelSize;
+  reduction_nrpts = nrpts;
+  reduction_pointtype = pointtype;
+}
+
+void Scan::setSearchTreeParameter(int nns_method, bool cuda_enabled)
+{
+  searchtree_nnstype = nns_method;
+  searchtree_cuda_enabled = cuda_enabled;
+}
+
+void Scan::setOcttreeParameter(double reduction_voxelSize, double voxelSize, PointType pointtype, bool loadOct, bool saveOct)
+{
+  octtree_reduction_voxelSize = reduction_voxelSize;
+  octtree_voxelSize = voxelSize;
+  octtree_pointtype = pointtype;
+  octtree_loadOct = loadOct;
+  octtree_saveOct = saveOct;
+}
+
+void Scan::clear(unsigned int types)
+{
+  if(types & DATA_XYZ) clear("xyz");
+  if(types & DATA_RGB) clear("rgb");
+  if(types & DATA_REFLECTANCE) clear("reflectance");
+  if(types & DATA_TEMPERATURE) clear("temperature");
+  if(types & DATA_AMPLITUDE) clear("amplitude");
+  if(types & DATA_TYPE) clear("type");
+  if(types & DATA_DEVIATION) clear("deviation");
+}
+
+SearchTree* Scan::getSearchTree()
+{
+  // if the search tree hasn't been created yet, calculate everything
+  if(kd == 0) {
+    createSearchTree();
+  }
+  return kd;
+}
+
+void Scan::toGlobal() {
+  calcReducedPoints();
+  transform(transMatOrg, INVALID);
+}
+
+/**
+ * Computes a search tree depending on the type.
+ */
+void Scan::createSearchTree()
+{
+  // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation
+  boost::lock_guard<boost::mutex> lock(m_mutex_create_tree);
+  if(kd != 0) return;
+
+  // make sure the original points are created before starting the measurement
+  DataXYZ xyz_orig(get("xyz reduced original"));
+
+#ifdef WITH_METRICS
+  Timer tc = ClientMetric::create_tree_time.start();
+#endif //WITH_METRICS
+
+  createSearchTreePrivate();
+  
+#ifdef WITH_METRICS
+  ClientMetric::create_tree_time.end(tc);
+#endif //WITH_METRICS
+}
+
+void Scan::calcReducedOnDemand()
+{
+  // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees count as zero) do the reduction
+  boost::lock_guard<boost::mutex> lock(m_mutex_reduction);
+  if(m_has_reduced) return;
+
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::on_demand_reduction_time.start();
+#endif //WITH_METRICS
+
+  calcReducedOnDemandPrivate();
+
+  m_has_reduced = true;
+
+#ifdef WITH_METRICS
+  ClientMetric::on_demand_reduction_time.end(t);
+#endif //WITH_METRICS
+}
+
+void Scan::copyReducedToOriginal()
+{
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::copy_original_time.start();
+#endif //WITH_METRICS
+
+  DataXYZ xyz_r(get("xyz reduced"));
+  unsigned int size = xyz_r.size();
+  DataXYZ xyz_r_orig(create("xyz reduced original", sizeof(double)*3*size));
+  for(unsigned int i = 0; i < size; ++i) {
+    for(unsigned int j = 0; j < 3; ++j) {
+      xyz_r_orig[i][j] = xyz_r[i][j];
+    }
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::copy_original_time.end(t);
+#endif //WITH_METRICS
+}
+
+void Scan::copyOriginalToReduced()
+{
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::copy_original_time.start();
+#endif //WITH_METRICS
+
+  DataXYZ xyz_r_orig(get("xyz reduced original"));
+  unsigned int size = xyz_r_orig.size();
+  DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size));
+  for(unsigned int i = 0; i < size; ++i) {
+    for(unsigned int j = 0; j < 3; ++j) {
+      xyz_r[i][j] = xyz_r_orig[i][j];
+    }
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::copy_original_time.end(t);
+#endif //WITH_METRICS
+}
+
+
+
+/**
+ * Computes an octtree of the current scan, then getting the
+ * reduced points as the centers of the octree voxels.
+ */
+void Scan::calcReducedPoints()
+{
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::scan_load_time.start();
+#endif //WITH_METRICS
+
+  // get xyz to start the scan load, separated here for time measurement
+  DataXYZ xyz(get("xyz"));
+
+  // if the scan hasn't been loaded we can't calculate anything
+  if(xyz.size() == 0)
+    throw runtime_error("Could not calculate reduced points, XYZ data is empty");
+
+#ifdef WITH_METRICS
+  ClientMetric::scan_load_time.end(t);
+  Timer tl = ClientMetric::calc_reduced_points_time.start();
+#endif //WITH_METRICS
+
+  // no reduction needed
+  // copy vector of points to array of points to avoid
+  // further copying
+  if(reduction_voxelSize <= 0.0) {
+    // copy the points
+    DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*xyz.size()));
+    for(unsigned int i = 0; i < xyz.size(); ++i) {
+      for(unsigned int j = 0; j < 3; ++j) {
+        xyz_r[i][j] = xyz[i][j];
+      }
+    }
+  } else {
+    // start reduction
+
+    // build octree-tree from CurrentScan
+    // put full data into the octtree
+    BOctTree<double> *oct = new BOctTree<double>(PointerArray<double>(xyz).get(),
+      xyz.size(), reduction_voxelSize, reduction_pointtype);
+
+    vector<double*> center;
+    center.clear();
+
+    if (reduction_nrpts > 0) {
+      if (reduction_nrpts == 1) {
+        oct->GetOctTreeRandom(center);
+      } else {
+        oct->GetOctTreeRandom(center, reduction_nrpts);
+      }
+    } else {
+      oct->GetOctTreeCenter(center);
+    }
+
+    // storing it as reduced scan
+    unsigned int size = center.size();
+    DataXYZ xyz_r(create("xyz reduced", sizeof(double)*3*size));
+    for(unsigned int i = 0; i < size; ++i) {
+      for(unsigned int j = 0; j < 3; ++j) {
+        xyz_r[i][j] = center[i][j];
+      }
+    }
+
+    delete oct;
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::calc_reduced_points_time.end(tl);
+#endif //WITH_METRICS
+}
+
+/**
+ * Merges the scan's intrinsic coordinates with the robot position.
+ * @param prevScan The scan that's transformation is extrapolated,
+ * i.e., odometry extrapolation
+ *
+ * For additional information see the following paper (jfr2007.pdf):
+ *
+ * Andreas Nüchter, Kai Lingemann, Joachim Hertzberg, and Hartmut Surmann,
+ * 6D SLAM - 3D Mapping Outdoor Environments Journal of Field Robotics (JFR),
+ * Special Issue on Quantitative Performance Evaluation of Robotic and Intelligent
+ * Systems, Wiley & Son, ISSN 1556-4959, Volume 24, Issue 8-9, pages 699 - 722,
+ * August/September, 2007
+ *
+ */
+void Scan::mergeCoordinatesWithRoboterPosition(Scan* prevScan)
+{
+  double tempMat[16], deltaMat[16];
+  M4inv(prevScan->get_transMatOrg(), tempMat);
+  MMult(prevScan->get_transMat(), tempMat, deltaMat);
+  transform(deltaMat, INVALID); //apply delta transformation of the previous scan
+}
+
+/**
+ * The method transforms all points with the given transformation matrix.
+ */
+void Scan::transformAll(const double alignxf[16])
+{
+  DataXYZ xyz(get("xyz"));
+  unsigned int i=0 ;
+//  #pragma omp parallel for
+  for(; i < xyz.size(); ++i) {
+    transform3(alignxf, xyz[i]);
+  }
+  // TODO: test for ManagedScan compability, may need a touch("xyz") to mark saving the new values
+}
+
+//! Internal function of transform which alters the reduced points
+void Scan::transformReduced(const double alignxf[16])
+{
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::transform_time.start();
+#endif //WITH_METRICS
+
+  DataXYZ xyz_r(get("xyz reduced"));
+  unsigned int i=0;
+ // #pragma omp parallel for
+  for( ; i < xyz_r.size(); ++i) {
+    transform3(alignxf, xyz_r[i]);
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::transform_time.end(t);
+#endif //WITH_METRICS
+}
+
+//! Internal function of transform which handles the matrices
+void Scan::transformMatrix(const double alignxf[16])
+{
+  double tempxf[16];
+
+  // apply alignxf to transMat and update pose vectors
+  MMult(alignxf, transMat, tempxf);
+  memcpy(transMat, tempxf, sizeof(transMat));
+  Matrix4ToEuler(transMat, rPosTheta, rPos);
+  Matrix4ToQuat(transMat, rQuat);
+
+#ifdef DEBUG
+  cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", "
+	  << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl;
+
+  cerr << transMat << endl;
+#endif
+
+  // apply alignxf to dalignxf
+  MMult(alignxf, dalignxf, tempxf);
+  memcpy(dalignxf, tempxf, sizeof(transMat));
+}
+
+/**
+ * Transforms the scan by a given transformation and writes a new frame. The idea
+ * is to write for every transformation in all files, such that the show program
+ * is able to determine, whcih scans have to be drawn in which color. Hidden scans
+ * (or later processed scans) are written with INVALID.
+ *
+ * @param alignxf Transformation matrix
+ * @param colour Specifies which colour should the written to the frames file
+ * @param islum Is the transformtion part of LUM, i.e., all scans are transformed?
+ *        In this case only LUM transformation is stored, otherwise all scans are processed
+ *        -1  no transformation is stored
+ *         0  ICP transformation
+ *         1  LUM transformation, all scans except last scan
+ *         2  LUM transformation, last scan only
+ */
+void Scan::transform(const double alignxf[16], const AlgoType type, int islum)
+{
+  MetaScan* meta = dynamic_cast<MetaScan*>(this);
+  
+  if(meta) {
+    for(unsigned int i = 0; i < meta->size(); ++i) {
+      meta->getScan(i)->transform(alignxf, type, -1);
+    }
+  }
+
+#ifdef TRANSFORM_ALL_POINTS
+  transformAll(alignxf);
+#endif //TRANSFORM_ALL_POINTS
+
+#ifdef DEBUG
+  cerr << alignxf << endl;
+  cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", "
+	  << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> ";
+#endif
+
+  // transform points
+  transformReduced(alignxf);
+
+  // update matrices
+  transformMatrix(alignxf);
+
+  // store transformation in frames
+  if(type != INVALID) {
+#ifdef WITH_METRICS
+    Timer t = ClientMetric::add_frames_time.start();
+#endif //WITH_METRICS
+    bool in_meta;
+    MetaScan* meta = dynamic_cast<MetaScan*>(this);
+    int found = 0;
+    unsigned int scans_size = allScans.size();
+
+    switch (islum) {
+    case -1:
+      // write no tranformation
+      break;
+    case 0:
+      for(unsigned int i = 0; i < scans_size; ++i) {
+        Scan* scan = allScans[i];
+        in_meta = false;
+        if(meta) {
+          for(unsigned int j = 0; j < meta->size(); ++j) {
+            if(meta->getScan(j) == scan) {
+              found = i;
+              in_meta = true;
+            }
+          }
+        }
+
+        if(scan == this || in_meta) {
+          found = i;
+          scan->addFrame(type);
+        } else {
+          if(found == 0) {
+            scan->addFrame(ICPINACTIVE);
+          } else {
+            scan->addFrame(INVALID);
+          }
+        }
+      }
+      break;
+    case 1:
+      addFrame(type);
+      break;
+    case 2:
+      for(unsigned int i = 0; i < scans_size; ++i) {
+        Scan* scan = allScans[i];
+        if(scan == this) {
+          found = i;
+          addFrame(type);
+          allScans[0]->addFrame(type);
+          continue;
+        }
+        if (found != 0) {
+          scan->addFrame(INVALID);
+        }
+      }
+      break;
+    default:
+      cerr << "invalid point transformation mode" << endl;
+    }
+
+#ifdef WITH_METRICS
+    ClientMetric::add_frames_time.end(t);
+#endif //WITH_METRICS
+  }
+}
+
+/**
+ * Transforms the scan by a given transformation and writes a new frame. The idea
+ * is to write for every transformation in all files, such that the show program
+ * is able to determine, whcih scans have to be drawn in which color. Hidden scans
+ * (or later processed scans) are written with INVALID.
+ *
+ * @param alignQuat Quaternion for the rotation
+ * @param alignt    Translation vector
+ * @param colour Specifies which colour should the written to the frames file
+ * @param islum Is the transformtion part of LUM, i.e., all scans are transformed?
+ *        In this case only LUM transformation is stored, otherwise all scans are processed
+ *        -1  no transformation is stored
+ *         0  ICP transformation
+ *         1  LUM transformation, all scans except last scan
+ *         2  LUM transformation, last scan only
+ */
+void Scan::transform(const double alignQuat[4], const double alignt[3],
+				 const AlgoType type, int islum)
+{
+  double alignxf[16];
+  QuatToMatrix4(alignQuat, alignt, alignxf);
+  transform(alignxf, type, islum);
+}
+
+/**
+ * Transforms the scan, so that the given Matrix
+ * prepresent the next pose.
+ *
+ * @param alignxf Transformation matrix to which this scan will be set to
+ * @param islum Is the transformation part of LUM?
+ */
+void Scan::transformToMatrix(double alignxf[16], const AlgoType type, int islum)
+{
+  double tinv[16];
+  M4inv(transMat, tinv);
+  transform(tinv, INVALID);
+  transform(alignxf, type, islum);
+}
+
+/**
+ * Transforms the scan, so that the given Euler angles
+ * prepresent the next pose.
+ *
+ * @param rP Translation to which this scan will be set to
+ * @param rPT Orientation as Euler angle to which this scan will be set
+ * @param islum Is the transformation part of LUM?
+ */
+void Scan::transformToEuler(double rP[3], double rPT[3], const AlgoType type, int islum)
+{
+#ifdef WITH_METRICS
+  // called in openmp context in lum6Deuler.cc:422
+  ClientMetric::transform_time.set_threadsafety(true);
+  ClientMetric::add_frames_time.set_threadsafety(true);
+#endif //WITH_METRICS
+
+  double tinv[16];
+  double alignxf[16];
+  M4inv(transMat, tinv);
+  transform(tinv, INVALID);
+  EulerToMatrix4(rP, rPT, alignxf);
+  transform(alignxf, type, islum);
+
+#ifdef WITH_METRICS
+  ClientMetric::transform_time.set_threadsafety(false);
+  ClientMetric::add_frames_time.set_threadsafety(false);
+#endif //WITH_METRICS
+}
+
+/**
+ * Transforms the scan, so that the given Euler angles
+ * prepresent the next pose.
+ *
+ * @param rP Translation to which this scan will be set to
+ * @param rPQ Orientation as Quaternion to which this scan will be set
+ * @param islum Is the transformation part of LUM?
+ */
+void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int islum)
+{
+  double tinv[16];
+  double alignxf[16];
+  M4inv(transMat, tinv);
+  transform(tinv, INVALID);
+  QuatToMatrix4(rPQ, rP, alignxf);
+  transform(alignxf, type, islum);
+}
+
+/**
+ * Calculates Source\Target
+ * Calculates a set of corresponding point pairs and returns them. It
+ * computes the k-d trees and deletes them after the pairs have been
+ * found. This slow function should be used only for testing
+ *
+ * @param pairs The resulting point pairs (vector will be filled)
+ * @param Target The scan to whiche the points are matched
+ * @param thread_num number of the thread (for parallelization)
+ * @param rnd randomized point selection
+ * @param max_dist_match2 maximal allowed distance for matching
+ */
+
+void Scan::getNoPairsSimple(vector <double*> &diff,
+  Scan* Source, Scan* Target,
+  int thread_num,
+  double max_dist_match2)
+{
+  DataXYZ xyz_r(Source->get("xyz reduced"));
+  KDtree* kd = new KDtree(PointerArray<double>(Target->get("xyz reduced")).get(), Target->size<DataXYZ>("xyz reduced"));
+
+  cout << "Max: " << max_dist_match2 << endl;
+  for (unsigned int i = 0; i < xyz_r.size(); i++) {
+
+    double p[3];
+    p[0] = xyz_r[i][0];
+    p[1] = xyz_r[i][1];
+    p[2] = xyz_r[i][2];
+
+
+    double *closest = kd->FindClosest(p, max_dist_match2, thread_num);
+    if (!closest) {
+	    diff.push_back(xyz_r[i]);
+	    //diff.push_back(closest);
+    }
+  }
+
+  delete kd;
+}
+
+/**
+ * Calculates a set of corresponding point pairs and returns them. It
+ * computes the k-d trees and deletes them after the pairs have been
+ * found. This slow function should be used only for testing
+ *
+ * @param pairs The resulting point pairs (vector will be filled)
+ * @param Source The scan whose points are matched to Targets' points
+ * @param Target The scan to whiche the points are matched
+ * @param thread_num number of the thread (for parallelization)
+ * @param rnd randomized point selection
+ * @param max_dist_match2 maximal allowed distance for matching
+ */
+void Scan::getPtPairsSimple(vector <PtPair> *pairs,
+  Scan* Source, Scan* Target,
+  int thread_num,
+  int rnd, double max_dist_match2,
+  double *centroid_m, double *centroid_d)
+{
+  KDtree* kd = new KDtree(PointerArray<double>(Source->get("xyz reduced")).get(), Source->size<DataXYZ>("xyz reduced"));
+  DataXYZ xyz_r(Target->get("xyz reduced"));
+
+  for (unsigned int i = 0; i < xyz_r.size(); i++) {
+    if (rnd > 1 && rand(rnd) != 0) continue;  // take about 1/rnd-th of the numbers only
+
+    double p[3];
+    p[0] = xyz_r[i][0];
+    p[1] = xyz_r[i][1];
+    p[2] = xyz_r[i][2];
+
+    double *closest = kd->FindClosest(p, max_dist_match2, thread_num);
+    if (closest) {
+      centroid_m[0] += closest[0];
+      centroid_m[1] += closest[1];
+      centroid_m[2] += closest[2];
+      centroid_d[0] += p[0];
+      centroid_d[1] += p[1];
+      centroid_d[2] += p[2];
+      PtPair myPair(closest, p);
+      pairs->push_back(myPair);
+    }
+  }
+  centroid_m[0] /= pairs[thread_num].size();
+  centroid_m[1] /= pairs[thread_num].size();
+  centroid_m[2] /= pairs[thread_num].size();
+  centroid_d[0] /= pairs[thread_num].size();
+  centroid_d[1] /= pairs[thread_num].size();
+  centroid_d[2] /= pairs[thread_num].size();
+
+  delete kd;
+}
+
+
+/**
+ * Calculates a set of corresponding point pairs and returns them.
+ * The function uses the k-d trees stored the the scan class, thus
+ * the function createTrees and deletTrees have to be called before
+ * resp. afterwards.
+ * Here we implement the so called "fast corresponding points"; k-d
+ * trees are not recomputed, instead the apply the inverse transformation
+ * to to the given point set.
+ *
+ * @param pairs The resulting point pairs (vector will be filled)
+ * @param Source The scan whose points are matched to Targets' points
+ * @param Target The scan to whiche the points are matched
+ * @param thread_num number of the thread (for parallelization)
+ * @param rnd randomized point selection
+ * @param max_dist_match2 maximal allowed distance for matching
+ * @return a set of corresponding point pairs
+ */
+void Scan::getPtPairs(vector <PtPair> *pairs,
+  Scan* Source, Scan* Target,
+  int thread_num,
+  int rnd, double max_dist_match2, double &sum,
+  double *centroid_m, double *centroid_d)
+{
+  // initialize centroids
+  for(unsigned int i = 0; i < 3; ++i) {
+    centroid_m[i] = 0;
+    centroid_d[i] = 0;
+  }
+
+  // get point pairs
+  DataXYZ xyz_r(Target->get("xyz reduced"));
+  Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf,
+      xyz_r, 0, xyz_r.size(),
+      thread_num,
+      rnd, max_dist_match2, sum, centroid_m, centroid_d);
+
+  // normalize centroids
+  unsigned int size = pairs->size();
+  if(size != 0) {
+    for(unsigned int i = 0; i < 3; ++i) {
+      centroid_m[i] /= size;
+      centroid_d[i] /= size;
+    }
+  }
+}
+
+
+/**
+ * Calculates a set of corresponding point pairs and returns them.
+ * The function uses the k-d trees stored the the scan class, thus
+ * the function createTrees and delteTrees have to be called before
+ * resp. afterwards.
+ *
+ * @param pairs The resulting point pairs (vector will be filled)
+ * @param Source The scan whose points are matched to Targets' points
+ * @param Target The scan to whiche the points are matched
+ * @param thread_num The number of the thread that is computing ptPairs in parallel
+ * @param step The number of steps for parallelization
+ * @param rnd randomized point selection
+ * @param max_dist_match2 maximal allowed distance for matching
+ * @param sum The sum of distances of the points
+ *
+ * These intermediate values are for the parallel ICP algorithm
+ * introduced in the paper
+ * "The Parallel Iterative Closest Point Algorithm"
+ *  by Langis / Greenspan / Godin, IEEE 3DIM 2001
+ *
+ */
+void Scan::getPtPairsParallel(vector <PtPair> *pairs, Scan* Source, Scan* Target,
+  int thread_num, int step,
+  int rnd, double max_dist_match2,
+  double *sum,
+  double centroid_m[OPENMP_NUM_THREADS][3], double centroid_d[OPENMP_NUM_THREADS][3])
+{
+  // initialize centroids
+  for(unsigned int i = 0; i < 3; ++i) {
+    centroid_m[thread_num][i] = 0;
+    centroid_d[thread_num][i] = 0;
+  }
+
+  // get point pairs
+  SearchTree* search = Source->getSearchTree();
+  // differentiate between a meta scan (which has no reduced points) and a normal scan
+  // if Source is also a meta scan it already has a special meta-kd-tree
+  MetaScan* meta = dynamic_cast<MetaScan*>(Target);
+  if(meta) {
+    for(unsigned int i = 0; i < meta->size(); ++i) {
+      // determine step for each scan individually
+      DataXYZ xyz_r(meta->getScan(i)->get("xyz reduced"));
+      unsigned int max = xyz_r.size();
+      unsigned int step = max / OPENMP_NUM_THREADS;
+      // call ptpairs for each scan and accumulate ptpairs, centroids and sum
+      search->getPtPairs(&pairs[thread_num], Source->dalignxf,
+        xyz_r, step * thread_num, step * thread_num + step,
+        thread_num,
+        rnd, max_dist_match2, sum[thread_num],
+        centroid_m[thread_num], centroid_d[thread_num]);
+    }
+  } else {
+    DataXYZ xyz_r(Target->get("xyz reduced"));
+    search->getPtPairs(&pairs[thread_num], Source->dalignxf,
+      xyz_r, thread_num * step, thread_num * step + step,
+      thread_num,
+      rnd, max_dist_match2, sum[thread_num],
+      centroid_m[thread_num], centroid_d[thread_num]);
+  }
+
+  // normalize centroids
+  unsigned int size = pairs[thread_num].size();
+  if(size != 0) {
+    for(unsigned int i = 0; i < 3; ++i) {
+      centroid_m[thread_num][i] /= size;
+      centroid_d[thread_num][i] /= size;
+    }
+  }
+}
+
+unsigned int Scan::getMaxCountReduced(ScanVector& scans)
+{
+  unsigned int max = 0;
+  for(std::vector<Scan*>::iterator it = scans.begin(); it != scans.end(); ++it) {
+    unsigned int count = (*it)->size<DataXYZ>("xyz reduced");
+    if(count > max)
+      max = count;
+  }
+  return max;
+}

.svn/pristine/68/686a5a70ccec35fc247ac32bf98d72287b251237.svn-base

@@ -0,0 +1,41 @@
+Project admins
+
+Andreas Nuechter          andreas@nuechti.de
+Kai Lingemann             kai.lingemann@gmx.de
+Dorit Borrmann            d.borrmann@jacobs-university.de
+
+List of contributors
+
+Andreas Nuechter          andreas@nuechti.de
+Kai Lingemann             kai.lingemann@gmx.de
+Dorit Borrmann            d.borrmann@jacobs-university.de
+Jan Elseberg              j.elseberg@jacobs-university.de
+Jochen Sprickerhof        jochen@sprickerhof.de
+HamidReza Houshiar        h.houshiar@jacobs-university.de
+Sven Albrecht             sven.albrecht@uni-osnabrueck.de
+Stan Serebryakov          cfr.ssv@gmail.com
+Thomas Escher             tescher@uni-osnabrueck.de
+Thomas Wiemann            twiemann@uni-osnabrueck.de
+Alexandru Tandrau         alexandru@tandrau.com
+Alexandru Eugen Ichim     eugen@alexichim.com
+Flavia Grosan             me@flaviagrosan.com
+Deyuan Qiu                deyuan.qiu@googlemail.com
+Darko Makreshanski        d.makreshanski@jacobs-university.de
+Mohammad Faisal Abdullah  m.faisal@jacobs-university.de
+Li Ming                   liming751218@whu.edu.cn
+Li Wei                    xpaulee@gmail.com
+Shams Feyzabadi           sh.feyzabadi@gmail.co
+Vladislav Perelmann		    v.perelman@jacobs-university.de
+Chen Long                 lchen.whu@gmail.com
+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
+
+Uwe Hebbelmann, Sebastian Stock, Andre Schemschat
+Hartmut Surmann
+Amuz Tamrakars, Ulugbek Makhmudov
+Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz

.svn/pristine/71/7159b4092ea0c92cd637e4b0cec8a72f7c0089ad.svn-base

@@ -0,0 +1,50 @@
+/**
+  * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm
+  *
+  * Copyright (C) Jacobs University Bremen
+  *
+  * Released under the GPL version 3.
+  *
+  * @author Mihai-Cotizo Sima
+  */
+
+#include <segmentation/segment-graph.h>
+
+bool operator<(const edge &a, const edge &b) {
+    return a.w < b.w;
+}
+
+universe *segment_graph(int num_vertices, int num_edges, edge *edges,
+                        float c) {
+    // sort edges by weight
+    std::sort(edges, edges + num_edges);
+
+    // make a disjoint-set forest
+    universe *u = new universe(num_vertices);
+
+    // init thresholds
+    float *threshold = new float[num_vertices];
+    for (int i = 0; i < num_vertices; i++)
+        threshold[i] = THRESHOLD(1,c);
+
+    // for each edge, in non-decreasing weight order...
+    for (int i = 0; i < num_edges; i++) {
+        edge *pedge = &edges[i];
+
+        // components conected by this edge
+        int a = u->find(pedge->a);
+        int b = u->find(pedge->b);
+        if (a != b) {
+            if ((pedge->w <= threshold[a]) &&
+                    (pedge->w <= threshold[b])) {
+                u->join(a, b);
+                a = u->find(a);
+                threshold[a] = pedge->w + THRESHOLD(u->size(a), c);
+            }
+        }
+    }
+
+    // free up
+    delete threshold;
+    return u;
+}

.svn/pristine/7b/7bffcb83acee8c5d40e56b49cbfbb32967b24045.svn-base

@@ -0,0 +1,124 @@
+/**
+ * @file
+ * @brief Inline helper functions for show program
+ * @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"
+
+/**
+ * converts a quaterion to a 4x4 matrix
+ * in OpenGL style
+ */
+template <class T>
+inline void QuaternionToMatrix4(const double *quat, T *mat)
+{
+  double xy = quat[0] * quat[1] * -1.0;
+  double xz = quat[0] * quat[2];
+  double yz = quat[1] * quat[2] * -1.0;
+  double wx = quat[3] * quat[0];
+  double wy = quat[3] * quat[1] * -1.0;
+  double wz = quat[3] * quat[2];
+  double x2 = sqr(quat[0]);
+  double y2 = sqr(quat[1]);
+  double z2 = sqr(quat[2]);
+
+  mat[0]  = 1.0 - 2.0 * (y2 + z2);
+  mat[4]  =       2.0 * (xy - wz);
+  mat[8]  =       2.0 * (xz + wy);
+
+  mat[1]  =       2.0 * (xy + wz);
+  mat[5]  = 1.0 - 2.0 * (x2 + z2);
+  mat[9]  =       2.0 * (yz - wx);
+
+  mat[2]  =       2.0 * (xz - wy);
+  mat[6]  =       2.0 * (yz + wx);
+  mat[10] = 1.0 - 2.0 * (x2 + y2);
+
+  mat[11] = mat[7] = mat[3] = mat[12] = mat[13] = mat[14] = 0.0;
+  mat[15] = 1.0;
+}
+
+/**
+ * converts a 4x4 matrix to a quaternion
+ */
+template <class T>
+inline void Matrix4ToQuaternion(const T *mat, double *quat)
+{
+  double S;
+  double Trace = 1 + mat[0] + mat[5] + mat[10];
+  if ( Trace > COMPARE_EPSILON ) {
+    S = sqrt(Trace) * 2;
+    quat[0] = -1.0 * ( mat[9] - mat[6] ) / S;       // q_x
+    quat[1] = -1.0 * ( mat[2] - mat[8] ) / S;       // q_y
+    quat[2] = -1.0 * ( mat[4] - mat[1] ) / S;       // q_z
+    quat[3] = 0.25 * S;                             // q_0
+  } else if ( mat[0] > mat[5] && mat[0] > mat[10] )  {	// Column 0: 
+    S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
+    quat[0] = -1.0 * 0.25 * S;
+    quat[1] = -1.0 * (mat[4] + mat[1] ) / S;
+    quat[2] = -1.0 * (mat[2] + mat[8] ) / S;
+    quat[3] = (mat[9] - mat[6] ) / S; 
+  } else if ( mat[5] > mat[10] ) {		          	// Column 1: 
+    S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
+    quat[0] = -1.0 * (mat[4] + mat[1] ) / S;
+    quat[1] = -1.0 * 0.25 * S;
+    quat[2] = -1.0 * (mat[9] + mat[6] ) / S;
+    quat[3] = (mat[2] - mat[8] ) / S;
+  } else {					               	// Column 2:
+    S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
+    quat[0] = -1.0 * (mat[2] + mat[8] ) / S;
+    quat[1] = -1.0 * (mat[9] + mat[6] ) / S;
+    quat[2] = -1.0 * 0.25 * S;
+    quat[3] = (mat[4] - mat[1] ) / S;
+  }
+}
+
+/**
+ * normalizes a quaternion to gain a unit quaternion
+ */
+template <class T>
+inline void QuatNormalize(T *q)
+{
+  T norm = sqrt(sqr(q[0]) + sqr(q[1]) + sqr(q[2]) + sqr(q[3]));
+  q[3] = q[3] / norm;
+  q[2] = q[2] / norm;
+  q[1] = q[1] / norm;
+  q[0] = q[0] / norm;
+}
+
+/**
+ * multiplication of quaternions
+ */
+template <class T>
+inline void QuatMult(const T *q, const T *q_new, T *qtemp)
+{
+  qtemp[3] = q_new[3] * q[3]  - q_new[0] * q[0]   - q_new[1] * q[1]   - q_new[2] * q[2];
+  qtemp[0] = q_new[3] * q[0]  + q_new[0] * q[3]   + q_new[1] * q[2]   - q_new[2] * q[1];
+  qtemp[1] = q_new[3] * q[1]  + q_new[1] * q[3]   + q_new[2] * q[0]   - q_new[0] * q[2];
+  qtemp[2] = q_new[3] * q[2]  + q_new[2] * q[3]   + q_new[0] * q[1]   - q_new[1] * q[0];
+}
+
+/**
+ * converts a quaternion to an axis angle
+ */
+inline void QuaternionToAxisAngle(const double *quat, double *axis, double &angle)
+{
+  double quaternion_norm = sqrt( sqr(quat[0]) + sqr(quat[1]) + sqr(quat[2]) + sqr(quat[3]) );
+  double normalized_quat[4];
+  normalized_quat[0] = quat[0] / quaternion_norm;
+  normalized_quat[1] = quat[1] / quaternion_norm;
+  normalized_quat[2] = quat[2] / quaternion_norm;
+  normalized_quat[3] = quat[3] / quaternion_norm;
+  double cos_a = normalized_quat[3];
+  angle = deg(acos( cos_a )) * 2.0;
+  double sin_a = sqrt( 1.0 - cos_a * cos_a );
+  if ( fabs( sin_a ) < DIV_EPSILON ) sin_a = 1;
+  axis[0] =  normalized_quat[0] / sin_a;
+  axis[1] =  normalized_quat[1] / sin_a;
+  axis[2] = -1.0 * normalized_quat[2] / sin_a;
+}
+#endif

.svn/pristine/80/80e4f43c46b62200ace933c4bc8098cd8b771da6.svn-base

@@ -0,0 +1,54 @@
+#ifndef __RANSAC_H__
+#define __RANSAC_H__
+
+#include "shape.h"
+#include "slam6d/scan.h"
+#include "shapes/ransac_Boctree.h"
+
+// TODO implement some parameters to modify ransac (maybe in CollisionShape?)
+template <class T>
+void Ransac(CollisionShape<T> &shape, Scan *scan, vector<T*> *best_points = 0) {
+  int best_score = 0;
+  CollisionShape<T> *best = shape.copy();
+
+  // stores 3 sample points    
+  vector<T *> ps;
+  // 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();
+    // randomly select points from the octree
+    oct->DrawPoints(ps, shape.getNrPoints());
+    // compute shape parameters from points
+    if ( shape.hypothesize(ps) ) {
+      // count number of points on the shape
+      int r = oct->PointsOnShape(shape);
+      if (r > best_score) {
+        if (best) delete best;
+        // remember this best fitted shape
+        best_score = r;
+        best = shape.copy();
+      }
+    }
+  }
+  cout << "5000 iterations done" << endl;
+  if (best_points) {
+    best_points->clear();
+    oct->PointsOnShape(*best, *best_points);
+    cout << "Nr points before refinement " << best_points->size() << endl;
+    best->refine(best_points);
+    best_points->clear();
+    oct->PointsOnShape(*best, *best_points);
+    cout << "Nr points after refinement " << best_points->size() << endl;
+  }
+  shape = *best;
+  delete best;
+
+  delete oct;
+
+
+}
+
+#endif

.svn/pristine/80/80fa4b2f16331352477b08809318d265d9999eff.svn-base

@@ -0,0 +1,48 @@
+/*
+Copyright (C) 2006 Pedro Felzenszwalb
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+*/
+
+#ifndef DISJOINT_SET
+#define DISJOINT_SET
+
+// disjoint-set forests using union-by-rank and path compression (sort of).
+
+typedef struct {
+    int rank;
+    int p;
+    int size;
+} uni_elt;
+
+class universe {
+public:
+    universe(int elements);
+    ~universe();
+    int find(int x);
+    void join(int x, int y);
+    int size(int x) const {
+        return elts[x].size;
+    }
+    int num_sets() const {
+        return num;
+    }
+
+private:
+    uni_elt *elts;
+    int num;
+};
+
+#endif

.svn/pristine/89/893c94fbc7bdd37fa6d7865fcd5f04794fec237a.svn-base

@@ -0,0 +1,249 @@
+/**
+ * @file 
+ * @brief Efficient representation of an octree for ransac
+ * @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany. 
+ */
+
+#ifndef RANSAC_OCTREE_H
+#define RANSAC_OCTREE_H
+
+#include <stdio.h>
+
+#include <vector>
+using std::vector;
+#include <deque>
+using std::deque;
+#include <set>
+using std::set;
+#include <list>
+using std::list;
+#include <iostream>
+#include <fstream>
+#include <string>
+
+#include "shapes/geom_math.h"
+
+#include "slam6d/globals.icc"
+#include "slam6d/Boctree.h"
+#include "shape.h"
+
+/**
+ * @brief Octree
+ * 
+ * A cubic bounding box is calculated
+ * from the given 3D points. Then it
+ * is recusivly subdivided into smaller
+ * subboxes
+ */
+template <class T>
+class RansacOctTree : public BOctTree<T> {
+
+public:
+  
+  template <class P>
+  RansacOctTree(P* const* pts, int n, T _voxelSize, PointType _pointtype = PointType() ) : BOctTree<T>(pts, n, _voxelSize, _pointtype) {}
+
+  //  RansacOctTree(vector<const T *> &pts, T voxelSize, PointType _pointtype = PointType() ) : BOctTree<T>(pts, voxelSize, _pointtype) {}
+
+  RansacOctTree(std::string filename) : BOctTree<T> (filename) {}
+
+  void DrawPoints(vector<T *> &p, unsigned char nrp) {
+    DrawPoints(p, *BOctTree<T>::root, nrp);
+  }
+ 
+
+  unsigned long PointsOnShape(CollisionShape<T> &shape) {
+    return PointsOnShape(*BOctTree<T>::root, BOctTree<T>::center, BOctTree<T>::size, shape);
+  }
+
+  void PointsOnShape(CollisionShape<T> &shape, vector<T *> &points) {
+    PointsOnShape(*BOctTree<T>::root, BOctTree<T>::center, BOctTree<T>::size, shape, points);
+//    exit(0);
+  }
+
+protected:
+void showbits(char a)
+{
+  int i  , k , mask;
+
+  for( i =7 ; i >= 0 ; i--)
+  {
+     mask = 1 << i;
+     k = a & mask;
+     if( k == 0)
+        cout<<"0 ";
+     else
+        cout<<"1 ";
+  }
+}
+
+  long PointsOnShape(bitoct &node, T *center, T size, CollisionShape<T> &shape ) {
+    if (! shape.isInCube(center[0], center[1], center[2], size)) {
+      return 0;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+    
+  /*  
+    printf("parent %p   children: %p \n", &node, children);
+    cout << "  ";
+    showbits(node.valid);
+    cout << endl;
+    cout << "  ";
+    showbits(node.leaf);
+    cout << endl;
+*/
+    long result = 0;
+
+//    int r = 0;
+
+    for (unsigned char i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+//    printf("i: %u r: %d  parent %p   child[r]: %p \n", i, r, &node, children);
+//    r++;
+
+        childcenter(center, ccenter, size, i);  // childrens center
+        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->getPointreps();
+            unsigned int length = points[0].length;
+            T *point = &(points[1].v);  // first point
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+              if ( shape.containsPoint(point) )
+                result++;
+              point+= BOctTree<T>::POINTDIM;
+            }
+          }
+        } else { // recurse
+          result += PointsOnShape( children->node, ccenter, size/2.0, shape);
+        }
+        ++children; // next child
+        //r++;
+      }
+    }
+
+    return result;
+  }
+  
+  void PointsOnShape(bitoct &node, T *center, T size, CollisionShape<T> &shape, vector<T*> &vpoints) {
+    if (! shape.isInCube(center[0], center[1], center[2], size)) {
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (unsigned char i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        childcenter(center, ccenter, size, i);  // childrens center
+        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->getPointreps();
+            unsigned int length = points[0].length;
+            T *point = &(points[1].v);  // first point
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+//              cerr << point[0] << " " << point[1] << " " << point[2] << endl;
+              if ( shape.containsPoint(point) ) {
+                T * p = new T[BOctTree<T>::POINTDIM];
+                for (unsigned int iterator = 0; iterator < BOctTree<T>::POINTDIM; iterator++) {
+                  p[iterator] = point[iterator];
+                }
+                vpoints.push_back(p);
+              }
+              point+= BOctTree<T>::POINTDIM;
+            }
+          }
+        } else { // recurse
+          PointsOnShape( children->node, ccenter, size/2.0, shape, vpoints);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+
+  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--;
+
+/*    cout << (unsigned int)r << " nc " << (unsigned int)n_children << endl;
+    showbits(node.valid);
+    cout << endl;
+    showbits(node.leaf);
+    cout << endl;
+*/
+    bool leaf = false;
+    unsigned char child_index = 0;
+/*    if (r == 2) {
+    for (unsigned char i = 0; i < 8; i++) {
+      cout << "i " << (unsigned int)i << endl;
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        cout << "valid " << endl;
+        if (child_index == r) {
+          cout << "ci == r" << endl;
+          if (  ( 1 << i ) & node.leaf ) {   // if ith node exists
+            cout << "leaf" << endl;
+            leaf = true; 
+          }
+          cout << "no leaf" << endl;
+          break;
+        }
+        child_index++;
+      }
+    }
+    } else {*/
+    for (unsigned char i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        if (child_index == r) {
+          if (  ( 1 << i ) & node.leaf ) {   // if ith node exists
+            leaf = true; 
+          }
+          break;
+        }
+        child_index++;
+      }
+    }
+//    }
+//    cout << (unsigned int)r << " nc " << (unsigned int)n_children <<  " " << (unsigned int)child_index << endl;
+    if (child_index != r) return;   // bitmask valid might be all zero
+
+    if (leaf) {
+/*      cout << "STOPPED" << endl;
+        return;*/
+     
+      pointrep *points = children[r].getPointreps();
+      unsigned int length = points[0].length;
+      if (length < nrp) return;
+      if (length == nrp) {
+        for (char c = 0; c < nrp; c++) {
+          p.push_back(&(points[BOctTree<T>::POINTDIM*c+1].v));
+        }
+        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);
+        p.push_back(&(points[BOctTree<T>::POINTDIM*tmp+1].v));
+      }
+    } else {
+/*      printf("r: %d   parent %p  children %p   child[r]: %p \n", r, &node, children, &(children[r].node));
+    showbits(node.valid);
+    cout << endl;
+    showbits(node.leaf);
+    cout << endl;
+      cout << "RECURSED" << endl;*/
+      DrawPoints(p, children[r].node, nrp);
+    }
+  }
+
+};
+
+#endif

.svn/pristine/89/898e5bef351f7d0252d548094b5b5ae1d5042441.svn-base

@@ -0,0 +1,81 @@
+/*
+ * kdManaged implementation
+ *
+ * Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/** @file 
+ *  @brief An optimized k-d tree implementation
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+
+#ifdef _MSC_VER
+#define  _USE_MATH_DEFINES
+#endif
+
+#include "slam6d/kdManaged.h"
+#include "slam6d/scan.h"
+#include "slam6d/globals.icc"
+
+#include <iostream>
+using std::cout;
+using std::cerr;
+using std::endl;
+#include <algorithm>
+using std::swap;
+#include <cmath>
+#include <cstring>
+
+// KDtree class static variables
+template<class PointData, class AccessorData, class AccessorFunc>
+KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
+
+KDtreeManaged::KDtreeManaged(Scan* scan) :
+  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;
+}
+
+unsigned int* KDtreeManaged::prepareTempIndices(unsigned int n)
+{
+  m_temp_indices = new unsigned int[n];
+  for(unsigned int i = 0; i < n; ++i)
+    m_temp_indices[i] = i;
+  return m_temp_indices;
+}
+
+double* KDtreeManaged::FindClosest(double *_p, double maxdist2, int threadNum) const
+{
+  params[threadNum].closest = 0;
+  params[threadNum].closest_d2 = maxdist2;
+  params[threadNum].p = _p;
+  _FindClosest(*m_data, threadNum);
+  return params[threadNum].closest;
+}
+
+void KDtreeManaged::lock()
+{
+  boost::lock_guard<boost::mutex> lock(m_mutex_locking);
+  ++m_count_locking;
+  if(m_data == 0) {
+    // aquire an array lock from the scan and hold it while the tree is in use
+    m_data = new DataXYZ(m_scan->get("xyz reduced original"));
+  }
+}
+
+void KDtreeManaged::unlock()
+{
+  boost::lock_guard<boost::mutex> lock(m_mutex_locking);
+  --m_count_locking;
+  if(m_count_locking == 0 && m_data != 0) {
+    delete m_data;
+    m_data = 0;
+  }
+}

.svn/pristine/8b/8bf13af2e1da80f949d9fbbedda0f35e1840884e.svn-base

@@ -0,0 +1,415 @@
+#ifndef SCAN_H
+#define SCAN_H
+
+#include "io_types.h"
+#include "data_types.h"
+#include "point_type.h"
+#include "ptpair.h"
+
+#include <string>
+#include <vector>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/locks.hpp>
+
+//! SearchTree types
+enum nns_type {
+  simpleKD, ANNTree, BOCTree
+};
+
+class Scan;
+typedef std::vector<Scan*> ScanVector;
+
+class SearchTree;
+class ANNkd_tree;
+
+/** HOWTO scan
+First: Load scans (if you want to use the scanmanager, use ManagedScan)
+
+  BasicScan/ManagedScan::openDirectory(path, type, start, end);
+
+Pass it to functions (by reference to link it to the same instance) or store it in a global variable
+
+After loading you might want to set parameters
+
+  for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
+    Scan* scan = *it;
+    scan->setRangeFilter(maxDist, minDist);
+    scan->setHeightFilter(top, bottom); // thermo
+    scan->setReductionParameter(voxelSize, nrpts[, pointtype]);
+    scan->setSearchTreeParameter(nns_method, use_cuda);
+  }
+
+Access the contained data, will be loaded and calculated on demand
+
+  DataXYZ xyz = scan->get("xyz");
+  DataXYZ reduced = scan->get("xyz reduced");
+  DataRGB rgb = scan->get("rgb");
+  
+  xyz[i][0..2]
+  reflectance[i]
+  
+  unsigned int size = scan->size("xyz reduced");
+
+In order to use the prefetching of all requested data field in the scanserver, mark them for use. This is relevant for efficiency, which would otherwise cause loading the files each time another data field is requested.
+
+  scan->get(DATA_XYZ | DATA_RGB | ...);
+
+Under circumstances the data fields are not available (e.g. no color in uos-type scans)
+
+  DataRGB rgb = scan->get("rgb");
+  if(rgb.valid()) { ok, do something }
+  
+If backward-compability to pointer arrays is needed, the PointerArray class can adapt
+
+  BOctTree(PointerArray(scan->get("xyz")).get(), scan->size("xyz"), ...);
+
+If data isn't needed anymore, flag it for removal
+
+  scan->clear("xyz");
+  scan->clear(DATA_XYZ | DATA_RGB | ...);
+  
+Creating data fields with the correct byte size
+
+  scan->create("xyz somethingelse", sizeof(double)*3*N);
+
+Reading frames in show:
+
+  unsigned int size = scan->readFrames();
+  
+  const double* pose;
+  AlgoType type;
+  scan->getFrame(i, pose, type);
+
+Last, if program ends, clean up
+
+  Scan::closeDirectory(scans);
+
+**/
+
+
+
+/**
+ * This class bundles common features of different scan implementations by
+ * abstraction. It handles the algorithmic parts and leaves IO and other
+ * features to the deriving classes.
+ */
+class Scan {
+  //friend class SearchTree; // TODO: is this neccessary?
+public:
+  enum AlgoType {
+    INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO,
+    GRAPHHOGMAN
+  };
+  
+  // delete copy-ctor and assignment, scans shouldn't be copied by basic class
+  Scan(const Scan& other) = delete;
+  Scan& operator=(const Scan& other) = delete;
+  
+  virtual ~Scan();
+
+  //! Holder of all scans - also used in transform for adding frames for each scan at the same time
+  static std::vector<Scan*> allScans;
+  
+  /**
+    * Attempt to read a directory under \a path and return its read scans.
+    * No scans are loaded at this point, only checked if all exist.
+    * 
+    * @param scanserver whether to use managed scans in the scanserver or not
+    * @param path to the directory containing the scans
+    * @param type determining which ScanIO to use
+    * @param start first scan to use
+    * @param end last scan to use, -1 means from start to last available
+    */
+  static void openDirectory(bool scanserver, const std::string& path, IOType type,
+    int start, int end = -1);
+  
+  /**
+   * "Close" a directory by deleting all its scans and emptying the
+   * Scan::allScans vector.
+   */
+  static void closeDirectory();
+  
+  
+  /* Input filtering and parameter functions */
+  
+  
+  //! Input filtering for all points based on their euclidean length
+  virtual void setRangeFilter(double max, double min) = 0;
+  
+  //! Input filtering for all points based on their height
+  virtual void setHeightFilter(double top, double bottom) = 0;
+
+  //! Input mutation to set range of all points to a constant value;
+  virtual void setRangeMutation(double range) { }
+  
+  //! Set reduction parameters, but don't reduce yet
+  virtual void setReductionParameter(double voxelSize, int nrpts = 0,
+    PointType pointtype = PointType());
+  
+  //! Set SearchTree type, but don't create it yet
+  void setSearchTreeParameter(int nns_method, bool cuda_enabled);
+  
+  /**
+   * Set octtree parameters for show
+   * @param loadOct will load the serialized octtree from disk regardless
+   * @param saveOct serialize octtree if not loaded by loadOct after creation
+   */
+  virtual void setOcttreeParameter(double reduction_voxelSize,
+    double octtree_voxelSize, PointType pointtype,
+    bool loadOct, bool saveOct);
+
+
+  /* Basic getter functions */
+
+
+  inline const double* get_rPos() const;
+  inline const double* get_rPosTheta() const;
+  inline const double* get_rPosQuat() const;
+  //! Pose matrix after initial and match transformations (org+dalign)
+  inline const double* get_transMat() const;
+  //! Original pose matrix after initial transform
+  inline const double* get_transMatOrg() const;
+  //! Accumulated delta transformation matrix 
+  inline const double* getDAlign() const;
+  
+  inline SearchTree* getSearchTree();
+  inline ANNkd_tree* getANNTree() const;
+  
+  virtual const char* getIdentifier() const = 0;
+  
+  //! Determine the maximum number of reduced points in \a scans
+  static unsigned int getMaxCountReduced(ScanVector& scans);
+  
+  
+  /* Functions for altering data fields, implementation specific */
+  
+  
+  /**
+   * Get the data field \a identifier, calculate it on demand if neccessary.
+   *
+   * If "xyz reduced" or "xyz reduced original" is requested, the reduction is
+   * started with "xyz" as input.
+   */
+  virtual DataPointer get(const std::string& identifier) = 0;
+
+  /**
+   * Load the requested IODataTypes, joined by |, from the scan file.
+   *
+   * This feature is neccessary to load multiple data fields at once, not all
+   * one by one with each get("...") access.
+   */
+  virtual void get(unsigned int types) = 0;
+  
+  /**
+   * Creates a data field \a identifier with \a size bytes.
+   */
+  virtual DataPointer create(const std::string& identifier, unsigned int size) = 0;
+  
+  /**
+   * Clear the data field \a identifier, removing its allocated memory if
+   * possible or marking it for prioritized removal.
+   */
+  virtual void clear(const std::string& identifier) = 0;
+
+  //! Extension to clear for more than one identifier, e.g. clear(DATA_XYZ | DATA_RGB);
+  void clear(unsigned int types);
+  
+  /**
+   * Get the size of \a identifier as if it were requested and size() called
+   * upon its type specialized DataPointer class.
+   * e.g size<DataXYZ>("xyz reduced")
+   */
+  template<typename T>
+  unsigned int size(const std::string& identifier) {
+    return (T(get(identifier))).size();
+  }
+
+
+  /* Frame handling functions */
+
+
+  /**
+   * Open the .frames-file and read its contents. If not read, the frame list
+   * will be empty.
+   * @return count of frames if file has been read, zero otherwise
+   */
+  virtual unsigned int readFrames() = 0;
+  
+  /**
+   * Write the accumulated frames into a .frames-file.
+   */
+  virtual void saveFrames() = 0;
+  
+  //! Count of frames
+  virtual unsigned int getFrameCount() = 0;
+  
+  //! Get contents of a frame, pass matrix pointer and type by reference
+  virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) = 0;
+  
+protected:
+  /**
+   * Called from transform, this will add its current transMat pose with
+   * the given type as a frame into the list of frames
+   */
+  virtual void addFrame(AlgoType type) = 0;
+  
+public:
+  
+  /* Direct creation of reduced points and search tree */
+  
+  //! Apply reduction and initial transMatOrg transformation
+  void toGlobal();
+  
+  //! Copy reduced points to original and create search tree on it
+  void createSearchTree();
+
+
+  /* Common transformation and matching functions */
+  void mergeCoordinatesWithRoboterPosition(Scan* prevScan);
+  void transformAll(const double alignxf[16]);
+  void transformAll(const double alignQuat[4], const double alignt[3]);
+  
+  void transform(const double alignxf[16],
+    const AlgoType type, int islum = 0);
+  void transform(const double alignQuat[4],
+    const double alignt[3], const AlgoType type, int islum = 0);
+  void transformToMatrix(double alignxf[16],
+    const AlgoType type, int islum = 0);
+  void transformToEuler(double rP[3], double rPT[3],
+    const AlgoType type, int islum = 0);
+  void transformToQuat(double rP[3], double rPQ[4],
+    const AlgoType type, int islum = 0);
+  
+  // Scan matching functions
+  static void getPtPairs(std::vector<PtPair> *pairs,
+    Scan* Source, Scan* Target,
+    int thread_num,
+    int rnd, double max_dist_match2, double &sum,
+    double *centroid_m, double *centroid_d);
+  static void getNoPairsSimple(std::vector<double*> &diff,
+    Scan* Source, Scan* Target,
+    int thread_num,
+    double max_dist_match2);
+  static void getPtPairsSimple(std::vector<PtPair> *pairs,
+    Scan* Source, Scan* Target,
+    int thread_num,
+    int rnd, double max_dist_match2,
+    double *centroid_m, double *centroid_d);
+  static void getPtPairsParallel(std::vector<PtPair> *pairs,
+    Scan* Source, Scan* Target,
+    int thread_num, int step,
+    int rnd, double max_dist_match2,
+    double *sum,
+    double centroid_m[OPENMP_NUM_THREADS][3],
+    double centroid_d[OPENMP_NUM_THREADS][3]);
+
+protected:
+  /**
+   * The pose of the scan
+   * Note: rPos/rPosTheta and transMat _should_
+   *       always represent the same pose!!!
+   */
+  double rPos[3],    //!< 3D position
+    rPosTheta[3],    //!< 3D rotation in Euler representation 
+    rQuat[4],        //!< 3D rotation in Quaternion representation
+    transMat[16],    //!< (4x4) transformation matrix
+    transMatOrg[16]; //!< The original pose of the scan, e.g., from odometry
+
+  /**
+   * The dalignxf transformation represents the delta transformation virtually applied
+   * to the tree and is used to compute are actual corresponding points.
+   */
+  double dalignxf[16];
+
+  //! Run ICP on GPU instead of CPU
+  bool cuda_enabled;
+  
+  //! Defines the method used for nearest neighbor search and which tree to use
+  int nns_method;
+  
+  //! SearchTree for point pair matching, works on the search points
+  SearchTree* kd;
+  
+  //! This KD tree is created only for the CUDA usages
+  ANNkd_tree* ann_kd_tree;
+  
+  //! Voxelsize of the octtree used for reduction
+  double reduction_voxelSize;
+  
+  //! Which point to take out of the reduction octtree, 0 for center
+  int reduction_nrpts;
+  
+  //! Pointtype used for the reduction octtree
+  PointType reduction_pointtype;
+  
+  //! Type of the searchtree to be created
+  int searchtree_nnstype;
+  
+  //! Use CUDA for searching
+  bool searchtree_cuda_enabled;
+
+  //! Flag whether "xyz reduced" has been initialized for this Scan yet
+  bool m_has_reduced;
+  
+  //! Reduction value used for octtree input
+  double octtree_reduction_voxelSize;
+  
+  //! Voxelsize used in the octtree itself
+  double octtree_voxelSize;
+  
+  //! Pointtype for the Octtree
+  PointType octtree_pointtype;
+  
+  //! Flags to load or save the octtrees from/to storage
+  bool octtree_loadOct, octtree_saveOct;
+  
+  /**
+   * Basic initializing constructor calling the initalization function.
+   * Can only be called from deriving classes.
+   */
+  Scan();
+  
+  /**
+   * This function handles the reduction of points. It builds a lock for
+   * multithread-safety and calls caldReducedOnDemandPrivate.
+   *
+   * The intention is to reduce points, transforme them to the initial pose and
+   * then copy them to original for the SearchTree.
+   */
+  void calcReducedOnDemand();
+
+  //! Create specific SearchTree variants matching the capability of the Scan
+  virtual void createSearchTreePrivate() = 0;
+  
+  //! Create reduced points in a multithread-safe environment matching the capability of the Scan
+  virtual void calcReducedOnDemandPrivate() = 0;
+
+  //! Internal function of transform which alters the reduced points
+  void transformReduced(const double alignxf[16]);
+  
+  //! Internal function of transform which handles the matrices
+  void transformMatrix(const double alignxf[16]);
+  
+  //! Creating reduced points
+  void calcReducedPoints();
+  
+  //! Copies reduced points to original points without any transformation.
+  void copyReducedToOriginal();
+  
+  //! Inverse functionality of copyReducedToOriginal.
+  void copyOriginalToReduced();
+
+private:
+  //! flag for openDirectory and closeDirectory to distinguish the scans
+  static bool scanserver;
+
+public:
+  //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment  
+  // 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"
+
+#endif //SCAN_H

.svn/pristine/8c/8c20ecab50d40e311803ad7e83ce6eba44eb1821.svn-base

@@ -0,0 +1,368 @@
+/*
+ * icp6D implementation
+ *
+ * Copyright (C) Andreas Nuechter, Kai Lingemann
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+
+/**
+ * @file 
+ * @brief Implementation of 3D scan matching with ICP
+ * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+
+#include "slam6d/icp6D.h"
+
+#include "slam6d/metaScan.h"
+#include "slam6d/globals.icc"
+
+#include <iomanip>
+using std::cerr;
+
+#include <string.h>
+
+#ifdef _MSC_VER
+#if !defined _OPENMP && defined OPENMP 
+#define _OPENMP
+#endif
+#endif
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+/**
+ * Constructor 
+ *
+ * @param my_icp6Dminimizer Pointer to the ICP-minimizer
+ * @param max_dist_match Maximum distance to which point pairs are collected
+ * @param max_num_iterations Maximum number of iterations
+ * @param quiet Whether to print to the standard output
+ * @param meta Match against a meta scan?
+ * @param rnd Randomized point selection
+ * @param eP Extrapolate odometry?
+ * @param anim Animate which frames?
+ * @param epsilonICP Termination criterion
+ * @param nns_method Selects NNS method to be used  
+ */
+icp6D::icp6D(icp6Dminimizer *my_icp6Dminimizer, double max_dist_match,
+		   int max_num_iterations, bool quiet, bool meta, int rnd, bool eP,
+		   int anim, double epsilonICP, int nns_method, bool cuda_enabled,
+       bool cad_matching)
+{
+  this->my_icp6Dminimizer = my_icp6Dminimizer;
+  this->anim              = anim;
+  this->cuda_enabled      = cuda_enabled;
+  this->nns_method        = nns_method;
+  
+  if (!quiet) {
+    cout << "Maximal distance match      : " << max_dist_match << endl
+	 << "Maximal number of iterations: " << max_num_iterations << endl << endl;
+  }
+  
+  // checks
+  if (max_dist_match < 0.0) {
+    cerr << "ERROR [ICP6D]: first parameter (max_dist_match) has to be >= 0," << endl;
+    exit(1);
+  }
+  if (max_num_iterations < 0) {
+    cerr << "ERROR [ICP6D]: second parameter (max_num_iterations) has to be >= 0." << endl;
+    exit(1);
+  }
+  
+  this->max_dist_match2    = sqr(max_dist_match);
+  this->max_num_iterations = max_num_iterations;
+  this->quiet              = quiet;
+  this->meta               = meta;
+  this->rnd                = rnd;
+  this->eP                 = eP;
+  this->epsilonICP         = epsilonICP;
+  
+  // Set initial seed (for "real" random numbers)
+  //  srand( (unsigned)time( NULL ) );
+  this->cad_matching = cad_matching;
+}
+
+/**
+ * Matches a 3D Scan against a 3D Scan
+ * @param PreviousScan The scan or metascan forming the model
+ * @param CurrentScan The current scan thas is to be matched
+ * @return The number of iterations done in this matching run
+ */
+int icp6D::match(Scan* PreviousScan, Scan* CurrentScan)
+{
+  // If ICP shall not be applied, then just write
+  // the identity matrix
+  if (max_num_iterations == 0) {
+    double id[16];
+    M4identity(id);
+    CurrentScan->transform(id, Scan::ICP, 0);  // write end pose
+    return 0;
+  }
+
+  // icp main loop
+  double ret = 0.0, prev_ret = 0.0, prev_prev_ret = 0.0;
+  int iter = 0;
+  double alignxf[16];
+  for (iter = 0; iter < max_num_iterations; iter++) {
+
+    prev_prev_ret = prev_ret;
+    prev_ret = ret;
+
+#ifdef _OPENMP
+    // Implementation according to the paper 
+    // "The Parallel Iterative Closest Point Algorithm"
+    // by Langis / Greenspan / Godin, IEEE 3DIM 2001
+    //
+    // The same information are given in (ecrm2007.pdf)
+    // Andreas Nüchter. Parallelization of Scan Matching
+    // for Robotic 3D Mapping. In Proceedings of the 3rd
+    // European Conference on Mobile Robots (ECMR '07),
+    // Freiburg, Germany, September 2007
+    omp_set_num_threads(OPENMP_NUM_THREADS);
+
+    int max = (int)CurrentScan->size<DataXYZ>("xyz reduced");
+    int step = max / OPENMP_NUM_THREADS;
+
+    vector<PtPair> pairs[OPENMP_NUM_THREADS];
+    double sum[OPENMP_NUM_THREADS];
+    double centroid_m[OPENMP_NUM_THREADS][3];
+    double centroid_d[OPENMP_NUM_THREADS][3];
+    double Si[OPENMP_NUM_THREADS][9];
+    unsigned int n[OPENMP_NUM_THREADS];
+
+    for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
+      sum[i] = centroid_m[i][0] = centroid_m[i][1] = centroid_m[i][2] = 0.0;
+      centroid_d[i][0] = centroid_d[i][1] = centroid_d[i][2] = 0.0;
+      Si[i][0] = Si[i][1] = Si[i][2] = Si[i][3] = Si[i][4] = Si[i][5] = Si[i][6] = Si[i][7] = Si[i][8] = 0.0;
+      n[i] = 0;
+    }
+
+#pragma omp parallel 
+    {
+      int thread_num = omp_get_thread_num();
+
+      Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
+          thread_num, step,
+          rnd, max_dist_match2,
+          sum, centroid_m, centroid_d);
+
+      n[thread_num] = (unsigned int)pairs[thread_num].size();
+
+      if ((my_icp6Dminimizer->getAlgorithmID() == 1) ||
+          (my_icp6Dminimizer->getAlgorithmID() == 2)) {
+        for (unsigned int i = 0; i < n[thread_num]; i++) {
+
+          double pp[3] = {pairs[thread_num][i].p1.x - centroid_m[thread_num][0],
+            pairs[thread_num][i].p1.y - centroid_m[thread_num][1],
+            pairs[thread_num][i].p1.z - centroid_m[thread_num][2]};
+          double qq[3] = {pairs[thread_num][i].p2.x - centroid_d[thread_num][0],
+            pairs[thread_num][i].p2.y - centroid_d[thread_num][1],
+            pairs[thread_num][i].p2.z - centroid_d[thread_num][2]};
+/*
+          double pp[3] = {pairs[thread_num][i].p1.x - centroid_d[thread_num][0],
+            pairs[thread_num][i].p1.y - centroid_d[thread_num][1],
+            pairs[thread_num][i].p1.z - centroid_d[thread_num][2]};
+          double qq[3] = {pairs[thread_num][i].p2.x - centroid_m[thread_num][0],
+            pairs[thread_num][i].p2.y - centroid_m[thread_num][1],
+            pairs[thread_num][i].p2.z - centroid_m[thread_num][2]};
+*/
+          // formula (6)
+          Si[thread_num][0] += pp[0] * qq[0];
+          Si[thread_num][1] += pp[0] * qq[1];
+          Si[thread_num][2] += pp[0] * qq[2];
+          Si[thread_num][3] += pp[1] * qq[0];
+          Si[thread_num][4] += pp[1] * qq[1];
+          Si[thread_num][5] += pp[1] * qq[2];
+          Si[thread_num][6] += pp[2] * qq[0];
+          Si[thread_num][7] += pp[2] * qq[1];
+          Si[thread_num][8] += pp[2] * qq[2];
+        }
+      }
+    } // end parallel
+    
+    // do we have enough point pairs?
+    unsigned int pairssize = 0;
+    for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
+      pairssize += n[i];
+    }
+    if (pairssize > 3) {
+      if ((my_icp6Dminimizer->getAlgorithmID() == 1) ||
+          (my_icp6Dminimizer->getAlgorithmID() == 2) ) {
+        ret = my_icp6Dminimizer->Point_Point_Align_Parallel(OPENMP_NUM_THREADS,
+            n, sum, centroid_m, centroid_d, Si, 
+            alignxf);
+      } else if (my_icp6Dminimizer->getAlgorithmID() == 6) {
+        ret = my_icp6Dminimizer->Point_Point_Align_Parallel(OPENMP_NUM_THREADS,
+            n, sum, centroid_m, centroid_d, 
+            pairs,
+            alignxf);
+      } else {
+        cout << "This parallel minimization algorithm is not implemented !!!" << endl;
+        exit(-1);
+      }
+    } else {
+      //break;
+    }
+#else
+
+    double centroid_m[3] = {0.0, 0.0, 0.0};
+    double centroid_d[3] = {0.0, 0.0, 0.0};
+    vector<PtPair> pairs;
+   
+    Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd,
+        max_dist_match2, ret, centroid_m, centroid_d);
+
+    // do we have enough point pairs?
+    if (pairs.size() > 3) {
+      if (my_icp6Dminimizer->getAlgorithmID() == 3 || my_icp6Dminimizer->getAlgorithmID() == 8 ) {
+        memcpy(alignxf, CurrentScan->get_transMat(), sizeof(alignxf));
+      }
+      ret = my_icp6Dminimizer->Point_Point_Align(pairs, alignxf, centroid_m, centroid_d);
+    } else {
+      break;
+    }
+   
+#endif
+
+    if ((iter == 0 && anim != -2) || ((anim > 0) && (iter % anim == 0))) {
+      CurrentScan->transform(alignxf, Scan::ICP, 0);   // transform the current scan
+    } else {
+      CurrentScan->transform(alignxf, Scan::ICP, -1);  // transform the current scan
+    }
+    
+    if ((fabs(ret - prev_ret) < epsilonICP) && (fabs(ret - prev_prev_ret) < epsilonICP)) {
+      double id[16];
+      M4identity(id);
+      if(anim == -2) {
+        CurrentScan->transform(id, Scan::ICP, -1);  // write end pose
+      } else {
+        CurrentScan->transform(id, Scan::ICP, 0);  // write end pose
+      }
+    break;
+    }
+  }
+  
+  return iter;
+}
+
+
+/**
+ * Computes the point to point error between two scans 
+ * 
+ *
+ */
+double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double max_dist_match, unsigned int *np) {
+  double error = 0;
+  unsigned int nr_ppairs = 0;
+
+#ifdef _OPENMP
+    omp_set_num_threads(OPENMP_NUM_THREADS);
+
+    int max = (int)CurrentScan->size<DataXYZ>("xyz reduced");
+    int step = max / OPENMP_NUM_THREADS;
+
+    vector<PtPair> pairs[OPENMP_NUM_THREADS];
+    double sum[OPENMP_NUM_THREADS];
+    double centroid_m[OPENMP_NUM_THREADS][3];
+    double centroid_d[OPENMP_NUM_THREADS][3];
+
+    for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
+      sum[i] = centroid_m[i][0] = centroid_m[i][1] = centroid_m[i][2] = 0.0;
+      centroid_d[i][0] = centroid_d[i][1] = centroid_d[i][2] = 0.0;
+    }
+
+#pragma omp parallel 
+    {
+      int thread_num = omp_get_thread_num();
+      Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
+          thread_num, step,
+          rnd, sqr(max_dist_match),
+          sum, centroid_m, centroid_d);
+
+    } 
+
+    for (unsigned int thread_num = 0; thread_num < OPENMP_NUM_THREADS; thread_num++) {
+      for (unsigned int i = 0; i < (unsigned int)pairs[thread_num].size(); i++) {
+        error += sqr(pairs[thread_num][i].p1.x - pairs[thread_num][i].p2.x)
+          + sqr(pairs[thread_num][i].p1.y - pairs[thread_num][i].p2.y)
+          + sqr(pairs[thread_num][i].p1.z - pairs[thread_num][i].p2.z);
+      }
+      nr_ppairs += (unsigned int)pairs[thread_num].size();
+    }
+#else
+
+    double centroid_m[3] = {0.0, 0.0, 0.0};
+    double centroid_d[3] = {0.0, 0.0, 0.0};
+    vector<PtPair> pairs;
+
+    Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),error, centroid_m, centroid_d);
+
+    // getPtPairs computes error as sum of squared distances
+    error = 0;
+
+    for (unsigned int i = 0; i < pairs.size(); i++) {
+      error += sqrt(
+          sqr(pairs[i].p1.x - pairs[i].p2.x)
+        + sqr(pairs[i].p1.y - pairs[i].p2.y)
+        + sqr(pairs[i].p1.z - pairs[i].p2.z) );
+    }
+    nr_ppairs = pairs.size();
+#endif
+
+    if (np) *np = nr_ppairs;
+//    return sqrt(error/nr_ppairs);
+    return error/nr_ppairs;
+}
+/**
+ * This function matches the scans only with ICP
+ * 
+ * @param allScans Contains all necessary scans.
+ */
+void icp6D::doICP(vector <Scan *> allScans)
+{
+  double id[16];
+  M4identity(id);
+  
+  vector < Scan* > meta_scans;
+  Scan* my_MetaScan = 0;
+
+  
+  for(unsigned int i = 0; i < allScans.size(); i++) {
+    cout << i << "*" << endl;
+
+    Scan *CurrentScan = allScans[i];
+    Scan *PreviousScan = 0;
+    
+    if (i > 0) {
+      PreviousScan = allScans[i-1];
+      if (eP) {                             // extrapolate odometry
+        CurrentScan->mergeCoordinatesWithRoboterPosition(PreviousScan);
+      }
+    }
+
+    if (i > 0) {
+      if (meta) {
+        match(my_MetaScan, CurrentScan);
+      } else
+      if (cad_matching) {
+        match(allScans[0], CurrentScan);
+      } else {
+        match(PreviousScan, CurrentScan);
+      }
+    }
+
+    // push processed scan
+    if ( meta && i != allScans.size()-1 ) {
+      meta_scans.push_back(CurrentScan);
+      if (my_MetaScan) {
+        delete my_MetaScan;
+      }
+      my_MetaScan = new MetaScan(meta_scans, nns_method, cuda_enabled);
+    }
+  }
+}
+

.svn/pristine/8c/8c67cafbe8df18cdcae38de178295aceb23c703b.svn-base

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

.svn/pristine/8e/8edbc31d77e82218f78f4577d47d00a87bba870c.svn-base

@@ -0,0 +1,929 @@
+/**
+ * @file 
+ * @brief Representation of an octree for show
+ * @author Jan Elseberg. Jacobs University Bremen gGmbH, Germany
+ * @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
+ */
+
+#ifndef SHOWBOCTREE_H
+#define SHOWBOCTREE_H
+
+#include "slam6d/Boctree.h"
+#include "show/colormanager.h"
+#include "show/scancolormanager.h"
+#include "show/viewcull.h"
+#include "show/colordisplay.h"
+#include "slam6d/scan.h"
+
+using namespace show;
+
+/**
+ * @brief Octree for show
+ * 
+ * A cubic bounding box is calculated
+ * from the given 3D points. Then it
+ * is recusivly subdivided into smaller
+ * subboxes
+ *
+ * It contains software culling functionalities
+ */
+template <class T>
+class Show_BOctTree : public colordisplay
+{
+protected:
+  BOctTree<T>* m_tree;
+  
+  unsigned long maxtargetpoints;
+
+  unsigned int current_lod_mode;
+
+  //! A copy of pointdim of the tree initialized from
+  unsigned int POINTDIM;
+
+  DataOcttree* m_cache_access;
+  Scan* m_scan;
+
+  void init(ScanColorManager* scm) {
+    setColorManager(0);
+    if (scm) {
+      scm->registerTree(this);
+      scm->updateRanges(m_tree->getMins());
+      scm->updateRanges(m_tree->getMaxs());
+    }
+    POINTDIM = m_tree->getPointdim();
+    maxtargetpoints = maxTargetPoints(m_tree->getRoot());
+    current_lod_mode = 0;
+    m_cache_access = 0;
+    m_scan = 0;
+  }
+  
+public:
+  //! Create with tree held in cache, lock indefinitely
+  Show_BOctTree(Scan* scan, DataOcttree* cache_access, ScanColorManager* scm = 0)
+  {
+    m_tree = &cache_access->get();
+    init(scm);
+    // save cache access and hold it until an unlock
+    m_cache_access = cache_access;
+    m_scan = scan;
+  }
+
+  //! Retrieve a cache access object and update the tree pointer
+  void lockCachedTree() {
+    if(m_scan != 0 && m_cache_access == 0) {
+      m_cache_access = new DataOcttree(m_scan->get("octtree"));
+      m_tree = &(m_cache_access->get());
+    }
+  }
+
+  //! Remove the data access
+  void unlockCachedTree() {
+    if(m_scan != 0 && m_cache_access != 0) {
+      delete m_cache_access; m_cache_access = 0;
+      //m_tree = 0;
+    }
+  }
+
+  //! Create with already constructed tree and take ownership
+  Show_BOctTree(BOctTree<T>* tree, ScanColorManager* scm = 0) :
+    m_tree(tree)
+  {
+    init(scm);
+  }
+  
+  //! Create tree by points
+  template <class P>
+  Show_BOctTree(P * const* pts, int n, T voxelSize, PointType pointtype = PointType(), ScanColorManager *scm = 0)
+  {
+    m_tree = new BOctTree<T>(pts, n, voxelSize, pointtype, true);
+    init(scm);
+  }
+  
+  //! Create tree by deserializing from file
+  Show_BOctTree(std::string filename, ScanColorManager *scm = 0)
+  {
+    m_tree = new BOctTree<T>(filename);
+    init(scm);
+  }
+  
+  virtual ~Show_BOctTree() {
+    // only delete cache access if created via this method
+    if(m_cache_access) {
+      delete m_cache_access;
+    }
+    // if not constructed by a cache-located tree, delete the owned tree
+    if(!m_scan)
+      delete m_tree;
+  }
+  
+  BOctTree<T>* getTree() const { return m_tree; }
+  
+  void serialize(const std::string& filename) const { m_tree->serialize(filename); }
+  
+  unsigned int getMemorySize() const { return m_tree->getMemorySize(); }
+  
+  // virtual functions from colordisplay
+
+  void selectRayBrushSize(set<T *> &points, int brushsize) {
+    selectRayBS(points, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), brushsize);
+  }
+  
+  void selectRay(set<T *> &points, int depth = INT_MAX) {
+    selectRay(points, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), depth);
+  }
+  
+  void selectRay(T * &point) {
+    selectRay(point, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), FLT_MAX);
+  }
+
+  void cycleLOD() {
+    current_lod_mode = (current_lod_mode+1)%3;
+  }
+
+  void drawLOD(float ratio) {
+    switch (current_lod_mode) {
+      case 0:
+        glBegin(GL_POINTS);
+        displayOctTreeCulledLOD(maxtargetpoints * ratio, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize());
+        glEnd();
+        break;
+      case 1:
+        glBegin(GL_POINTS);
+        displayOctTreeCulledLOD2(ratio, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize());
+        glEnd();
+        break;
+      case 2:
+#ifdef WITH_GLEE
+        if (GLEE_ARB_point_parameters) {
+          glPointParameterfARB(GL_POINT_SIZE_MIN_ARB, 1.0);
+          glPointParameterfARB(GL_POINT_SIZE_MAX_ARB, 100000.0);
+          GLfloat p[3] = {0.0, 0.0000, 0.0000005};
+          glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
+          displayOctTreeCPAllCulled(m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(),
+							 m_tree->getSize() / pow(2, min( (int)(ratio * m_tree->getMaxDepth()), (int)(m_tree->getMaxDepth() - 3))));
+          p[0] = 1.0;
+          p[2] = 0.0;
+          glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
+        }
+#endif
+        break;
+      default:
+        break;
+    }
+  }
+  
+  void draw() {
+    glBegin(GL_POINTS);
+    displayOctTreeAllCulled(m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize());
+    glEnd();
+  }
+  
+  // reroute center call (for recast from colordisplay to show_bocttree)
+  void getCenter(double _center[3]) const {
+    m_tree->getCenter(_center);
+  }
+  
+protected:
+  
+  //! ?
+  unsigned long maxTargetPoints(const bitoct &node) {
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    unsigned long max = 0;
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          pointrep *points = children->getPointreps();
+          unsigned long length = points[0].length;
+          if (length > max) max = length;
+        } else { // recurse
+          unsigned long tp = maxTargetPoints(children->node);
+          if (tp > max) max = tp;
+        }
+        ++children; // next child
+      }
+    }
+
+    return max*POPCOUNT(node.valid);
+  }
+
+  void displayOctTreeAll(const bitoct &node) {
+//    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          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++ ) {
+            if(cm) cm->setColor(point);
+            glVertex3f( point[0], point[1], point[2]);
+            point+=POINTDIM;
+          }
+        } else { // recurse
+          displayOctTreeAll( children->node);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  void displayOctTreeAllCulled(const bitoct &node, const T* center, T size ) {
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return;  // culled do not continue with this branch of the tree
+
+    if (res == 2) { // if entirely within frustrum discontinue culling
+      displayOctTreeAll(node);
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+       //   if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            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++ ) {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+              point+=POINTDIM;
+            }
+          //}
+        } else { // recurse
+          displayOctTreeAllCulled( children->node, ccenter, size/2.0);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeLOD2(float ratio, const bitoct &node, const T* center, T size ) {
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          pointrep *points = children->getPointreps();
+          unsigned int length = points[0].length;
+          T *point = &(points[1].v);  // first point
+
+          int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+          l = max((int)(l*l*ratio), 0);
+          if (l > 1) {
+            if ((int)length > l ) {
+              T each = (T)POINTDIM * (T)((T)length/(T)l);
+              T *p;
+              int index;
+              for(int iterator = 0; iterator < l; iterator++ ) {
+                index = (T)iterator * each;
+                p = point + index - index%POINTDIM;
+                if(cm) cm->setColor(p);
+                glVertex3f( p[0], p[1], p[2]);
+              }
+            } else if ((int)length <= l) { 
+              for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+                if(cm) cm->setColor(point);
+                glVertex3f( point[0], point[1], point[2]);
+                point+=POINTDIM;
+              }
+            } /* else if (l == 1) {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+            }*/
+          } else {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+          }
+        } else { // recurse
+            int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+            l = max((int)(l*l*ratio), 0);
+            if (l > 0) {
+              displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0);
+            }
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeCulledLOD2(float ratio, const bitoct &node, const T* center, T size ) {
+
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return;  // culled do not continue with this branch of the tree
+
+    if (res == 2) { // if entirely within frustrum discontinue culling
+      displayOctTreeLOD2(ratio, node, center, size);
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            pointrep *points = children->getPointreps();
+            unsigned int length = points[0].length;
+            T *point = &(points[1].v);  // first point
+
+            int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+            l = max((int)(l*l*ratio), 0);
+            if (l != 0) {
+              if ((int)length > l ) {
+                T each = (T)POINTDIM * (T)((T)length/(T)l);
+                T *p;
+                int index;
+                for(int iterator = 0; iterator < l; iterator++ ) {
+                  index = (T)iterator * each;
+                  p = point + index - index%POINTDIM;
+                  if(cm) cm->setColor(p);
+                  glVertex3f( p[0], p[1], p[2]);
+                }
+              } else if ((int)length <= l) { 
+                for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+                  if(cm) cm->setColor(point);
+                  glVertex3f( point[0], point[1], point[2]);
+                  point+=POINTDIM;
+                }
+              } else if (l == 1) {
+                if(cm) cm->setColor(point);
+                glVertex3f( point[0], point[1], point[2]);
+              }
+            } 
+          }
+        } else { // recurse
+          //int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+          //l = max((int)(l*l*ratio), 0);
+          //if (l > 0) {
+            displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0);
+          //}
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeLOD3(long targetpts, const bitoct &node, const T* center, T size ) {
+    if (targetpts <= 0) return; // no need to display anything
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          pointrep *points = children->getPointreps();
+          unsigned int length = points[0].length;
+          T *point = &(points[1].v);  // first point
+          int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+          
+          if ( l <= targetpts) {  // only a single pixel on screen only paint one point
+            if(cm) cm->setColor(point);
+            glVertex3f( point[0], point[1], point[2]);
+          } else { 
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+              point+=POINTDIM;
+            }
+          }
+        } else { // recurse
+          displayOctTreeLOD3(targetpts, children->node, ccenter, size/2.0);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeCulledLOD3(long targetpts, const bitoct &node, const T* center, T size ) {
+    if (targetpts <= 0) return; // no need to display anything
+
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return;  // culled do not continue with this branch of the tree
+
+    if (res == 2) { // if entirely within frustrum discontinue culling
+      displayOctTreeLOD3(targetpts, node, center, size);
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        //l = std::min( max((int)(l*l*ratio), 1), targetpts);
+        
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            pointrep *points = children->getPointreps();
+            unsigned int length = points[0].length;
+            T *point = &(points[1].v);  // first point
+
+            int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0);  // only a single pixel on screen only paint one point
+          if (targetpts <= 0 ) cout << l << " " << targetpts << endl;
+            if ( l <= targetpts) {  // only a single pixel on screen only paint one point
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+            } else { 
+              for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+                if(cm) cm->setColor(point);
+                glVertex3f( point[0], point[1], point[2]);
+                point+=POINTDIM;
+              }
+            }
+          }
+
+        } else { // recurse
+          displayOctTreeCulledLOD3(targetpts, children->node, ccenter, size/2.0);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  void displayOctTreeCulledLOD(long targetpts, const bitoct &node, const T* center, T size ) {
+    if (targetpts <= 0) return; // no need to display anything
+
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return;  // culled do not continue with this branch of the tree
+
+    if (res == 2) { // if entirely within frustrum discontinue culling
+      displayOctTreeLOD(targetpts, node, center, size);
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    unsigned short nc = POPCOUNT(node.valid);
+    long newtargetpts = targetpts;
+    if (nc > 0) {
+      newtargetpts = newtargetpts/nc;
+      if (newtargetpts <= 0 ) return;
+    }
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            pointrep *points = children->getPointreps();
+            unsigned int length = points[0].length;
+            T *point = &(points[1].v);  // first point
+
+            if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {  // only a single pixel on screen only paint one point
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+            } else if (length <= newtargetpts) {        // more points requested than possible, plot all
+              for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+                if(cm) cm->setColor(point);
+                glVertex3f( point[0], point[1], point[2]);
+                point+=POINTDIM;
+              }
+            } else {                         // select points to show
+              // TODO smarter subselection of points here
+              T each = (T)POINTDIM * (T)((T)length/(T)newtargetpts);
+              T *p;
+              int index;
+              for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
+                index = (T)iterator * each;
+                p = point + index - index%POINTDIM;
+                if(cm) cm->setColor(p);
+                glVertex3f( p[0], p[1], p[2]);
+                //point += each;
+              }
+            }
+          }
+
+        } else { // recurse
+          displayOctTreeCulledLOD(newtargetpts, children->node, ccenter, size/2.0);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  void displayOctTreeLOD(long targetpts, const bitoct &node, const T* center, T size ) {
+    if (targetpts <= 0) return; // no need to display anything
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    unsigned short nc = POPCOUNT(node.valid);
+    long newtargetpts = targetpts;
+    if (nc > 0) {
+      newtargetpts = newtargetpts/nc;
+      if (newtargetpts <= 0 ) return;
+    }
+
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          pointrep *points = children->getPointreps();
+          unsigned int length = points[0].length;
+          T *point = &(points[1].v);  // first point
+          if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {  // only a single pixel on screen only paint one point
+            if(cm) cm->setColor(point);
+            glVertex3f( point[0], point[1], point[2]);
+          } else if (length <= newtargetpts) {        // more points requested than possible, plot all
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+              point+=POINTDIM;
+            }
+          } else {                         // select points to show
+            // TODO smarter subselection of points here
+            T each = (T)POINTDIM * (T)((T)length/(T)newtargetpts);
+            T *p;
+            int index;
+            for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
+              index = (T)iterator * each;
+              p = point + index - index%POINTDIM;
+              if(cm) cm->setColor(p);
+              glVertex3f( p[0], p[1], p[2]);
+              //point += each;
+            }
+          }
+        } else { // recurse
+          displayOctTreeLOD(newtargetpts, children->node, ccenter, size/2.0);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  void selectRay(set<T *> &selpoints, const bitoct &node, const T* center, T size, int max_depth, int depth = 0) {
+    if (depth < max_depth &&  !HitBoundingBox(center, size ))return;
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( !(depth+1  < max_depth) || HitBoundingBox(ccenter, size) ) {
+            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++ ) {
+                selpoints.insert(point);
+              point+=POINTDIM;
+            }
+          }
+        } else { // recurse
+          selectRay( selpoints, children->node, ccenter, size/2.0, max_depth, depth+1);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  void selectRayBS(set<T *> &selpoints, const bitoct &node, const T* center, T size, int brushsize) {
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( HitBoundingBox(ccenter, size) ) {
+            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++ ) {
+              if (ScreenDist(point) < brushsize && RayDist(point) > 100.0) 
+                selpoints.insert(point);
+              point+=POINTDIM;
+            }
+          }
+        } else { // recurse
+          selectRayBS( selpoints, children->node, ccenter, size/2.0, brushsize);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  void selectRay(T * &selpoint, const bitoct &node, const T* center, T size, float min) {
+    if (!HitBoundingBox(center, size ))return;
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( HitBoundingBox(ccenter, size) ) {
+            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++ ) {
+              T dist = RayDist(point);
+              if (min > dist && ScreenDist(point) < 5 && dist > 100.0) {
+                selpoint = point;
+                min = dist;
+              }
+              point+=POINTDIM;
+            }
+          }
+        } else { // recurse
+          selectRay( selpoint, children->node, ccenter, size/2.0, min);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeCAllCulled(const bitoct &node, const T* center, T size, T minsize ) {
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return;  // culled do not continue with this branch of the tree
+
+    if (res == 2) { // if entirely within frustrum discontinue culling
+      displayOctTreeCAll(node, center, size, minsize);
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf || minsize > size ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            showCube(ccenter, size/2.0);
+          }
+        } else { // recurse
+          displayOctTreeCAllCulled( children->node, ccenter, size/2.0, minsize);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeCAll(const bitoct &node, const T* center, T size, T minsize ) {
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf || minsize > size ) {   // if ith node is leaf get center
+          showCube(ccenter, size/2.0);
+        } else { // recurse
+          displayOctTreeCAll( children->node, ccenter, size/2.0, minsize);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  
+  void displayOctTreeCPAllCulled(const bitoct &node, const T* center, T size, T minsize ) {
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return;  // culled do not continue with this branch of the tree
+
+    if (res == 2) { // if entirely within frustrum discontinue culling
+      displayOctTreeCPAll(node, center, size, minsize);
+      return;
+    }
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if ( minsize > size ) {
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            if(cm) {
+              if (( 1 << i ) & node.leaf ) 
+                cm->setColor( &(children->getPoints()[1]) );
+              else 
+                cm->setColor( m_tree->pickPoint(children->node) );
+            }
+
+            glPointSize(size/2.0);
+            glBegin(GL_POINTS);
+            glVertex3f( ccenter[0], ccenter[1], ccenter[2] ); 
+            glEnd();
+          }
+        }else if ( ( 1 << i ) & node.leaf ) {
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            pointrep *points = children->getPointreps();
+            unsigned int length = points[0].length;
+            T *point = &(points[1].v);  // first point
+            glPointSize(1.0);
+            glBegin(GL_POINTS);
+            for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+              if(cm) cm->setColor(point);
+              glVertex3f( point[0], point[1], point[2]);
+              point+=POINTDIM;
+            }
+            glEnd();
+          }
+        } else { // recurse
+          displayOctTreeCPAllCulled( children->node, ccenter, size/2.0, minsize);
+        }
+        ++children; // next child
+      }
+    }
+  }
+  
+  void displayOctTreeCPAll(const bitoct &node, const T* center, T size, T minsize ) {
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if ( minsize > size ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            if(cm) {
+              if (( 1 << i ) & node.leaf ) 
+                cm->setColor( &(children->getPoints()[1]) );
+              else 
+                cm->setColor( m_tree->pickPoint(children->node) );
+            }
+
+            glPointSize(size/2.0);
+            glBegin(GL_POINTS);
+            glVertex3f( ccenter[0], ccenter[1], ccenter[2] ); 
+            glEnd();
+          }
+        }else if ( ( 1 << i ) & node.leaf ) {
+          pointrep *points = children->getPointreps();
+          unsigned int length = points[0].length;
+          T *point = &(points[1].v);  // first point
+          glPointSize(1.0);
+          glBegin(GL_POINTS);
+          for(unsigned int iterator = 0; iterator < length; iterator++ ) {
+            if(cm) cm->setColor(point);
+            glVertex3f( point[0], point[1], point[2]);
+            point+=POINTDIM;
+          }
+          glEnd();
+        } else { // recurse
+          displayOctTreeCPAll( children->node, ccenter, size/2.0, minsize);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  void showCube(const T* center, T size) {
+    glLineWidth(1.0);
+    glBegin(GL_QUADS);      // draw a cube with 6 quads
+    glColor3f(0.0f,1.0f,0.0f);      // Set The Color To Green
+    glVertex3f(center[0] + size, center[1] + size, center[2] - size);
+    glVertex3f(center[0] - size, center[1] + size, center[2] - size);
+    glVertex3f(center[0] - size, center[1] + size, center[2] + size);
+    glVertex3f(center[0] + size, center[1] + size, center[2] + size);
+    glColor3f(1.0f,0.5f,0.0f);      // Set The Color To Orange
+
+    glVertex3f(center[0] + size, center[1] - size, center[2] + size); 
+    glVertex3f(center[0] - size, center[1] - size, center[2] + size);
+    glVertex3f(center[0] - size, center[1] - size, center[2] - size);
+    glVertex3f(center[0] + size, center[1] - size, center[2] - size);
+
+    glColor3f(1.0f,0.0f,0.0f);      // Set The Color To Red
+    glVertex3f(center[0] + size, center[1] + size, center[2] + size); 
+    glVertex3f(center[0] - size, center[1] + size, center[2] + size);
+    glVertex3f(center[0] - size, center[1] - size, center[2] + size);
+    glVertex3f(center[0] + size, center[1] - size, center[2] + size);
+
+    glColor3f(1.0f,1.0f,0.0f);      // Set The Color To Yellow
+    glVertex3f(center[0] + size, center[1] - size, center[2] - size); 
+    glVertex3f(center[0] - size, center[1] - size, center[2] - size);
+    glVertex3f(center[0] - size, center[1] + size, center[2] - size);
+    glVertex3f(center[0] + size, center[1] + size, center[2] - size);
+
+    glColor3f(0.0f,0.0f,1.0f);      // Set The Color To Blue
+    glVertex3f(center[0] - size, center[1] + size, center[2] + size); 
+    glVertex3f(center[0] - size, center[1] + size, center[2] - size);
+    glVertex3f(center[0] - size, center[1] - size, center[2] - size);
+    glVertex3f(center[0] - size, center[1] - size, center[2] + size);
+
+    glColor3f(1.0f,0.0f,1.0f);      // Set The Color To Violet
+    glVertex3f(center[0] + size, center[1] + size, center[2] - size); 
+    glVertex3f(center[0] + size, center[1] + size, center[2] + size);
+    glVertex3f(center[0] + size, center[1] - size, center[2] + size);
+    glVertex3f(center[0] + size, center[1] - size, center[2] - size);
+
+    glEnd();
+  }
+  
+  
+  void displayOctTreeAllCulled(const bitoct &node, const T* center, T size, float *frustum[6], unsigned char frustsize ) {
+    float *new_frustum[6]; unsigned char counter = 0;
+    for (unsigned char p = 0; p < frustsize; p++ ) {
+      char res = PlaneAABB(center[0], center[1], center[2], size, frustum[p]);
+      if (res == 0) { // cube is on the wrong side of the plane (not visible)
+        return;
+      } else if ( res == 1 ) {  // plane intersects this volume continue culling with this plane
+        new_frustum[counter++] = frustum[p];
+      } // other case is simply not to continue culling with the respective plane
+    }
+    if (counter == 0) { // if entirely within frustrum discontinue culling
+      displayOctTreeAll(node);
+      return;
+    }
+    
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          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++ ) {
+            if(cm) cm->setColor(point);
+            glVertex3f( point[0], point[1], point[2]);
+            point+=POINTDIM;
+          }
+        } else { // recurse
+          displayOctTreeAllCulled( children->node, ccenter, size/2.0, new_frustum, counter);
+        }
+        ++children; // next child
+      }
+    }
+  }
+
+  unsigned long int countVisiblePoints(const bitoct &node, const T* center, T size ) {
+    unsigned long int result = 0;
+    int res = CubeInFrustum2(center[0], center[1], center[2], size);
+    if (res==0) return 0;  // culled do not continue with this branch of the tree
+
+    T ccenter[3];
+    bitunion<T> *children;
+    bitoct::getChildren(node, children);
+
+    for (short i = 0; i < 8; i++) {
+      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
+        BOctTree<T>::childcenter(center, ccenter, size, i);  // childrens center
+        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
+          // check if leaf is visible
+          if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
+            pointrep *points = children->getPointreps();
+            unsigned int length = points[0].length;
+            result += length;
+          }
+        } else { // recurse
+          result += countVisiblePoints( children->node, ccenter, size/2.0);
+        }   
+        ++children; // next child
+      }
+    }         
+    return result;
+  }
+};
+
+#endif

.svn/pristine/92/92769b3f0cbdb909581dce5eb72ac79f3b4b593d.svn-base

@@ -0,0 +1,37 @@
+#ifndef __THERMO_H__
+#define __THERMO_H__
+
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+#include <string>
+#include <slam6d/scan.h>
+using namespace std;
+//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);
+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);
+
+void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir,
+IOType type, int scale, double rot_angle, double minDist, double maxDist,
+bool correction, int neighborhood, int method=0);
+
+bool readPoints(string filename, CvPoint3D32f *corners, int size) ;
+void sortElementByElement(CvMat * vectors, int nr_elems, int nr_vectors); 
+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, 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);
+
+
+#endif

.svn/pristine/93/93ab3110d14d8d16d540a7cc2b137ba3f644dd08.svn-base

@@ -0,0 +1,33 @@
+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)
+  ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
+    SET(SHOW_LIBS ${SHOW_LIBS}  ${CMAKE_SOURCE_DIR}/3rdparty/windows/freeglut.lib XGetopt)
+  ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
+ENDIF(WIN32)
+IF (UNIX)
+  SET(SHOW_LIBS newmat dl ${SHOW_LIBS} ${GLUT_LIBRARIES})
+ENDIF(UNIX)
+
+IF(WITH_GLEE)
+  SET(SHOW_LIBS ${SHOW_LIBS} glee)
+ENDIF(WITH_GLEE)
+
+SET(SHOW_SRCS NurbsPath.cc  PathGraph.cc vertexarray.cc  viewcull.cc colormanager.cc compacttree.cc scancolormanager.cc display.cc)
+
+IF (WITH_SHOW)
+  add_executable(show show.cc ${SHOW_SRCS})
+  target_link_libraries(show ${SHOW_LIBS})
+ENDIF(WITH_SHOW)
+
+IF(WITH_WXSHOW)
+  add_executable(wxshow wxshow.cc selectionframe.cc ${SHOW_SRCS})
+  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)

.svn/pristine/98/98c86c975579fc3529238a3892d5ae76a563a763.svn-base

@@ -0,0 +1,105 @@
+/*
+ * ann_kd implementation
+ *
+ * Copyright (C) Andreas Nuechter, Ulugbek Makhmudov 
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/** @file 
+ *  @brief Encapsules the implementation of ANN k-d trees. 
+ *  @author Ulugbek Makhmudov, Jacobs University Bremen, Bremen, Germany.
+ *  @author Andreas Nuechter, Jacobs University Bremen, Bremen, Germany.
+ */
+
+#ifdef _MSC_VER
+#define  _USE_MATH_DEFINES
+#endif
+
+#include "slam6d/ann_kd.h"
+#include "slam6d/globals.icc"          
+
+#include <iostream>
+using std::cout;
+using std::cerr;
+using std::endl;
+#include <algorithm>
+using std::swap;
+#include <cmath>
+#include <cstring>
+
+
+
+/**
+ * Constructor
+ *
+ * Create ANN KD tree from the points pointed to by the array pts
+ *
+ * @param pts 3D array of points
+ * @param n number of points
+ */
+ANNtree::ANNtree(PointerArray<double>&_pts, int n)
+{
+  /*
+  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];
+  nn_idx = new ANNidx[1];    
+}
+
+/**
+ * Destructor
+ *
+ * Cleans up the instance of ANN KD tree
+ *
+ *
+ */
+ANNtree::~ANNtree()
+{
+  delete annkd; //links to the destructor of ANNkd_tree
+  delete [] nn;
+  delete [] nn_idx;
+  delete [] pts[0]; 
+  delete [] pts;
+}
+
+
+/**
+ * Finds the closest point within the tree,
+ * wrt. the point given as first parameter.
+ * @param _p point
+ * @param maxdist2 maximal search distance.
+ * @param threadNum Thread number, for parallelization
+ * @return Pointer to the closest point
+ */  
+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;
+
+  return pts[idx];
+}  
+

.svn/pristine/9a/9a3d9fe78edded475f414ab692cc66858d5dfad2.svn-base

@@ -0,0 +1,437 @@
+/*
+ * basicScan implementation
+ *
+ * Copyright (C) Thomas Escher, Kai Lingemann
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/basicScan.h"
+
+#include "scanio/scan_io.h"
+#include "slam6d/kd.h"
+#include "slam6d/Boctree.h"
+#include "slam6d/ann_kd.h"
+
+#ifdef WITH_METRICS
+#include "slam6d/metrics.h"
+#endif //WITH_METRICS
+
+#include <list>
+#include <utility>
+#include <fstream>
+using std::ifstream;
+using std::ofstream;
+using std::flush;
+using std::string;
+using std::map;
+using std::pair;
+using std::vector;
+
+#include <boost/filesystem/operations.hpp>
+using namespace boost::filesystem;
+
+
+
+void BasicScan::openDirectory(const std::string& path, IOType type, int start, int end)
+{
+#ifdef WITH_METRICS
+  Timer t = ClientMetric::read_scan_time.start();
+#endif //WITH_METRICS
+
+  // create an instance of ScanIO
+  ScanIO* sio = ScanIO::getScanIO(type);
+
+  // query available scans in the directory from the ScanIO
+  std::list<std::string> identifiers(sio->readDirectory(path.c_str(), start, end));
+
+  Scan::allScans.reserve(identifiers.size());
+
+  // for each identifier, create a scan
+  for(std::list<std::string>::iterator it = identifiers.begin(); it != identifiers.end(); ++it) {
+    Scan::allScans.push_back(new BasicScan(path, *it, type));
+  }
+
+#ifdef WITH_METRICS
+  ClientMetric::read_scan_time.end(t);
+#endif //WITH_METRICS
+}
+
+void BasicScan::closeDirectory()
+{
+  // clean up the scan vector
+  for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
+    delete *it;
+  Scan::allScans.clear();
+}
+
+BasicScan::BasicScan(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)
+{
+  init();
+
+  // request pose from file
+  double euler[6];
+  ScanIO* sio = ScanIO::getScanIO(m_type);
+  sio->readPose(m_path.c_str(), m_identifier.c_str(), euler);
+  rPos[0] = euler[0];
+  rPos[1] = euler[1];
+  rPos[2] = euler[2];
+  rPosTheta[0] = euler[3];
+  rPosTheta[1] = euler[4];
+  rPosTheta[2] = euler[5];
+
+  // write original pose matrix
+  EulerToMatrix4(euler, &euler[3], transMatOrg);
+
+  // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead
+  transformMatrix(transMatOrg);
+
+  // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one
+  M4identity(dalignxf);
+}
+
+BasicScan::~BasicScan()
+{
+  // TODO: clean m_data up
+}
+
+void BasicScan::init()
+{
+  m_filter_max = 0.0;
+  m_filter_min = 0.0;
+  m_filter_top = 0.0;
+  m_filter_bottom = 0.0;
+  m_range_mutation = 0.0;
+  m_filter_range_set = false;
+  m_filter_height_set = false;
+  m_range_mutation_set = false;
+}
+
+
+void BasicScan::setRangeFilter(double max, double min)
+{
+  m_filter_max = max;
+  m_filter_min = min;
+  m_filter_range_set = true;
+}
+
+void BasicScan::setHeightFilter(double top, double bottom)
+{
+  m_filter_top = top;
+  m_filter_bottom = bottom;
+  m_filter_height_set = true;
+}
+
+void BasicScan::setRangeMutation(double range)
+{
+  m_range_mutation_set = true;
+  m_range_mutation = range;
+}
+
+void BasicScan::get(unsigned int types)
+{
+  ScanIO* sio = ScanIO::getScanIO(m_type);
+
+  vector<double> xyz;
+  vector<unsigned char> rgb;
+  vector<float> reflectance;
+  vector<float> temperature;
+  vector<float> amplitude;
+  vector<int> type;
+  vector<float> deviation;
+
+  PointFilter filter;
+  if(m_filter_range_set)
+    filter.setRange(m_filter_max, m_filter_min);
+  if(m_filter_height_set)
+    filter.setHeight(m_filter_top, m_filter_bottom);
+  if(m_range_mutation_set)
+    filter.setRangeMutator(m_range_mutation);
+
+  sio->readScan(m_path.c_str(),
+                m_identifier.c_str(),
+                filter,
+                &xyz,
+                &rgb,
+                &reflectance,
+                &temperature,
+                &amplitude,
+                &type,
+                &deviation);
+
+  // for each requested and filled data vector, allocate and write contents to their new data fields
+  if(types & DATA_XYZ && !xyz.empty()) {
+    double* data = reinterpret_cast<double*>(create("xyz", sizeof(double) * xyz.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < xyz.size(); ++i) data[i] = xyz[i];
+  }
+  if(types & DATA_RGB && !rgb.empty()) {
+    unsigned char* data = reinterpret_cast<unsigned char*>(create("rgb", sizeof(unsigned char) * rgb.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < rgb.size(); ++i) data[i] = rgb[i];
+  }
+  if(types & DATA_REFLECTANCE && !reflectance.empty()) {
+    float* data = reinterpret_cast<float*>(create("reflectance", sizeof(float) * reflectance.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < reflectance.size(); ++i) data[i] = reflectance[i];
+  }
+  if(types & DATA_TEMPERATURE && !temperature.empty()) {
+    float* data = reinterpret_cast<float*>(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i];
+  }
+  if(types & DATA_AMPLITUDE && !amplitude.empty()) {
+    int* data = reinterpret_cast<int*>(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i];
+  }
+  if(types & DATA_TYPE && !type.empty()) {
+    float* data = reinterpret_cast<float*>(create("type", sizeof(double) * type.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < type.size(); ++i) data[i] = type[i];
+  }
+  if(types & DATA_DEVIATION && !deviation.empty()) {
+    float* data = reinterpret_cast<float*>(create("deviation", sizeof(float) * deviation.size()).get_raw_pointer());
+    for(unsigned int i = 0; i < deviation.size(); ++i) data[i] = deviation[i];
+  }
+}
+
+DataPointer BasicScan::get(const std::string& identifier)
+{
+  // try to get data
+  map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
+
+  // create data fields
+  if(it == m_data.end()) {
+    // load from file
+    if(identifier == "xyz") get(DATA_XYZ); else
+    if(identifier == "rgb") get(DATA_RGB); else
+    if(identifier == "reflectance") get(DATA_REFLECTANCE); else
+    if(identifier == "temperature") get(DATA_TEMPERATURE); else
+    if(identifier == "amplitude") get(DATA_AMPLITUDE); else
+    if(identifier == "type") get(DATA_TYPE); else
+    if(identifier == "deviation") get(DATA_DEVIATION); else
+    // reduce on demand
+    if(identifier == "xyz reduced") calcReducedOnDemand(); else
+    if(identifier == "xyz reduced original") calcReducedOnDemand(); else
+    // show requests reduced points, manipulate in showing the same entry
+    if(identifier == "xyz reduced show") {
+      calcReducedOnDemand();
+      m_data["xyz reduced show"] = m_data["xyz reduced"];
+    } else
+    if(identifier == "octtree") {
+      createOcttree();
+    }
+
+    it = m_data.find(identifier);
+  }
+
+    // if nothing can be loaded, return an empty pointer
+  if(it == m_data.end())
+    return DataPointer(0, 0);
+  else
+    return DataPointer(it->second.first, it->second.second);
+}
+
+DataPointer BasicScan::create(const std::string& identifier, unsigned int size)
+{
+  map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
+  if(it != m_data.end()) {
+    // try to reuse, otherwise reallocate
+    if(it->second.second != size) {
+      delete it->second.first;
+      it->second.first = new unsigned char[size];
+      it->second.second = size;
+    }
+  } else {
+    // create a new block of data
+    it = m_data.insert(
+      std::make_pair(
+        identifier,
+        std::make_pair(
+          new unsigned char[size],
+          size
+        )
+      )
+    ).first;
+  }
+  return DataPointer(it->second.first, it->second.second);
+}
+
+void BasicScan::clear(const std::string& identifier)
+{
+  map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
+  if(it != m_data.end()) {
+    delete it->second.first;
+    m_data.erase(it);
+  }
+}
+
+
+void BasicScan::createSearchTreePrivate()
+{
+  DataXYZ xyz_orig(get("xyz reduced original"));
+  PointerArray<double> ar(xyz_orig);
+  switch(searchtree_nnstype)
+  {
+    case simpleKD:
+      kd = new KDtree(ar.get(), xyz_orig.size());
+      break;
+    case ANNTree:
+      kd = new ANNtree(ar, xyz_orig.size());
+      break;
+    case BOCTree:
+      kd = new BOctTree<double>(ar.get(), xyz_orig.size(), 10.0, PointType(), true);
+      break;
+    case -1:
+      throw runtime_error("Cannot create a SearchTree without setting a type.");
+    default:
+      throw runtime_error("SearchTree type not implemented");
+  }
+
+  // TODO: make the switch cases above work with CUDA
+  if (searchtree_cuda_enabled) createANNTree();
+}
+
+void BasicScan::calcReducedOnDemandPrivate()
+{
+  // create reduced points and transform to initial position, save a copy of this for SearchTree
+  calcReducedPoints();
+  transformReduced(transMatOrg);
+  copyReducedToOriginal();
+}
+
+void BasicScan::createANNTree()
+{
+  // TODO: metrics
+#ifdef WITH_CUDA
+  if(!ann_kd_tree) {
+    DataXYZ xyz_orig(get("xyz reduced original"));
+    ann_kd_tree = new ANNkd_tree(PointArray<double>(xyz_orig).get(), xyz_orig.size(), 3, 1, ANN_KD_STD);
+    cout << "Cuda tree was generated with " << xyz_orig.size() << " points" << endl;
+  } else {
+    cout << "Cuda tree exists. No need for another creation" << endl;
+  }
+#endif
+}
+
+void BasicScan::createOcttree()
+{
+  string scanFileName = m_path + "scan" + m_identifier + ".oct";
+  BOctTree<float>* btree = 0;
+
+  // try to load from file, if successful return
+  if(octtree_loadOct && exists(scanFileName)) {
+    btree = new BOctTree<float>(scanFileName);
+    m_data.insert(
+      std::make_pair(
+        "octtree",
+        std::make_pair(
+          reinterpret_cast<unsigned char*>(btree),
+          0 // or memorySize()?
+        )
+      )
+    );
+    return;
+  }
+
+  // create octtree from scan
+  if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points
+    DataXYZ xyz_r(get("xyz reduced show"));
+    btree = new BOctTree<float>(PointerArray<double>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
+  } else { // without reduction, xyz + attribute points
+    float** pts = octtree_pointtype.createPointArray<float>(this);
+    unsigned int nrpts = size<DataXYZ>("xyz");
+    btree = new BOctTree<float>(pts, nrpts, octtree_voxelSize, octtree_pointtype, true);
+    for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts;
+  }
+
+  // save created octtree
+  if(octtree_saveOct) {
+    cout << "Saving octree " << scanFileName << endl;
+    btree->serialize(scanFileName);
+  }
+
+  m_data.insert(
+    std::make_pair(
+      "octtree",
+      std::make_pair(
+        reinterpret_cast<unsigned char*>(btree),
+        0 // or memorySize()?
+      )
+    )
+  );
+}
+
+unsigned int BasicScan::readFrames()
+{
+  string filename = m_path + "scan" + m_identifier + ".frames";
+  ifstream file(filename.c_str());
+  file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
+  try {
+    double transformation[16];
+    unsigned int type;
+    do {
+      file >> transformation >> type;
+      m_frames.push_back(Frame(transformation, type));
+    } while(file.good());
+  } catch(...) {}
+
+  return m_frames.size();
+}
+
+void BasicScan::saveFrames()
+{
+  string filename = m_path + "scan" + m_identifier + ".frames";
+  ofstream file(filename.c_str());
+  for(vector<Frame>::iterator it = m_frames.begin(); it != m_frames.end(); ++it) {
+    file << it->transformation << it->type << '\n';
+  }
+  file << flush;
+  file.close();
+}
+
+unsigned int BasicScan::getFrameCount()
+{
+  return m_frames.size();
+}
+
+void BasicScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type)
+{
+  const Frame& frame(m_frames.at(i));
+  pose_matrix = frame.transformation;
+  type = static_cast<AlgoType>(frame.type);
+}
+
+void BasicScan::addFrame(AlgoType type)
+{
+  m_frames.push_back(Frame(transMat, type));
+}

.svn/pristine/a7/a7bec2140b91dce3babcd38795dea82c82ec1485.svn-base

@@ -0,0 +1,72 @@
+#ifndef BASIC_SCAN_H
+#define BASIC_SCAN_H
+
+#include "scan.h"
+#include "frame.h"
+
+#include <vector>
+#include <map>
+
+
+class BasicScan : public Scan {
+public:
+  BasicScan() {};
+
+  static void openDirectory(const std::string& path, IOType type, int start, int end);
+  static void closeDirectory();
+/*
+  Scan(const double *euler, int maxDist = -1);
+  Scan(const double rPos[3], const double rPosTheta[3], int maxDist = -1);
+  Scan(const double _rPos[3], const double _rPosTheta[3], vector<double *> &pts);
+*/
+  virtual ~BasicScan();
+
+  virtual void setRangeFilter(double max, double min);
+  virtual void setHeightFilter(double top, double bottom);
+  virtual void setRangeMutation(double range);
+
+  virtual const char* getIdentifier() const { return m_identifier.c_str(); }
+
+  virtual DataPointer get(const std::string& identifier);
+  virtual void get(unsigned int types);
+  virtual DataPointer create(const std::string& identifier, unsigned int size);
+  virtual void clear(const std::string& identifier);
+  virtual unsigned int readFrames();
+  virtual void saveFrames();
+  virtual unsigned int getFrameCount();
+  virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type);
+  
+  //! Constructor for creation of Scans without openDirectory
+  BasicScan(double * rPos, double * rPosTheta, std::vector<double*> points);
+
+protected:
+  virtual void createSearchTreePrivate();
+  virtual void calcReducedOnDemandPrivate();
+  virtual void addFrame(AlgoType type);
+
+private:
+  //! Path and identifier of where the scan is located
+  std::string m_path, m_identifier;
+
+  IOType m_type;
+
+  double m_filter_max, m_filter_min, m_filter_top, m_filter_bottom, m_range_mutation;
+  bool m_filter_range_set, m_filter_height_set, m_range_mutation_set;
+
+  std::map<std::string, std::pair<unsigned char*, unsigned int>> m_data;
+
+  std::vector<Frame> m_frames;
+
+
+  //! Constructor for openDirectory
+  BasicScan(const std::string& path, const std::string& identifier, IOType type);
+
+  //! Initialization function
+  void init();
+
+  void createANNTree();
+
+  void createOcttree();
+};
+
+#endif //BASIC_SCAN_H

.svn/pristine/ae/aec7ea9b471ff8f28d6520bc427532b9b0643e02.svn-base

@@ -0,0 +1,682 @@
+/*
+ * caliboard implementation
+ *
+ * Copyright (C) Dorit Borrmann
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file 
+ * @author Dorit Borrmann. Institute of Computer Science, University of Osnabrueck, Germany.
+*/
+
+#include <cfloat>
+#include <fstream>
+#ifndef _MSC_VER
+#include <getopt.h>
+#else
+#include "XGetopt.h"
+#endif
+
+#include <iostream>
+using std::ofstream;
+using std::flush;
+using std::cout;
+using std::string;
+using std::cerr;
+using std::endl;
+#include <errno.h>
+
+#ifdef _MSC_VER
+#define strcasecmp _stricmp
+#define strncasecmp _strnicmp
+#include <windows.h>
+#include <direct.h>
+#else
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <strings.h>
+#include <dlfcn.h>
+#endif
+
+#include "shapes/hough.h"
+#include "slam6d/basicScan.h"
+
+#include "shapes/shape.h"
+#include "shapes/ransac.h"
+#include "slam6d/icp6D.h"
+#include "slam6d/icp6Dsvd.h"
+#include "slam6d/icp6Dquat.h"
+
+#ifdef WITH_SCANSERVER
+#include "scanserver/clientInterface.h"
+#endif
+
+
+
+void usage(char* prog) {
+#ifndef _MSC_VER
+  const string bold("\033[1m");
+  const string normal("\033[m");
+#else
+  const string bold("");
+  const string normal("");
+#endif
+  cout << endl
+	  << bold << "USAGE " << normal << endl
+	  << "   " << prog << " [options] directory" << endl << endl;
+  cout << bold << "OPTIONS" << normal << endl
+	  << bold << "  -b" << normal << " NR, " << bold << "--bottom=" << normal << "NR" << endl
+	  << "         trim the scan with lower boundary NR" << endl
+	  << endl
+	  << bold << "  -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
+	  << "         end after scan NR" << endl
+	  << endl
+
+	  << bold << "  -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
+	  << "         using shared library F for input" << endl
+	  << "         (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl
+	  << endl
+	  << bold << "  -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
+	  << "         neglegt all data points with a distance larger than NR 'units'" << endl
+	  << endl
+	  << bold << "  -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
+	  << "         neglegt all data points with a distance smaller than NR 'units'" << endl
+	  << endl
+	  << bold << "  -p" << normal << " NR, " << bold << "--pattern=" << normal << "NR" << endl
+	  << "         use pattern NR for plane detection" << endl
+	  << "         0: lightbulb pattern" << endl
+	  << "         1: chess pattern on cardboard" << endl
+	  << "         2: chess pattern on wooden board" << endl
+	  << endl
+    << bold << "  -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
+	  << "         turns on octree based point reduction (voxel size=<NR>)" << endl
+	  << endl
+	  << bold << "  -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl
+	  << "         use randomized octree based point reduction (pts per voxel=<NR>)" << endl
+	  << "         requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl
+	  << endl
+	  << bold << "  -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
+	  << "         start at scan NR (i.e., neglects the first NR scans)" << endl
+	  << "         [ATTENTION: counting naturally starts with 0]" << endl
+	  << endl
+	  << bold << "  -t" << normal << " NR, " << bold << "--top=" << normal << "NR" << endl
+	  << "         trim the scan with upper boundary NR" << endl
+	  << endl
+    	  << endl << endl;
+  
+  cout << bold << "EXAMPLES " << normal << endl
+	  << "   " << prog << " -m 500 -r 5 dat" << endl
+	  << "   " << prog << " --max=5000 -r 10.2 dat" << endl
+	  << "   " << prog << " -s 2 -e 10 -r dat" << endl << endl;
+  exit(1);
+
+}
+
+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};
+
+  vector<double *> boardpoints;
+  double halfwidth; 
+  double halfheight; 
+  double w_step = 0.5;
+  double h_step = 0.5;
+
+  switch(pattern) {
+    case 0: 
+      halfheight = 28.5;
+      halfwidth = 25.0;
+      break;
+    case 1:
+      halfwidth = 18.3;
+      halfheight = 18.5;
+      w_step = 0.6;
+      break;
+    case 2:
+    case 3:
+      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) {
+    for(double j = -halfheight; j <= halfheight; j+=h_step) {
+      double * p = new double[3];
+      p[0] = i;
+      p[1] = j;
+      p[2] = 0.0;
+      //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 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;
+  my_icp6Dminimizer = new icp6D_SVD(quiet);
+
+  icp6D *my_icp = 0;
+  double mdm = 50;
+  int mni = 50;
+  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;
+  
+  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, 3.0, sum, centroid_s, centroid_t);  
+  int nr_matches = pairs_out.size();
+  
+  cout << "Result " << nr_matches << " " << nr_points << " " << nr_points2 << endl;
+  const double * pos = board->get_rPos();
+  const double * postheta = board->get_rPosTheta();
+  const double * transMat = board->get_transMat();
+  for(int i = 0; i < 16; i++) {
+    cout << transMat[i] << " ";
+  }
+
+  cout << endl << endl;
+  cout << "Transform new: " << endl; 
+  for(int i = 0; i < 3; i++) {
+    cout << pos[i] << " ";
+  }
+  cout << endl;
+  for(int i = 0; i < 3; i++) {
+    cout << deg(postheta[i]) << " ";
+  }
+  cout << endl;
+  cout << "Calipoints Start" << endl;
+  
+  ofstream caliout(output.c_str());
+  
+  if(nr_matches < nr_points) {
+    caliout << "failed" << endl;
+  } else {
+    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) {
+        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;
+  // chessboard on wooden board pattern top
+  case 2:
+    for(double x = -7.8; x < 10; x+=5.2) {
+      for(double y = 4.1; y < 33.0; y+=5.2) {
+        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;
+  // chessboard on wooden board pattern bottom 
+  case 3:
+    for(double y = -4.1; y > -33.0; y-=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);
+        caliout << p[0] << " " << p[1] << " " << p[2] << endl;
+        delete[] p;
+      }
+    }
+    break;
+  // chessboard on cardboard
+  case 1:
+    for(double x = -12; x < 16; x+=4.0) {
+      for(double y = -12; y < 16; y+=4.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;
+    // 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) {
+
+  bool reduced = false;
+  int  c;
+  extern char *optarg;
+  extern int optind;
+
+  /* options descriptor */
+  // 0: no arguments, 1: required argument, 2: optional argument
+  static struct option longopts[] = {
+    { "format",          required_argument,   0,  'f' },  
+    { "max",             required_argument,   0,  'm' },
+    { "min",             required_argument,   0,  'M' },
+    { "start",           required_argument,   0,  's' },
+    { "reduce",          required_argument,   0,  'r' },
+    { "pattern",         required_argument,   0,  'p' },
+    { "quiet",           no_argument,         0,  'q' },
+    { "octree",          optional_argument,   0,  'O' },
+    { "end",             required_argument,   0,  'e' },
+    { "top",             required_argument,   0,  't' },
+    { "bottom",          required_argument,   0,  'b' },
+    { 0,           0,   0,   0}                    // needed, cf. getopt.h
+  };
+
+  cout << endl;
+  while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:O:qp:e:t:b:", longopts, NULL)) != -1) 
+  switch (c)
+	 {
+	 case 'r':
+     red = atof(optarg);
+     reduced = true;
+	   break;
+	 case 's':
+	   start = atoi(optarg);
+	   if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
+	   break;
+	 case 'e':
+	   end = atoi(optarg);
+	   if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
+	   if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
+	   break;
+	 case 'f':
+     try {
+       type = formatname_to_io_type(optarg);
+     } catch (...) { // runtime_error
+       cerr << "Format " << optarg << " unknown." << endl;
+       abort();
+     }
+     break;
+   case 'p':
+      pattern = atoi(optarg);
+      if(pattern < 0 || pattern > 5) { cerr << "Error: choose pattern between 0 and 3!\n"; exit(1); }
+      break;
+	 case 'q':
+     quiet = true;
+     break;
+   case 'm':
+	   maxDist = atoi(optarg);
+	   break;
+	 case 'O':
+     if (optarg) {
+       octree = atoi(optarg);
+     } else {
+       octree = 1;
+     }
+	   break;
+	 case 't':
+     top = atof(optarg);
+	   break;
+	 case 'b':
+	   bottom = atof(optarg);
+     break;
+	 case 'M':
+	   minDist = atoi(optarg);
+	   break;
+   case '?':
+	   usage(argv[0]);
+	   return 1;
+   default:
+	    abort ();
+      break;
+   }
+
+  if (optind != argc-1) {
+    cerr << "\n*** Directory missing ***" << endl;
+    usage(argv[0]);
+  }
+  dir = argv[optind];
+
+#ifndef _MSC_VER
+  if (dir[dir.length()-1] != '/') dir = dir + "/";
+#else
+  if (dir[dir.length()-1] != '\\') dir = dir + "\\";
+#endif
+
+  return 0;
+}
+
+/**
+ * Main function. The Hough Transform is called for the scan indicated as
+ * argument.
+ *
+ */
+int main(int argc, char **argv) 
+{
+
+  cout << "(c) Jacobs University Bremen, gGmbH, 2010" << endl << endl;
+
+  if (argc <= 1) {
+    usage(argv[0]);
+  }
+  // parsing the command line parameters
+  // init, default values if not specified
+  string dir;
+  double red   = -1.0;
+  int    start = 0;
+  int    end = -1;
+  int    maxDist    = -1;
+  int    minDist    = -1;
+  int    octree     = 0;
+  bool   quiet = false;
+  int    pattern = 0;
+  double bottom = -5;
+  double top = 170;
+  IOType type    = UOS;
+
+  cout << "Parse args" << endl;
+  parseArgs(argc, argv, dir, red, start, end, pattern, maxDist, minDist, top, bottom, octree, type, quiet);
+  int fileNr = start;
+  string calidir = dir + "/cali";
+  
+#ifdef WITH_SCANSERVER
+  try {
+    ClientInterface::create();
+  } catch(std::runtime_error& e) {
+    cerr << "ClientInterface could not be created: " << e.what() << endl;
+    cerr << "Start the scanserver first." << endl;
+    exit(-1);
+  }
+#endif //WITH_SCANSERVER
+
+#ifdef _MSC_VER
+  int success = mkdir(calidir.c_str());
+#else
+  int success = mkdir(calidir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
+#endif
+  if(success == 0) {
+    if(!quiet) {
+      cout << "Writing calibration results to " << calidir << endl;
+    }
+  } else if(errno == EEXIST) {
+    cout << "Directory " << calidir << " exists already.  CONTINUE" << endl;
+  } else {
+    cerr << "Creating directory " << calidir << " failed" << endl;
+    exit(1);
+  }
+
+  cout << start << " " << end << endl;
+  int successes = 0;
+  int failures = 0;
+  
+  long calitime = GetCurrentTimeInMilliSec();
+
+//#ifndef WITH_SCANSERVER
+  while (fileNr <= end) {
+    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]->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)
+  {
+    Scan* scan = *it;
+    string output = calidir + "/scan" + scan->getIdentifier() + ".3d";
+    cout << "Top: " << top << " Bottom: " << bottom << endl;
+    // set trimming, don't want to put it into readScansRedSearch too
+    scan->trim(top, bottom);
+
+    double id[16];
+    M4identity(id);
+    for(int i = 0; i < 10; i++) {
+      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
+    Ransac(*plane, Scan::allScans[0], &points);
+/*
+#else //WITH_SCANSERVER
+
+    cout << "S" << endl;
+    Ransac(*plane, scan, &points);
+    cout << "T" << endl;
+//#endif //WITH_SCANSERVER
+*/
+    starttime = (GetCurrentTimeInMilliSec() - starttime);
+
+    cout << "nr points " << points.size() << endl;
+    double nx,ny,nz,d;
+    plane->getPlane(nx,ny,nz,d);
+    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)) {
+        successes++;
+      } else {
+        failures++;
+      }
+
+    }
+   
+    
+    for(int i = points.size() - 1; i > -1; i--) {
+      delete[] points[i];
+    }
+    
+    
+    delete plane;
+
+    cout << "Time for Plane Detection " << starttime << endl;
+//#ifndef WITH_SCANSERVER
+    delete Scan::allScans[0];
+    Scan::allScans.clear();
+    fileNr++;
+//#endif //WITH_SCANSERVER
+  }
+  calitime = (GetCurrentTimeInMilliSec() - calitime);
+
+  cout << "Calibration done with " << successes << " successes and " << failures
+  << " failures!" << endl;
+  cout << "Time for Calibration " << calitime << endl;
+
+/*
+#ifdef WITH_SCANSERVER
+  Scan::clearScans();
+  ClientInterface::destroy();
+#endif //WITH_SCANSERVER
+*/
+}
+

.svn/pristine/ae/aeea7442cea67a8152af68ec023a043e48af2f16.svn-base

@@ -0,0 +1,847 @@
+/*
+ * viewcull implementation
+ *
+ * Copyright (C) Jan Elseberg
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file
+ * @brief Frustrum culling routines
+ * 
+ * @author Jan Elseberg. Automation Group, Jacobs University Bremen gGmbH, Germany. 
+ */
+#include "show/viewcull.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+#ifdef __APPLE__
+#include <GLUT/glut.h>
+#include <OpenGL/glu.h>
+#else
+#include <GL/glut.h>
+#include <GL/glu.h>
+#endif
+
+#include "slam6d/globals.icc"
+namespace show{
+
+/** The 6 planes of the viewing frustum */
+float frustum[6][4];
+
+/** the modelview * projection matrix to map a model point to an onscreen coordinate */
+float matrix[16];
+/** some useful variables for faster calculation of the pixel coordinates */
+short VP[4];
+/** a unit vector pointing to the right of the screen */
+float right[3];
+/** how much detail is shown, 0 means everything is plotted */
+short DETAIL;
+
+double SX, SY, SZ, EX, EY, EZ;
+float origin[3], dir[3];   /*ray */
+float dist;
+int rayX,rayY;
+float  rayVP[4];
+
+void calcRay(int x, int y, double znear, double zfar)  {
+
+  GLdouble modelMatrix[16];
+  GLdouble projMatrix[16];
+  int viewport[4];
+  glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix);
+  glGetDoublev(GL_PROJECTION_MATRIX,projMatrix);
+  glGetIntegerv(GL_VIEWPORT,viewport);
+
+  gluUnProject(x, viewport[3]-y, zfar, modelMatrix, projMatrix, viewport, &SX, &SY, &SZ);
+  gluUnProject(x, viewport[3]-y, znear, modelMatrix, projMatrix, viewport, &EX, &EY, &EZ);
+
+  origin[0] = SX;
+  origin[1] = SY;
+  origin[2] = SZ;
+  dir[0] = EX-SX;
+  dir[1] = EY-SY;
+  dir[2] = EZ-SZ;
+  double t = sqrt( dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2] );
+  dir[0] /= t;
+  dir[1] /= t;
+  dir[2] /= t;
+
+  dist = SX * dir[0] + SY * dir[1] + SZ * dir[2];
+  rayVP[0] = 0.5*viewport[2];
+  rayVP[1] = 0.5*viewport[2] + viewport[0];
+
+  rayVP[2] = 0.5*viewport[3];
+  rayVP[3] = 0.5*viewport[3] + viewport[1];
+  rayX = x;
+  rayY = viewport[3]-y;
+
+  remViewport();
+}
+
+void remViewport() {
+  GLdouble modelMatrix[16];
+  GLdouble projMatrix[16];
+  int viewport[4];
+  glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix);
+  glGetDoublev(GL_PROJECTION_MATRIX,projMatrix);
+  glGetIntegerv(GL_VIEWPORT,viewport);
+
+  MMult( projMatrix, modelMatrix, matrix );
+  VP[0] = 0.5*viewport[2];
+  VP[1] = 0.5*viewport[2] + viewport[0];
+
+  VP[2] = 0.5*viewport[3];
+  VP[3] = 0.5*viewport[3] + viewport[1];
+
+  right[0] = modelMatrix[0];
+  right[1] = modelMatrix[4];
+  right[2] = modelMatrix[8];
+}
+
+void ExtractFrustum(short detail)
+{
+   DETAIL = detail + 1;
+   remViewport();
+
+   float   proj[16];
+   float   modl[16];
+   float   clip[16];
+   float   t;
+
+   /* Get the current PROJECTION matrix from OpenGL */
+   glGetFloatv( GL_PROJECTION_MATRIX, proj );
+
+   /* Get the current MODELVIEW matrix from OpenGL */
+   glGetFloatv( GL_MODELVIEW_MATRIX, modl );
+
+   /* Combine the two matrices (multiply projection by modelview) */
+   clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12];
+   clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13];
+   clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14];
+   clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15];
+
+   clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12];
+   clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13];
+   clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14];
+   clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15];
+
+   clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12];
+   clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13];
+   clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14];
+   clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15];
+
+   clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12];
+   clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13];
+   clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14];
+   clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15];
+
+   /* Extract the numbers for the RIGHT plane */
+   frustum[0][0] = clip[ 3] - clip[ 0];
+   frustum[0][1] = clip[ 7] - clip[ 4];
+   frustum[0][2] = clip[11] - clip[ 8];
+   frustum[0][3] = clip[15] - clip[12];
+
+   /* Normalize the result */
+   t = sqrt( frustum[0][0] * frustum[0][0] + frustum[0][1] * frustum[0][1] + frustum[0][2] * frustum[0][2] );
+   frustum[0][0] /= t;
+   frustum[0][1] /= t;
+   frustum[0][2] /= t;
+   frustum[0][3] /= t;
+
+   /* Extract the numbers for the LEFT plane */
+   frustum[1][0] = clip[ 3] + clip[ 0];
+   frustum[1][1] = clip[ 7] + clip[ 4];
+   frustum[1][2] = clip[11] + clip[ 8];
+   frustum[1][3] = clip[15] + clip[12];
+
+   /* Normalize the result */
+   t = sqrt( frustum[1][0] * frustum[1][0] + frustum[1][1] * frustum[1][1] + frustum[1][2] * frustum[1][2] );
+   frustum[1][0] /= t;
+   frustum[1][1] /= t;
+   frustum[1][2] /= t;
+   frustum[1][3] /= t;
+
+   /* Extract the BOTTOM plane */
+   frustum[2][0] = clip[ 3] + clip[ 1];
+   frustum[2][1] = clip[ 7] + clip[ 5];
+   frustum[2][2] = clip[11] + clip[ 9];
+   frustum[2][3] = clip[15] + clip[13];
+
+   /* Normalize the result */
+   t = sqrt( frustum[2][0] * frustum[2][0] + frustum[2][1] * frustum[2][1] + frustum[2][2] * frustum[2][2] );
+   frustum[2][0] /= t;
+   frustum[2][1] /= t;
+   frustum[2][2] /= t;
+   frustum[2][3] /= t;
+
+   /* Extract the TOP plane */
+   frustum[3][0] = clip[ 3] - clip[ 1];
+   frustum[3][1] = clip[ 7] - clip[ 5];
+   frustum[3][2] = clip[11] - clip[ 9];
+   frustum[3][3] = clip[15] - clip[13];
+
+   /* Normalize the result */
+   t = sqrt( frustum[3][0] * frustum[3][0] + frustum[3][1] * frustum[3][1] + frustum[3][2] * frustum[3][2] );
+   frustum[3][0] /= t;
+   frustum[3][1] /= t;
+   frustum[3][2] /= t;
+   frustum[3][3] /= t;
+
+   /* Extract the FAR plane */
+   frustum[4][0] = clip[ 3] - clip[ 2];
+   frustum[4][1] = clip[ 7] - clip[ 6];
+   frustum[4][2] = clip[11] - clip[10];
+   frustum[4][3] = clip[15] - clip[14];
+
+   /* Normalize the result */
+   t = sqrt( frustum[4][0] * frustum[4][0] + frustum[4][1] * frustum[4][1] + frustum[4][2] * frustum[4][2] );
+   frustum[4][0] /= t;
+   frustum[4][1] /= t;
+   frustum[4][2] /= t;
+   frustum[4][3] /= t;
+
+   /* Extract the NEAR plane */
+   frustum[5][0] = clip[ 3] + clip[ 2];
+   frustum[5][1] = clip[ 7] + clip[ 6];
+   frustum[5][2] = clip[11] + clip[10];
+   frustum[5][3] = clip[15] + clip[14];
+
+   /* Normalize the result */
+   t = sqrt( frustum[5][0] * frustum[5][0] + frustum[5][1] * frustum[5][1] + frustum[5][2] * frustum[5][2] );
+   frustum[5][0] /= t;
+   frustum[5][1] /= t;
+   frustum[5][2] /= t;
+   frustum[5][3] /= t;
+
+}
+
+
+void ExtractFrustum(float *frust[6])
+{
+   remViewport();
+
+   float   proj[16];
+   float   modl[16];
+   float   clip[16];
+   float   t;
+
+   /* Get the current PROJECTION matrix from OpenGL */
+   glGetFloatv( GL_PROJECTION_MATRIX, proj );
+
+   /* Get the current MODELVIEW matrix from OpenGL */
+   glGetFloatv( GL_MODELVIEW_MATRIX, modl );
+
+   /* Combine the two matrices (multiply projection by modelview) */
+   clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12];
+   clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13];
+   clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14];
+   clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15];
+
+   clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12];
+   clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13];
+   clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14];
+   clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15];
+
+   clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12];
+   clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13];
+   clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14];
+   clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15];
+
+   clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12];
+   clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13];
+   clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14];
+   clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15];
+
+   /* Extract the numbers for the RIGHT plane */
+   frust[0][0] = clip[ 3] - clip[ 0];
+   frust[0][1] = clip[ 7] - clip[ 4];
+   frust[0][2] = clip[11] - clip[ 8];
+   frust[0][3] = clip[15] - clip[12];
+
+   /* Normalize the result */
+   t = sqrt( frust[0][0] * frust[0][0] + frust[0][1] * frust[0][1] + frust[0][2] * frust[0][2] );
+   frust[0][0] /= t;
+   frust[0][1] /= t;
+   frust[0][2] /= t;
+   frust[0][3] /= t;
+
+   /* Extract the numbers for the LEFT plane */
+   frust[1][0] = clip[ 3] + clip[ 0];
+   frust[1][1] = clip[ 7] + clip[ 4];
+   frust[1][2] = clip[11] + clip[ 8];
+   frust[1][3] = clip[15] + clip[12];
+
+   /* Normalize the result */
+   t = sqrt( frust[1][0] * frust[1][0] + frust[1][1] * frust[1][1] + frust[1][2] * frust[1][2] );
+   frust[1][0] /= t;
+   frust[1][1] /= t;
+   frust[1][2] /= t;
+   frust[1][3] /= t;
+
+   /* Extract the BOTTOM plane */
+   frust[2][0] = clip[ 3] + clip[ 1];
+   frust[2][1] = clip[ 7] + clip[ 5];
+   frust[2][2] = clip[11] + clip[ 9];
+   frust[2][3] = clip[15] + clip[13];
+
+   /* Normalize the result */
+   t = sqrt( frust[2][0] * frust[2][0] + frust[2][1] * frust[2][1] + frust[2][2] * frust[2][2] );
+   frust[2][0] /= t;
+   frust[2][1] /= t;
+   frust[2][2] /= t;
+   frust[2][3] /= t;
+
+   /* Extract the TOP plane */
+   frust[3][0] = clip[ 3] - clip[ 1];
+   frust[3][1] = clip[ 7] - clip[ 5];
+   frust[3][2] = clip[11] - clip[ 9];
+   frust[3][3] = clip[15] - clip[13];
+
+   /* Normalize the result */
+   t = sqrt( frust[3][0] * frust[3][0] + frust[3][1] * frust[3][1] + frust[3][2] * frust[3][2] );
+   frust[3][0] /= t;
+   frust[3][1] /= t;
+   frust[3][2] /= t;
+   frust[3][3] /= t;
+
+   /* Extract the FAR plane */
+   frust[4][0] = clip[ 3] - clip[ 2];
+   frust[4][1] = clip[ 7] - clip[ 6];
+   frust[4][2] = clip[11] - clip[10];
+   frust[4][3] = clip[15] - clip[14];
+
+   /* Normalize the result */
+   t = sqrt( frust[4][0] * frust[4][0] + frust[4][1] * frust[4][1] + frust[4][2] * frust[4][2] );
+   frust[4][0] /= t;
+   frust[4][1] /= t;
+   frust[4][2] /= t;
+   frust[4][3] /= t;
+
+   /* Extract the NEAR plane */
+   frust[5][0] = clip[ 3] + clip[ 2];
+   frust[5][1] = clip[ 7] + clip[ 6];
+   frust[5][2] = clip[11] + clip[10];
+   frust[5][3] = clip[15] + clip[14];
+
+   /* Normalize the result */
+   t = sqrt( frust[5][0] * frust[5][0] + frust[5][1] * frust[5][1] + frust[5][2] * frust[5][2] );
+   frust[5][0] /= t;
+   frust[5][1] /= t;
+   frust[5][2] /= t;
+   frust[5][3] /= t;
+
+}
+
+
+
+void myProject(float x, float y, float z, short &Xi ) {
+  float pn[2];
+  // x coordinate on screen, not normalized
+  pn[0] = x * matrix[0] + y * matrix[4] + z * matrix[8]  + matrix[12];
+  // normalization
+  pn[1] = x * matrix[3] + y * matrix[7] + z * matrix[11] + matrix[15];
+
+  // normalized x coordinate on screen
+  pn[0] /= pn[1];
+
+  // true x coordinate in viewport coordinate system
+  Xi = pn[0]*VP[0] + VP[1];
+}
+
+int LOD2(float x, float y, float z, float size)
+{
+  size = sqrt(3*size*size);
+
+  short X1;
+  short X2;
+ 
+  // onscreen position of the leftmost point
+  myProject(x - size*right[0], y - size*right[1], z - size*right[2], X1);
+  // onscreen position of the rightmost point
+  myProject(x + size*right[0], y + size*right[1], z + size*right[2], X2);
+
+  if (X1 > X2) {
+    return (X1-X2);
+  } else {
+    return (X2-X1);
+  }
+}
+
+bool LOD(float x, float y, float z, float size)
+{
+  size = sqrt(3*size*size);
+
+  short X1;
+  short X2;
+ 
+  // onscreen position of the leftmost point
+  myProject(x - size*right[0], y - size*right[1], z - size*right[2], X1);
+  // onscreen position of the rightmost point
+  myProject(x + size*right[0], y + size*right[1], z + size*right[2], X2);
+
+  if (X1 > X2) {
+    return (X1-X2) > DETAIL;
+  } else {
+    return (X2-X1) > DETAIL;
+  }
+}
+
+
+/**
+ * 0 if not in frustrum
+ * 1 if partial overlap
+ * 2 if entirely within frustrum
+ */
+int CubeInFrustum2( float x, float y, float z, float size )
+{
+   int p;
+
+   float xm, xp, ym, yp, zm, zp;
+   float Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
+   xm = x - size;
+   xp = x + size;
+   ym = y - size;
+   yp = y + size;
+   zm = z - size;
+   zp = z + size;
+
+   for( p = 0; p < 6; p++ )
+   {
+      Fxm = frustum[p][0] * xm;
+      Fym = frustum[p][1] * ym;
+      Fzm = frustum[p][2] * zm;
+      if( Fxm + Fym + Fzm + frustum[p][3] > 0 )
+         continue;
+
+      Fxp = frustum[p][0] * xp;
+      if( Fxp + Fym + Fzm + frustum[p][3] > 0 )
+         continue;
+
+      Fyp = frustum[p][1] * yp;
+      if( Fxm + Fyp + Fzm + frustum[p][3] > 0 )
+         continue;
+
+      if( Fxp + Fyp + Fzm + frustum[p][3] > 0 )
+         continue;
+      
+      Fzp = frustum[p][2] * zp;
+      if( Fxm + Fym + Fzp + frustum[p][3] > 0 )
+         continue;
+
+      if( Fxp + Fym + Fzp + frustum[p][3] > 0 )
+         continue;
+      if( Fxm + Fyp + Fzp + frustum[p][3] > 0 )
+         continue;
+      if( Fxp + Fyp + Fzp + frustum[p][3] > 0 )
+         continue;
+      return 0;
+   }
+
+   // box is in frustrum, now check wether all corners are within.
+   // If one is outside return 1 otherwise 2
+   for( p = 0; p < 6; p++ )
+   {
+      Fxm = frustum[p][0] * xm;
+      Fym = frustum[p][1] * ym;
+      Fzm = frustum[p][2] * zm;
+      if( Fxm + Fym + Fzm + frustum[p][3] < 0 )
+         return 1;
+      
+      Fxp = frustum[p][0] * xp;
+      if( Fxp + Fym + Fzm + frustum[p][3] < 0 )
+         return 1;
+      
+      Fyp = frustum[p][1] * yp;
+      if( Fxm + Fyp + Fzm + frustum[p][3] < 0 )
+         return 1;
+
+      if( Fxp + Fyp + Fzm + frustum[p][3] < 0 )
+         return 1;
+
+      Fzp = frustum[p][2] * zp;
+      if( Fxm + Fym + Fzp + frustum[p][3] < 0 )
+         return 1;
+      
+      if( Fxp + Fym + Fzp + frustum[p][3] < 0 )
+         return 1;
+      if( Fxm + Fyp + Fzp + frustum[p][3] < 0 )
+         return 1;
+      if( Fxp + Fyp + Fzp + frustum[p][3] < 0 )
+         return 1;
+   }
+   return 2;
+
+}
+
+/*
+char PlaneAABB( float x, float y, float z, float size, float *plane )
+{
+   float dist = plane[3];
+
+   float xm, xp, ym, yp, zm, zp;
+   float Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
+
+   float dmmm, dpmm, dmpm, dppm, dmmp, dpmp, dmpp, dppp;
+   xm = x - size;
+   xp = x + size;
+   ym = y - size;
+   yp = y + size;
+   zm = z - size;
+   zp = z + size;
+
+   {
+      Fxm = plane[0] * xm;
+      Fym = plane[1] * ym;
+      Fzm = plane[2] * zm;
+      dmmm = Fxm + Fym + Fzm + dist;
+      if( dmmm > 0 )
+         goto intersects;
+
+      Fxp = plane[0] * xp;
+      dpmm = Fxp + Fym + Fzm + dist;
+      if( dpmm > 0 )
+         goto intersects;
+
+      Fyp = plane[1] * yp;
+      dmpm = Fxm + Fyp + Fzm + dist;
+      if( dmpm > 0 )
+         goto intersects;
+
+      dppm = Fxp + Fyp + Fzm + dist;
+      if( dppm > 0 )
+         goto intersects;
+      
+      Fzp = plane[2] * zp;
+      dmmp = Fxm + Fym + Fzp + dist;
+      if( dmmp > 0 )
+         goto intersects;
+
+      dpmp = Fxp + Fym + Fzp + dist;
+      if( dpmp > 0 )
+         goto intersects;
+      dmpp = Fxm + Fyp + Fzp + dist;
+      if( dmpp > 0 )
+         goto intersects;
+      dppp = Fxp + Fyp + Fzp + dist;
+      if( dppp > 0 )
+         goto intersects;
+      return 0; // outside
+   }
+
+   intersects:
+
+   // cube is inside, determine if plane intersects the cube
+   {
+      if( dmmm < 0 )
+         return 1;
+      Fxp = plane[0] * xp;
+      dpmm = Fxp + Fym + Fzm + dist;
+      if( dpmm < 0 )
+         return 1;
+      Fyp = plane[1] * yp;
+      dmpm = Fxm + Fyp + Fzm + dist;
+      if( dmpm < 0 )
+         return 1;
+      dppm = Fxp + Fyp + Fzm + dist;
+      if( dppm < 0 )
+         return 1;
+      Fzp = plane[2] * zp;
+      dmmp = Fxm + Fym + Fzp + dist;
+      if( dmmp < 0 )
+         return 1;
+      dpmp = Fxp + Fym + Fzp + dist;
+      if( dpmp < 0 )
+         return 1;
+      dmpp = Fxm + Fyp + Fzp + dist;
+      if( dmpp < 0 )
+         return 1;
+      dppp = Fxp + Fyp + Fzp + dist;
+      if( dppp < 0 )
+         return 1;
+   }
+
+   // plane is outside
+   return 2;
+
+}
+*/
+char PlaneAABB( float x, float y, float z, float size, float *plane )
+{
+   float dist = plane[3];
+
+   float xm, xp, ym, yp, zm, zp;
+   float Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
+
+   float dmmm, dpmm, dmpm, dppm, dmmp, dpmp, dmpp, dppp;
+   xm = x - size;
+   xp = x + size;
+   ym = y - size;
+   yp = y + size;
+   zm = z - size;
+   zp = z + size;
+
+   Fxm = plane[0] * xm;
+   Fym = plane[1] * ym;
+   Fzm = plane[2] * zm;
+   dmmm = Fxm + Fym + Fzm + dist;
+   if( dmmm > 0 )
+   {
+     Fxp = plane[0] * xp;
+     dpmm = Fxp + Fym + Fzm + dist;
+     if( dpmm < 0 )
+       return 1;
+     Fyp = plane[1] * yp;
+     dmpm = Fxm + Fyp + Fzm + dist;
+     if( dmpm < 0 )
+       return 1;
+     dppm = Fxp + Fyp + Fzm + dist;
+     if( dppm < 0 )
+       return 1;
+     Fzp = plane[2] * zp;
+     dmmp = Fxm + Fym + Fzp + dist;
+     if( dmmp < 0 )
+       return 1;
+     dpmp = Fxp + Fym + Fzp + dist;
+     if( dpmp < 0 )
+       return 1;
+     dmpp = Fxm + Fyp + Fzp + dist;
+     if( dmpp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+
+   Fxp = plane[0] * xp;
+   dpmm = Fxp + Fym + Fzm + dist;
+   if( dpmm > 0 )
+   {
+     if( dmmm < 0 )
+       return 1;
+     Fyp = plane[1] * yp;
+     dmpm = Fxm + Fyp + Fzm + dist;
+     if( dmpm < 0 )
+       return 1;
+     dppm = Fxp + Fyp + Fzm + dist;
+     if( dppm < 0 )
+       return 1;
+     Fzp = plane[2] * zp;
+     dmmp = Fxm + Fym + Fzp + dist;
+     if( dmmp < 0 )
+       return 1;
+     dpmp = Fxp + Fym + Fzp + dist;
+     if( dpmp < 0 )
+       return 1;
+     dmpp = Fxm + Fyp + Fzp + dist;
+     if( dmpp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+
+   Fyp = plane[1] * yp;
+   dmpm = Fxm + Fyp + Fzm + dist;
+   if( dmpm > 0 )
+   {
+     if( dmmm < 0 ||  dpmm < 0 )
+       return 1;
+     dppm = Fxp + Fyp + Fzm + dist;
+     if( dppm < 0 )
+       return 1;
+     Fzp = plane[2] * zp;
+     dmmp = Fxm + Fym + Fzp + dist;
+     if( dmmp < 0 )
+       return 1;
+     dpmp = Fxp + Fym + Fzp + dist;
+     if( dpmp < 0 )
+       return 1;
+     dmpp = Fxm + Fyp + Fzp + dist;
+     if( dmpp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+
+   dppm = Fxp + Fyp + Fzm + dist;
+   if( dppm > 0 )
+   {
+     if( dmmm < 0 ||  dpmm < 0 || dmpm < 0 )
+       return 1;
+     Fzp = plane[2] * zp;
+     dmmp = Fxm + Fym + Fzp + dist;
+     if( dmmp < 0 )
+       return 1;
+     dpmp = Fxp + Fym + Fzp + dist;
+     if( dpmp < 0 )
+       return 1;
+     dmpp = Fxm + Fyp + Fzp + dist;
+     if( dmpp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+
+   Fzp = plane[2] * zp;
+   dmmp = Fxm + Fym + Fzp + dist;
+   if( dmmp > 0 )
+   {
+     if( dmmm < 0 ||  dpmm < 0 || dmpm < 0 || dppm < 0 )
+       return 1;
+     dpmp = Fxp + Fym + Fzp + dist;
+     if( dpmp < 0 )
+       return 1;
+     dmpp = Fxm + Fyp + Fzp + dist;
+     if( dmpp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+
+   dpmp = Fxp + Fym + Fzp + dist;
+   if( dpmp > 0 )
+   {
+     if( dmmm < 0 ||  dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 )
+       return 1;
+     dmpp = Fxm + Fyp + Fzp + dist;
+     if( dmpp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+   dmpp = Fxm + Fyp + Fzp + dist;
+   if( dmpp > 0 )
+   {
+     if( dmmm < 0 ||  dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 || dpmp < 0 )
+       return 1;
+     dppp = Fxp + Fyp + Fzp + dist;
+     if( dppp < 0 )
+       return 1;
+     return 2;
+   }
+   dppp = Fxp + Fyp + Fzp + dist;
+   if( dppp > 0 )
+   {
+     if( dmmm < 0 ||  dpmm < 0 || dmpm < 0 || dppm < 0 || dmmp < 0 || dpmp < 0 || dmpp < 0)
+       return 1;
+     return 2;
+   }
+   return 0; // outside
+}
+
+int QuadInFrustrum2old( float x, float y, float z, float size )
+{
+   int p;
+
+   for( p = 0; p < 6; p++ )
+   {
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 )
+         continue;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] > 0 )
+         continue;
+      return 0;
+   }
+
+   // box is in frustrum, now check wether all corners are within. If one is outside return 1 otherwise 2
+   for( p = 0; p < 6; p++ )
+   {
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z - size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y - size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x - size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 )
+         return 1;
+      if( frustum[p][0] * (x + size) + frustum[p][1] * (y + size) + frustum[p][2] * (z + size) + frustum[p][3] < 0 )
+         return 1;
+   }
+   return 2;
+
+}
+
+bool CubeInFrustum( float x, float y, float z, float size )
+{
+   int p;
+
+   float xm, xp, ym, yp, zm, zp;
+   float Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
+   xm = x - size;
+   xp = x + size;
+   ym = y - size;
+   yp = y + size;
+   zm = z - size;
+   zp = z + size;
+
+   for( p = 0; p < 6; p++ )
+   {
+      Fxm = frustum[p][0] * xm;
+      Fym = frustum[p][1] * ym;
+      Fzm = frustum[p][2] * zm;
+      if( Fxm + Fym + Fzm + frustum[p][3] > 0 )
+         continue;
+
+      Fxp = frustum[p][0] * xp;
+      if( Fxp + Fym + Fzm + frustum[p][3] > 0 )
+         continue;
+
+      Fyp = frustum[p][1] * yp;
+      if( Fxm + Fyp + Fzm + frustum[p][3] > 0 )
+         continue;
+
+      if( Fxp + Fyp + Fzm + frustum[p][3] > 0 )
+         continue;
+      
+      Fzp = frustum[p][2] * zp;
+      if( Fxm + Fym + Fzp + frustum[p][3] > 0 )
+         continue;
+
+      if( Fxp + Fym + Fzp + frustum[p][3] > 0 )
+         continue;
+      if( Fxm + Fyp + Fzp + frustum[p][3] > 0 )
+         continue;
+      if( Fxp + Fyp + Fzp + frustum[p][3] > 0 )
+         continue;
+      return false;
+   }
+   return true;
+}
+
+
+
+
+float minB[NUMDIM], maxB[NUMDIM];    /*box */
+float coord[NUMDIM];       /* hit point */
+}

.svn/pristine/b0/b056f289f2ded3cf1ca7db3f73dd6418bffe3a19.svn-base

@@ -0,0 +1,238 @@
+/** @file 
+ *  @brief Representation of the optimized k-d tree.
+ *  @author Remus Dumitru. Jacobs University Bremen, Germany
+ *  @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+
+#ifndef __KD_TREE_IMPL_H__
+#define __KD_TREE_IMPL_H__
+
+#include "slam6d/kdparams.h"
+#include "globals.icc"
+
+#ifdef _MSC_VER
+#if !defined _OPENMP && defined OPENMP 
+#define _OPENMP
+#endif
+#endif
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+/**
+ * @brief The optimized k-d tree. 
+ * 
+ * A kD tree for points, with limited
+ * capabilities (find nearest point to
+ * a given point, or to a ray).
+ **/
+template<class PointData, class AccessorData, class AccessorFunc>
+class KDTreeImpl {
+public:
+  inline KDTreeImpl() { }
+  
+  virtual inline ~KDTreeImpl() {
+    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;
+    }
+  }
+
+  virtual void create(PointData pts, AccessorData *indices, size_t n) {
+    AccessorFunc point;
+    
+    // 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 AccessorData[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 AccessorData[n];
+      // fill leaf index array with indices
+      for(unsigned int i = 0; i < n; ++i) {
+        leaf.p[i] = indices[i];
+      }
+      return;
+    }
+
+    AccessorData* 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;
+      std::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 KDTreeImpl();
+        node.child1->create(pts, indices, left - indices);
+      }
+      if (i == 1) {
+        node.child2 = new KDTreeImpl();
+        node.child2->create(pts, left, n - (left - indices));
+      }
+    }
+  }
+
+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
+      KDTreeImpl *child1;  ///< pointers to the childs
+      KDTreeImpl *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
+       */
+      AccessorData* p;
+    } leaf;
+  };
+
+  void _FindClosest(const PointData& pts, int threadNum) const {
+    AccessorFunc point;
+
+    // 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);
+      }
+    }
+  }
+};
+
+#endif

.svn/pristine/c4/c460c133ba1883f7187e9dde77510e05e4b94939.svn-base

@@ -0,0 +1,66 @@
+/** @file 
+ *  @brief Encapsules the implementation of ANN k-d trees. 
+ *  @author Ulugbek Makhmudov, Jacobs University Bremen, Bremen, Germany.
+ *  @author Andreas Nuechter, Jacobs University Bremen, Bremen, Germany.
+ */
+
+#ifndef __ANN_KD_H__
+#define __ANN_KD_H__
+
+#include "slam6d/kdparams.h"
+#include "slam6d/searchTree.h"
+#include "ANN/ANN.h"
+
+/**
+ * @brief Encapsulating class to create and store ANN KD Trees. 
+ * 
+ * A kD tree for points, with limited
+ * capabilities (find nearest point to
+ * a given point, or in a fixed radius distance).
+ **/
+class ANNtree : public SearchTree {
+
+public:
+  
+  /**
+   * default constructor
+   */
+  ANNtree();
+  
+  /**
+   * Constructor using the point set pts and the num_of_pts n
+   */
+  ANNtree(PointerArray<double>&_pts, int n);
+  
+  /**
+   * destructor
+   */
+  virtual ~ANNtree(); 
+  
+  
+ /**
+  * Finds the closest point within the tree,
+  * wrt. the point given as first parameter.
+  * @param _p point
+  * @param maxdist2 maximal search distance.
+  * @param threadNum Thread number, for parallelization
+  * @return Pointer to the closest point
+  */  
+  double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
+
+private:
+
+  /**
+   * a pointer to ANNkd_tree instance
+   */
+  ANNkd_tree* annkd;
+  ANNdistArray nn; //temporary ANNdistArray instance to use for storing the nearest neighbor
+  ANNidxArray nn_idx; //temporary ANNdistArray instance to use for storing the nearest neighbor
+
+  double** pts;
+  
+};
+
+#endif
+
+

.svn/pristine/c9/c969cb15b1ff8c8771f040288107b7b1b0d8eabf.svn-base

@@ -0,0 +1,87 @@
+/**
+ * @file
+ * @brief Representation of a 3D scan and implementation of scan matching
+ * @author Andreas Nuechter. Jacobs University Bremen, Germany
+ * @author Li Wei, Wuhan University, China
+ * @author Li Ming, Wuhan University, China
+ */
+
+#ifndef __VELOSCAN_H__
+#define __VELOSCAN_H__
+
+#ifdef _MSC_VER
+#define snprintf _snprintf
+#undef _STDIO_DEFINED
+#define  _USE_MATH_DEFINES
+#endif
+
+#include <vector>
+#include <map>
+
+#include "slam6d/basicScan.h"
+#include "veloslam/gridcell.h"
+#include "veloslam/gridcluster.h"
+
+bool FilterNOMovingObjcets(clusterFeature &glu, cluster &gluData);
+
+class Trajectory
+{
+  public:
+     Trajectory();
+  public:
+     vector <Point> path;
+};
+
+
+/**
+ * @brief 3D scan representation and implementation of dynamic velodyne scan matching
+ */
+class VeloScan : public BasicScan {
+
+public:
+  VeloScan();
+  VeloScan(const VeloScan& s);
+  ~VeloScan();
+
+public:
+  // FIXME
+  void FindingAllofObject(int maxDist, int minDist);
+  void TrackingAllofObject(int trackingAlgo);
+  void ExchangePointCloud();
+  void ClassifiAllofObject();
+
+  int DumpScan(string filename);
+  int DumpScanRedPoints(string filename);
+  int DeletePoints();
+
+  int CalcRadAndTheta();
+  int TransferToCellArray(int maxDist, int minDist);
+
+  void MarkStaticorMovingPointCloud();
+  void FreeAllCellAndCluterMemory();
+  void ClassifiAllObject();
+  void ClassifibyTrackingAllObject(int currentNO ,int windowsize);
+  void calcReducedPoints_byClassifi(double voxelSize, int nrpts, PointType pointtype);
+
+  int CalcScanCellFeature();
+  int CalcCellFeature(cell& cellobj,cellFeature& f);
+  int FindAndCalcScanClusterFeature();
+  int SearchNeigh(cluster& clu,charvv& flagvv,int i,int j);
+  int CalcClusterFeature(cluster& clu,clusterFeature& f);
+  void SaveObjectsInPCD(int index, cluster &gClusterData );
+  void SaveFrameInPCD( );
+
+  bool isTrackerHandled;
+  long scanid;
+
+  /** scanCellFeatureArray */
+  cellArray scanCellArray;
+  cellFeatureArray scanCellFeatureArray;
+
+  clusterArray scanClusterArray;
+  clusterFeatureArray scanClusterFeatureArray;
+
+  int clusterNum;//the number of clusters to be tracked, added by yuanjun
+};
+
+#endif

.svn/pristine/ce/ceebc6cd1fa5bafc9044862d682daa76900564d0.svn-base

@@ -0,0 +1,50 @@
+/** @file 
+ *  @brief Representation of the optimized k-d tree. 
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher
+ */
+
+#ifndef __KD_H__
+#define __KD_H__
+
+#include "slam6d/kdparams.h"
+#include "slam6d/searchTree.h"
+#include "slam6d/kdTreeImpl.h"
+
+#ifdef _MSC_VER
+#if !defined _OPENMP && defined OPENMP 
+#define _OPENMP
+#endif
+#endif
+
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
+
+struct Void { };
+
+struct PtrAccessor {
+    inline double *operator() (Void, double* indices) {
+        return indices;
+    }
+};
+
+/**
+ * @brief The optimized k-d tree. 
+ * 
+ * A kD tree for points, with limited
+ * capabilities (find nearest point to
+ * a given point, or to a ray).
+ **/
+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;
+};
+
+#endif

.svn/pristine/d3/d363c05176b5c741c8e0b7bced15231a1aae80af.svn-base

@@ -0,0 +1,628 @@
+/*
+ * scan_io_velodyne implementation
+ *
+ * Copyright (C) Mohammad Faisal, Dorit Borrmann, Andreas Nuechter
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file
+ * @brief Implementation of reading 3D scans
+ * @author Andreas Nuechter. Jacobs University Bremen gGmbH
+ * @author Dorit Borrmann. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
+ * @author Mohammad Faisal. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
+ */
+#include "slam6d/point.h"
+#include "veloslam/velodefs.h"
+#include "scanio/scan_io_velodyne.h"
+#include "slam6d/globals.icc"
+#include <fstream>
+#include<strstream>
+#include <iostream>
+using std::ifstream;
+using std::ostrstream;
+using std::cerr;
+using std::endl;
+using std::ends;
+
+#include <algorithm>
+using std::swap;
+
+#include<cstdio>
+#include<cstdlib>
+#include<cmath>
+#include<cstring>
+#include <errno.h>
+
+using namespace std;
+
+#ifdef _MSC_VER
+#include <windows.h>
+#endif
+#include <boost/filesystem/operations.hpp>
+#include <boost/filesystem/fstream.hpp>
+using namespace boost::filesystem;
+
+#define DATA_PATH_PREFIX "scan"
+#define DATA_PATH_SUFFIX ".bin"
+#define POSE_PATH_PREFIX "scan"
+#define POSE_PATH_SUFFIX ".pose"
+
+#define BLOCK_OFFSET 42+16
+
+#define BLOCK_SIZE 1206
+#define CIRCLELENGTH 360
+#define VELODYNE_NUM_LASERS 64
+#define CircleBufferSize CIRCLELENGTH*32*12
+#define CIRCLEROUND CIRCLELENGTH*6
+
+#define RADIANS_PER_LSB 0.0174532925
+#define METERS_PER_LSB 0.002
+#define METERS_PER_CM 0.01
+
+#define TWOPI_INV (0.5/M_PI)
+#define TWOPI (2*M_PI)
+
+typedef struct raw_packet
+  {
+    unsigned char dat[1200];
+    unsigned short revolution;
+    unsigned char status[4];
+  } raw_packet_t;
+
+
+typedef unsigned char BYTE;
+
+long CountOfLidar = 0;
+
+//Changes the variable to be filled dynamically. Added a 6th variable to enable or disable a specific sensor
+double velodyne_calibrated[VELODYNE_NUM_LASERS][6];
+
+/**
+ * Method to fill the calibration data using the Velodyne64 calibration data from Wuhan.
+ */
+int backup()
+{
+	double velodyne_calibrated1[64][6] = 
+	{
+		{	-7.1581192,	-4.5,	102,	21.560343,	-2.5999999, 1},
+		{	-6.8178215,	-3.4000001,	125,	21.516994,	2.5999999, 1},
+		{	0.31782165,	3,	130,	20.617426,	-2.5999999, 1},
+		{	0.65811908,	4.5999999,	128,	20.574717,	2.5999999, 1},
+		{	-6.4776502,	-0.5,	112,	21.473722,	-2.5999999, 1},
+		{	-6.1375928,	1,	125,	21.430525,	2.5999999, 1},
+		{	-8.520812,	-1.5,	106,	21.734608,	-2.5999999, 1},
+		{	-8.1798887,	0.40000001,	127,	21.690901,	2.5999999, 1},
+		{	-5.797637,	4,	111,	21.387396,	-2.5999999, 1},
+		{	-5.4577708,	5.5,	126,	21.34433,	2.5999999, 1},
+		{	-7.8391404,	3.0999999,	113,	21.647291,	-2.5999999, 1},
+		{	-7.4985547,	4.5,	123,	21.603773,	2.5999999, 1},
+		{	-3.0802133,	-4.5,	105,	21.044245,	-2.5999999, 1},
+		{	-2.7406337,	-3.2,	133,	21.001518,	2.5999999, 1},
+		{	-5.1179824,	-5.5,	110,	21.301321,	-2.5999999, 1},
+		{	-4.7782598,	-4,	129,	21.258366,	2.5999999, 1},
+		{	-2.4010365,	-0.2,	111,	20.958813,	-2.5999999, 1},
+		{	-2.0614092,	1,	130,	20.916126,	2.5999999, 1},
+		{	-4.4385905,	-1.2,	115,	21.215462,	-2.5999999, 1},
+		{	-4.0989642,	0,	133,	21.172602,	2.5999999, 1},
+		{	-1.7217404,	3.8,	113,	20.873451,	-2.5999999, 1},
+		{	-1.3820176,	5,	130,	20.830786,	2.5999999, 1},
+		{	-3.7593663,	3,	117,	21.129782,	-2.5999999, 1},
+		{	-3.4197867,	4.5,	129,	21.086998,	2.5999999, 1},
+		{	0.998555,	-4.5,	107,	20.531982,	-2.5999999, 1},
+		{	1.339141,	-3.2,	131,	20.489222,	2.5999999, 1},
+		{	-1.0422293,	-5.4000001,	128,	20.788124,	-2.5999999, 1},
+		{	-0.70236301,	-4,	134,	20.745461,	2.5999999, 1},
+		{	1.679889,	-0.5,	124,	20.446428,	-2.5999999, 1},
+		{	2.0208123,	1,	136,	20.403601,	2.5999999, 1},
+		{	-0.36240739,	-1.5,	131,	20.702793,	-2.5999999, 1},
+		{	-0.022349782,	0.2,	136,	20.660116,	2.5999999, 1},
+		{	-22.737886,	-7.8000002,	101,	16.019152,	-2.5999999, 1},
+		{	-22.226072,	-5,	88,	15.954137,	2.5999999, 1},
+		{	-11.513928,	4.5,	121,	14.680806,	-2.5999999, 1},
+		{	-11.002114,	7.4000001,	88,	14.623099,	2.5999999, 1},
+		{	-21.714685,	-1,	94,	15.889649,	-2.5999999, 1},
+		{	-21.203688,	2,	88,	15.82566,	2.5999999, 1},
+		{	-24.790272,	-2.5,	114,	16.284933,	-2.5999999, 1},
+		{	-24.276321,	0.5,	89,	16.217583,	2.5999999, 1},
+		{	-20.693031,	6,	98,	15.762167,	-2.5999999, 1},
+		{	-20.182682,	9,	92,	15.699132,	2.5999999, 1},
+		{	-23.762968,	4.5,	107,	16.15085,	-2.5999999, 1},
+		{	-23.250172,	7.5,	80,	16.084715,	2.5999999, 1},
+		{	-16.615318,	-7.5,	121,	15.26925,	-2.5999999, 1},
+		{	-16.105938,	-5,	92,	15.209245,	2.5999999, 1},
+		{	-19.672594,	-9,	119,	15.63654,	-2.5999999, 1},
+		{	-19.162729,	-6,	89,	15.574372,	2.5999999, 1},
+		{	-15.596496,	-1,	109,	15.14954,	-2.5999999, 1},
+		{	-15.086954,	2,	88,	15.090119,	2.5999999, 1},
+		{	-18.653046,	-2,	117,	15.51261,	-2.5999999, 1},
+		{	-18.143503,	0.69999999,	88,	15.451235,	2.5999999, 1},
+		{	-14.577271,	5.5,	112,	15.030966,	-2.5999999, 1},
+		{	-14.067405,	8.3999996,	87,	14.972065,	2.5999999, 1},
+		{	-17.634062,	5,	119,	15.390228,	-6.1999998, 1},
+		{	-17.124681,	7.5,	97,	15.329572,	2.5999999, 1},
+		{	-10.489829,	-7.5,	119,	14.565539,	-2.5999999, 1},
+		{	-9.9770317,	-4.6999998,	95,	14.508112,	2.5999999, 1},
+		{	-13.557318,	-8.5,	126,	14.913401,	-2.5999999, 1},
+		{	-13.046968,	-6,	92,	14.854958,	2.5999999, 1},
+		{	-9.4636793,	-1,	112,	14.450804,	-2.5999999, 1},
+		{	-8.949728,	1.5,	93,	14.3936,	2.5999999, 1},
+		{	-12.536313,	-2,	121,	14.796721,	-2.5999999, 1},
+		{	-12.025314,	0.40000001,	96,	14.738676,	2.5999999, 1},
+	};
+
+	for ( int i = 0; i < VELODYNE_NUM_LASERS; i++ )
+	{
+	velodyne_calibrated[i][0] = velodyne_calibrated1[i][0];
+	velodyne_calibrated[i][1] = velodyne_calibrated1[i][1];
+	velodyne_calibrated[i][2] = velodyne_calibrated1[i][2];
+	velodyne_calibrated[i][3] = velodyne_calibrated1[i][3];
+	velodyne_calibrated[i][4] = velodyne_calibrated1[i][4];
+	velodyne_calibrated[i][5] = velodyne_calibrated1[i][5];
+	}
+
+	return 0;
+}
+
+double rotCorrection[VELODYNE_NUM_LASERS];
+double vertCorrection[VELODYNE_NUM_LASERS];
+double distCorrection[VELODYNE_NUM_LASERS];
+double vertoffsetCorrection[VELODYNE_NUM_LASERS];
+double horizdffsetCorrection[VELODYNE_NUM_LASERS];
+double enabled[VELODYNE_NUM_LASERS];	//New variable to change enabling and disabling of data.
+
+int physical2logical[VELODYNE_NUM_LASERS];
+int logical2physical[VELODYNE_NUM_LASERS];
+
+double absf ( double a )
+{
+    if ( a < 0 )
+        return -a;
+    return a;
+}
+
+int velodyne_physical_to_logical ( int phys )
+{
+    return physical2logical[phys];
+}
+
+int velodyne_logical_to_physical ( int logical )
+{
+    return logical2physical[logical];
+}
+
+int laser_phi_compare(const void *_a, const void *_b)
+{
+    int a = *((int*) _a);
+    int b = *((int*) _b);
+
+    if (velodyne_calibrated[a][0] < velodyne_calibrated[b][0])
+        return -1;
+
+    return 1;
+}
+
+/** valid only for v > 0 **/
+static inline double mod2pi_positive(double vin)
+{
+    double q = vin * TWOPI_INV + 0.5;
+    int qi = (int) q;
+
+    return vin - qi*TWOPI;
+}
+
+/** Map v to [-PI, PI] **/
+static inline double mod2pi(double vin)
+{
+    if (vin < 0)
+        return -mod2pi_positive(-vin);
+    else
+        return mod2pi_positive(vin);
+}
+
+/** Return vin such that it is within PI degrees of ref **/
+static inline double mod2pi_ref(double ref, double vin)
+{
+    return ref + mod2pi(vin - ref);
+}
+
+int velodyne_calib_precompute()
+{
+    int i;
+    int logical;
+    for ( i = 0; i < VELODYNE_NUM_LASERS; i++ )
+        logical2physical[i] = i;
+
+    qsort ( logical2physical, VELODYNE_NUM_LASERS, sizeof ( int ), laser_phi_compare );
+
+    for ( logical = 0; logical < VELODYNE_NUM_LASERS; logical++ )
+    {
+        physical2logical[logical2physical[logical]] = logical;
+    }
+
+    //vertCorrection  rotCorrection  distCorrection  vertOffsetCorrection  horizOffsetCorrection  enable
+    for ( i = 0; i < VELODYNE_NUM_LASERS; i++ )
+    {
+        vertCorrection[i] = velodyne_calibrated[i][0] * RADIANS_PER_LSB;
+        rotCorrection[i] =  velodyne_calibrated[i][1] * RADIANS_PER_LSB;
+        distCorrection[i] = velodyne_calibrated[i][2] * METERS_PER_CM;
+        vertoffsetCorrection[i] = velodyne_calibrated[i][3] * METERS_PER_CM;
+        horizdffsetCorrection[i] = velodyne_calibrated[i][4] * METERS_PER_CM;
+	enabled[i] = velodyne_calibrated[i][5];
+    }
+
+    return 0;
+}
+
+
+int calibrate(string filename)
+{
+	int linecount = 0;
+	string line;
+	std::ifstream myfile (filename.c_str());
+	if (myfile.is_open())
+	{	
+		cout << "Using Calibration File"<<endl;
+		string data[6];
+		getline (myfile,line);
+		string theDelimiter = ",";
+		int j=0;
+		do
+		{
+			getline (myfile,line);
+			int i = 0;
+			size_t  start = 0, end = 0;
+			while ( end != string::npos  && i < 6)
+			{
+				end = line.find( theDelimiter, start);
+				data[i] = line.substr(start, (end == string::npos)?string::npos:end - start);
+				start = ((end>(string::npos - theDelimiter.size()))?string::npos:end + theDelimiter.size());
+				i++;
+			}
+
+			velodyne_calibrated[j][0] = atof(data[0].c_str());
+			velodyne_calibrated[j][1] = atof(data[1].c_str());
+			velodyne_calibrated[j][2] = atof(data[2].c_str());
+			velodyne_calibrated[j][3] = atof(data[3].c_str());
+			velodyne_calibrated[j][4] = atof(data[4].c_str());
+			velodyne_calibrated[j][5] = atof(data[5].c_str());
+			linecount++;
+			j++;
+		}
+		while(!myfile.eof());
+		
+		myfile.close();
+
+		if (linecount < 60)
+		{
+			for (int i = 32; i<64; i++)
+			{
+				velodyne_calibrated[i][0] = 0.0;
+				velodyne_calibrated[i][1] = 0.0;
+				velodyne_calibrated[i][2] = 0.0;
+				velodyne_calibrated[i][3] = 0.0;
+				velodyne_calibrated[i][4] = 0.0;
+				velodyne_calibrated[i][5] = 0.0;
+			}
+		}
+	}
+	else
+	{
+		cout << "Unable to open calibration file.\nUsing Wuhan hardcored values."<<endl;
+		backup();
+	}
+
+	return 0;
+}
+
+
+int read_one_packet (
+    FILE *fp,
+    PointFilter& filter,
+    std::vector<double>* xyz,
+    std::vector<unsigned char>* rgb,
+    std::vector<float>* reflectance,
+    std::vector<float>* amplitude,
+    std::vector<int>* type,
+    std::vector<float>* deviation)
+{
+
+	int  c, i, j;
+    unsigned char Head;
+    BYTE buf[BLOCK_SIZE];
+    Point point;
+    BYTE *p;
+    unsigned short *ps;
+    unsigned short *pt;
+    unsigned short *pshort;
+
+    double ctheta;
+    double theta, phi;
+
+    double sin_ctheta, cos_ctheta;
+    double sin_theta, cos_theta;
+    double sin_phi, cos_phi;
+
+    unsigned short physicalNO;
+    unsigned short logicalNO;
+
+    float rotational;
+    float distance;
+    float corredistance;
+    int intensity;
+    int physical;
+    int size;
+
+    unsigned short rot;
+    double x, y, z;
+
+    int circle_col = 0;
+    int circle_row = 0;
+
+	for ( c = 0 ; c < CIRCLELENGTH; c++ )
+	{
+#ifdef _MSC_VER 
+		fseek(fp , BLOCK_OFFSET, SEEK_CUR);
+#else
+#ifdef __CYGWIN__
+		fseek(fp , BLOCK_OFFSET, SEEK_CUR);
+#else		
+		fseeko(fp , BLOCK_OFFSET, SEEK_CUR);
+#endif
+#endif		
+		size=fread ( buf, 1, BLOCK_SIZE, fp );
+
+		if(size<BLOCK_SIZE)
+			return -1;
+
+		ps = ( unsigned short * ) buf;
+		p = buf;
+		physicalNO = 0;
+
+		for ( i = 0; i < 12; i++ )
+		{
+			//Each frame start with 0xEEFF || 0xDDFF
+			if ( *ps == 0xEEFF )
+				Head = 0;
+			else if ( *ps == 0xDDFF )
+				Head = 32;
+
+			pshort = ( unsigned short * ) ( p + 2 );
+			rot = ( ( unsigned short ) ( *pshort ) / 100.0 );
+
+			circle_col =  c * 6 + i / 2;
+			rotational = ( ( float ) ( *pshort ) ) / 100.0;
+
+			for ( j = 0; j < 32; j++ )
+			{
+				physicalNO  = j + Head;
+				logicalNO = velodyne_physical_to_logical ( physicalNO );
+
+				circle_col =  c * 6 + i / 2;
+				circle_row =  logicalNO;
+
+				pt = ( unsigned short * ) ( p + 4 + j * 3 );
+				distance = absf ( ( *pt ) * 0.002 );
+
+				if( distance < 120   &&	   distance > 2.2 )
+				{
+					BYTE *inty = ( BYTE * ) ( p + 4 + j * 3 + 2 );
+					intensity = *inty;
+
+					ctheta = 2 * M_PI - rotational * RADIANS_PER_LSB;
+					if ( ctheta == 2*M_PI )
+						ctheta = 0;
+
+					sin_ctheta = sin ( ctheta );
+					cos_ctheta = cos ( ctheta );
+
+					//vertCorrection  rotCorrection  distCorrection  vertOffsetCorrection  horizOffsetCorrection
+					//                corredistance = ( distance + distCorrection[physicalNO] ) * ( 1.0 + vertoffsetCorrection[physicalNO] );
+					corredistance = ( distance + distCorrection[physicalNO] ) ;
+					theta     = mod2pi_ref ( M_PI, ctheta + rotCorrection[physicalNO] ); /////////????////////
+					phi    = vertCorrection[physicalNO];  ////////?????/////////////////
+
+					sin_theta = sin ( theta );
+					cos_theta = cos ( theta );
+					sin_phi = sin ( phi );
+					cos_phi = cos ( phi );
+
+					/////////////////////?a??¡Á?¡À¨º/////////////////////
+					x = corredistance * cos_theta * cos_phi;
+					y = corredistance * sin_theta * cos_phi;
+					z = corredistance * sin_phi +vertoffsetCorrection[physicalNO]*cos_phi;
+
+					x -= horizdffsetCorrection[physicalNO] * cos_ctheta;
+					y -= horizdffsetCorrection[physicalNO] * sin_ctheta;
+
+					point.rad  = sqrt(  x*x 	+  y*y );
+					point.tan_theta  = y/x;
+
+					point.type = POINT_TYPE_GROUND;
+					point.x=x*100;
+					point.y=z*100;
+					point.z=-y*100;
+
+				        double p[3];
+				        p[0] = point.x;
+				        p[1] = point.y;
+				        p[2] = point.z;
+
+					//Enable Check. Could be moved earlier to remove all the unnecessary calculations.
+					//Here to simplify the implementation
+			                if(filter.check(p) && (enabled[physicalNO]==1))
+   					{
+						 for(int ii = 0; ii < 3; ++ii) xyz->push_back(p[ii]);
+					}
+				}
+			}
+            p = p + 100;
+            ps = ( unsigned short * ) p;
+        }
+
+
+    }
+
+    return 0;
+}
+
+std::list<std::string> ScanIO_velodyne::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
+{
+	//Calling the calibration method with the file name "calibration.txt" in the same folder as the scan.bin
+	string califilename;
+	string dir(dir_path);
+	califilename = dir + "calibration" + ".txt";
+	calibrate(califilename);
+   std::list<std::string> identifiers;
+//   fileCounter = start;
+
+   velodyne_calib_precompute();
+   for(unsigned int i = start; i <= end; ++i)
+   {
+	   std::string identifier(to_string(i,3));
+
+	   path data(dir_path);
+	   data /= path(std::string(DATA_PATH_PREFIX) + DATA_PATH_SUFFIX);
+
+	   path pose(dir_path);
+	   pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
+
+	   if(!exists(data))
+		   break;
+	   identifiers.push_back(identifier);
+   }
+
+  return identifiers;
+}
+
+void ScanIO_velodyne::readPose(const char* dir_path, const char* identifier, double* pose)
+{
+  unsigned int i;
+  // on pose information for veloslam
+  for(i = 0; i < 6; ++i)  pose[i] = 0.0;
+  for(i = 3; i < 6; ++i)  pose[i] = 0.0;
+   return;
+}
+
+bool ScanIO_velodyne::supports(IODataType type)
+{
+  return !!(type & (DATA_XYZ));
+}
+
+
+void ScanIO_velodyne::readScan(
+    const char* dir_path,
+    const char* identifier,
+    PointFilter& filter,
+    std::vector<double>* xyz,
+    std::vector<unsigned char>* rgb,
+    std::vector<float>* reflectance,
+    std::vector<float>* temperature,
+    std::vector<float>* amplitude,
+    std::vector<int>* type,
+    std::vector<float>* deviation)
+{
+    unsigned int i;
+    FILE *scan_in;
+
+    path data_path(dir_path);
+    data_path /= path(std::string(DATA_PATH_PREFIX) +  DATA_PATH_SUFFIX);
+    if(!exists(data_path))
+        throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
+
+	char filename[256];
+	sprintf(filename, "%s%s%s",dir_path ,DATA_PATH_PREFIX,  DATA_PATH_SUFFIX );
+
+#ifdef _MSC_VER 
+    scan_in = fopen(filename,"rb");
+#else
+#ifdef __CYGWIN__
+    scan_in = fopen(filename,"rb");
+#else    
+    scan_in = fopen64(filename,"rb");
+#endif
+#endif
+
+    if(scan_in == NULL)
+    {
+      cerr<<data_path <<endl;
+      cerr << "ERROR: Missing file " << data_path <<" "<<strerror(errno)<< endl;
+      exit(1);
+      return;
+    }
+
+    cout << "Processing Scan " << data_path;
+    cout.flush();
+
+    xyz->reserve(12*32*CIRCLELENGTH);
+
+	fileCounter=  atoi(identifier);
+
+#ifdef _MSC_VER 
+    fseek(scan_in, 24, SEEK_SET);
+    fseek(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR);
+#else
+#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(
+                scan_in,
+                filter,
+                xyz,
+                rgb,
+                reflectance,
+                amplitude,
+                type,
+                deviation);
+
+    cout << " with " << xyz->size() << " Points";
+    cout << " done " << fileCounter<<endl;
+
+    fclose(scan_in);
+    // process next frames
+}
+
+/**
+ * class factory for object construction
+ *
+ * @return Pointer to new object
+ */
+#ifdef _MSC_VER
+extern "C" __declspec(dllexport) ScanIO* create()
+#else
+extern "C" ScanIO* create()
+#endif
+{
+  return new ScanIO_velodyne;
+}
+
+
+/**
+ * class factory for object construction
+ *
+ * @return Pointer to new object
+ */
+#ifdef _MSC_VER
+extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
+#else
+extern "C" void destroy(ScanIO *sio)
+#endif
+{
+  delete sio;
+}
+
+#ifdef _MSC_VER
+BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
+{
+	return TRUE;
+}
+#endif
+

.svn/pristine/ec/ecb7555697b87181f525ef3de68a909bc80a41a5.svn-base

@@ -0,0 +1,729 @@
+/*
+ * scan_red implementation
+ *
+ * Copyright (C) Johannes Schauer
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+#include "slam6d/scan.h"
+#include "slam6d/globals.icc"
+
+#include <string>
+using std::string;
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+#include <algorithm>
+
+#include <boost/program_options.hpp>
+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, PYR_SEGMENTATION, WATERSHED};
+
+/* Function used to check that 'opt1' and 'opt2' are not specified
+   at the same time. */
+void conflicting_options(const po::variables_map & vm,
+                         const char *opt1, const char *opt2)
+{
+    if (vm.count(opt1) && !vm[opt1].defaulted()
+        && vm.count(opt2) && !vm[opt2].defaulted())
+        throw std::logic_error(string("Conflicting options '")
+                               + opt1 + "' and '" + opt2 + "'.");
+}
+
+/* Function used to check that if 'for_what' is specified, then
+   'required_option' is specified too. */
+void option_dependency(const po::variables_map & vm,
+                       const char *for_what, const char *required_option)
+{
+    if (vm.count(for_what) && !vm[for_what].defaulted())
+        if (vm.count(required_option) == 0
+            || vm[required_option].defaulted())
+            throw std::logic_error(string("Option '") + for_what +
+                                   "' requires option '" +
+                                   required_option + "'.");
+}
+
+/*
+ * validates panorama method specification
+ */
+namespace fbr {
+    void validate(boost::any& v, const std::vector<std::string>& values,
+                  projection_method*, int) {
+        if (values.size() == 0)
+            throw std::runtime_error("Invalid model specification");
+        string arg = values.at(0);
+        if(strcasecmp(arg.c_str(), "EQUIRECTANGULAR") == 0) v = EQUIRECTANGULAR;
+        else if(strcasecmp(arg.c_str(), "CYLINDRICAL") == 0) v = CYLINDRICAL;
+        else if(strcasecmp(arg.c_str(), "MERCATOR") == 0) v = MERCATOR;
+        else if(strcasecmp(arg.c_str(), "RECTILINEAR") == 0) v = RECTILINEAR;
+        else if(strcasecmp(arg.c_str(), "PANNINI") == 0) v = PANNINI;
+        else if(strcasecmp(arg.c_str(), "STEREOGRAPHIC") == 0) v = STEREOGRAPHIC;
+        else if(strcasecmp(arg.c_str(), "ZAXIS") == 0) v = ZAXIS;
+        else if(strcasecmp(arg.c_str(), "CONIC") == 0) v = CONIC;
+        else throw std::runtime_error(std::string("projection method ") + arg + std::string(" is unknown"));
+    }
+}
+
+/*
+ * validates segmentation method specification
+ */
+void validate(boost::any& v, const std::vector<std::string>& values,
+        segment_method*, int) {
+    if (values.size() == 0)
+        throw std::runtime_error("Invalid model specification");
+    string arg = values.at(0);
+    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"));
+}
+
+/*
+ * validates input type specification
+ */
+void validate(boost::any& v, const std::vector<std::string>& values,
+    IOType*, int) {
+    if (values.size() == 0)
+        throw std::runtime_error("Invalid model specification");
+    string arg = values.at(0);
+    try {
+        v = formatname_to_io_type(arg.c_str());
+    } catch (...) { // runtime_error
+        throw std::runtime_error("Format " + arg + " unknown.");
+    }
+}
+
+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
+ */
+void parse_options(int argc, char **argv, int &start, int &end,
+        bool &scanserver, image_type &itype, int &width, int &height,
+        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,
+        double &pyrlinks, double &pyrcluster, int &pyrlevels)
+{
+    po::options_description generic("Generic options");
+    generic.add_options()
+        ("help,h", "output this help message");
+
+    po::options_description input("Input options");
+    input.add_options()
+        ("start,s", po::value<int>(&start)->default_value(0),
+         "start at scan <arg> (i.e., neglects the first <arg> scans) "
+         "[ATTENTION: counting naturally starts with 0]")
+        ("end,e", po::value<int>(&end)->default_value(-1),
+         "end after scan <arg>")
+        ("format,f", po::value<IOType>(&iotype)->default_value(UOS),
+         "using shared library <arg> for input. (chose F from {uos, uos_map, "
+         "uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, "
+         "riegl_txt, riegl_rgb, riegl_bin, zahn, ply})")
+        ("max,M", po::value<int>(&maxDist)->default_value(-1),
+         "neglegt all data points with a distance larger than <arg> 'units")
+        ("min,m", po::value<int>(&minDist)->default_value(-1),
+         "neglegt all data points with a distance smaller than <arg> 'units")
+        ("scanserver,S", po::value<bool>(&scanserver)->default_value(false),
+         "Use the scanserver as an input method and handling of scan data");
+
+    po::options_description image("Panorama image options");
+    image.add_options()
+        ("range,r", "create range image")
+        ("intensity,i", "create intensity image")
+        ("width,w", po::value<int>(&width)->default_value(1280),
+         "width of panorama")
+        ("height,h", po::value<int>(&height)->default_value(960),
+         "height of panorama")
+        ("panorama,p", po::value<fbr::projection_method>(&ptype)->
+         default_value(fbr::EQUIRECTANGULAR), "panorama type (EQUIRECTANGULAR, "
+         "CYLINDRICAL, MERCATOR, RECTILINEAR, PANNINI, STEREOGRAPHIC, ZAXIS, "
+         "CONIC)")
+        ("num-images,N", po::value<int>(&nImages)->default_value(1),
+         "number of images used for some projections")
+        ("proj-param,P", po::value<int>(&pParam)->default_value(0),
+         "special projection parameter")
+        ("dump-pano,d", po::bool_switch(&dump_pano),
+         "output panorama (useful to create marker image for watershed)");
+
+    po::options_description range_image("Range image options");
+    range_image.add_options()
+        ("logarithm,L", po::bool_switch(&logarithm),
+         "use the logarithm for range image panoramas")
+        ("cutoff,C", po::value<float>(&cutoff)->default_value(0.0), // FIXME: percentage is the wrong word
+         "percentage of furthest away data points to cut off to improve "
+         "precision for closer values (values from 0.0 to 1.0)");
+
+    po::options_description segment("Segmentation options");
+    segment.add_options()
+        ("segment,g", po::value<segment_method>(&stype)->
+         default_value(PYR_MEAN_SHIFT), "segmentation method (THRESHOLD, "
+         "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),
+         "threshold for threshold segmentation")
+        ("maxlevel,X", po::value<int>(&maxlevel),
+         "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)");
+
+    po::options_description hidden("Hidden options");
+    hidden.add_options()
+        ("input-dir", po::value<string>(&dir), "input dir");
+
+    // all options
+    po::options_description all;
+    all.add(generic).add(input).add(image).add(range_image).add(segment).add(hidden);
+
+    // options visible with --help
+    po::options_description cmdline_options;
+    cmdline_options.add(generic).add(input).add(image).add(range_image).add(segment);
+
+    // positional argument
+    po::positional_options_description pd;
+    pd.add("input-dir", 1);
+
+    // process options
+    po::variables_map vm;
+    po::store(po::command_line_parser(argc, argv).
+              options(all).positional(pd).run(), vm);
+    po::notify(vm);
+
+    // 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);
+    }
+
+    // option conflicts and dependencies
+    conflicting_options(vm, "range", "intensity");
+    option_dependency(vm, "logarithm", "range");
+    option_dependency(vm, "cutoff", "range");
+
+    // decide between range and intensity panorama
+    if (vm.count("range"))
+        itype = M_RANGE;
+    else
+        itype = M_INTENSITY;
+
+    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) {
+        pParam = 1;
+        if(nImages < 3) nImages = 3;
+    }
+    if (ptype == fbr::STEREOGRAPHIC && pParam == 0) {
+        pParam = 2;
+        if(nImages < 3) nImages = 3;
+    }
+    if (ptype == fbr::RECTILINEAR && nImages < 3) {
+        nImages = 3;
+    }
+
+    // add trailing slash to directory if not present yet
+    if (dir[dir.length()-1] != '/') dir = dir + "/";
+}
+
+/*
+ * retrieve a cv::Mat with x,y,z,r from a scan object
+ * functionality borrowed from scan_cv::convertScanToMat but this function
+ * does not allow a scanserver to be used, prints to stdout and can only
+ * handle a single scan
+ */
+cv::Mat scan2mat(Scan *source)
+{
+    DataXYZ xyz = source->get("xyz");
+    DataReflectance xyz_reflectance = source->get("reflectance");
+    unsigned int nPoints = xyz.size();
+    cv::Mat scan(nPoints,1,CV_32FC(4));
+    scan = cv::Scalar::all(0);
+    cv::MatIterator_<cv::Vec4f> it;
+    it = scan.begin<cv::Vec4f>();
+    for(unsigned int i = 0; i < nPoints; i++){
+        float x, y, z, reflectance;
+        x = xyz[i][0];
+        y = xyz[i][1];
+        z = xyz[i][2];
+        reflectance = xyz_reflectance[i];
+
+        //normalize the reflectance
+        reflectance += 32;
+        reflectance /= 64;
+        reflectance -= 0.2;
+        reflectance /= 0.3;
+        if (reflectance < 0) reflectance = 0;
+        if (reflectance > 1) reflectance = 1;
+
+        (*it)[0] = x;
+        (*it)[1] = y;
+        (*it)[2] = z;
+        (*it)[3] = reflectance;
+
+        ++it;
+    }
+    return scan;
+}
+
+/*
+ * convert a matrix of float values (range image) to a matrix of unsigned
+ * eight bit characters using different techniques
+ */
+cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff)
+{
+    cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0));
+    float max = 0;
+    // find maximum value
+    if (cutoff == 0.0) {
+        // without cutoff, just iterate through all values to find the largest
+        for (cv::MatIterator_<float> it = source.begin<float>();
+                it != source.end<float>(); ++it) {
+            float val = *it;
+            if (val > max) {
+                max = val;
+            }
+        }
+    } else {
+        // when a cutoff is specified, sort all the points by value and then
+        // specify the max so that <cutoff> values are larger than it
+        vector<float> sorted(source.cols*source.rows);
+        int i = 0;
+        for (cv::MatIterator_<float> it = source.begin<float>();
+                it != source.end<float>(); ++it, ++i) {
+            sorted[i] = *it;
+        }
+        std::sort(sorted.begin(), sorted.end());
+        max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))];
+        cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl;
+    }
+
+    cv::MatIterator_<float> src = source.begin<float>();
+    cv::MatIterator_<uchar> dst = result.begin<uchar>();
+    cv::MatIterator_<float> end = source.end<float>();
+    if (logarithm) {
+        // stretch values from 0 to max logarithmically over 0 to 255
+        // using the logarithm allows to represent smaller values with more
+        // precision and larger values with less
+        max = log(max+1);
+        for (; src != end; ++src, ++dst) {
+            float val = (log(*src+1)*255.0)/max;
+            if (val > 255)
+                *dst = 255;
+            else
+                *dst = (uchar)val;
+        }
+    } else {
+        // stretch values from 0 to max linearly over 0 to 255
+        for (; src != end; ++src, ++dst) {
+            float val = (*src*255.0)/max;
+            if (val > 255)
+                *dst = 255;
+            else
+                *dst = (uchar)val;
+        }
+    }
+    return result;
+}
+
+/*
+ * from grayscale image, create a binary image using a fixed threshold
+ */
+cv::Mat calculateThreshold(vector<vector<cv::Vec3f>> &segmented_points,
+        cv::Mat &img, vector<vector<vector<cv::Vec3f> > > extendedMap,
+        double thresh)
+{
+    int i, j, idx;
+    cv::Mat res;
+    cv::threshold(img, res, thresh, 255, cv::THRESH_BINARY);
+    segmented_points.resize(2);
+
+    for (i = 0; i < res.rows; i++) {
+        for (j = 0; j < res.cols; j++) {
+            idx = res.at<uchar>(i,j);
+            if (idx != 0)
+                idx = 1;
+            segmented_points[idx].insert(segmented_points[idx].end(),
+								 extendedMap[i][j].begin(),
+								 extendedMap[i][j].end());
+        }
+    }
+
+    return res;
+}
+
+/*
+ * calculate the pyramid mean shift segmentation of the image
+ */
+cv::Mat calculatePyrMeanShift(vector<vector<cv::Vec3f>> &segmented_points,
+        cv::Mat &img, vector<vector<vector<cv::Vec3f> > > extendedMap,
+        int maxlevel, int radius)
+{
+    int i, j, idx;
+    cv::Mat imgGray, res, tmp;
+    cvtColor(img, imgGray, CV_GRAY2BGR);
+    cv::pyrMeanShiftFiltering(imgGray, tmp, radius, radius, maxlevel);
+    cvtColor(tmp, res, CV_BGR2GRAY);
+
+    // some colors will be empty
+    // fill histogram first and then pick those entries that are not empty
+    vector<vector<cv::Vec3f>> histogram(256);
+
+    for (i = 0; i < res.rows; i++) {
+        for (j = 0; j < res.cols; j++) {
+            idx = res.at<uchar>(i,j);
+            histogram[idx].insert(histogram[idx].end(),
+						    extendedMap[i][j].begin(),
+						    extendedMap[i][j].end());
+        }
+    }
+
+    for (i = 0; i < 256; i++) {
+        if (!histogram[i].empty()) {
+            segmented_points.push_back(histogram[i]);
+        }
+    }
+
+    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
+ */
+cv::Mat calculateAdaptiveThreshold(vector<vector<cv::Vec3f>> &segmented_points,
+        cv::Mat &img, vector<vector<vector<cv::Vec3f> > > extendedMap)
+{
+    int i, j, idx;
+    cv::Mat res;
+    cv::adaptiveThreshold(img, res, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 49, 5);
+    segmented_points.resize(2);
+
+    for (i = 0; i < res.rows; i++) {
+        for (j = 0; j < res.cols; j++) {
+            idx = res.at<uchar>(i,j);
+            if (idx != 0)
+                idx = 1;
+            segmented_points[idx].insert(segmented_points[idx].end(),
+								 extendedMap[i][j].begin(),
+								 extendedMap[i][j].end());
+        }
+    }
+
+    return res;
+}
+
+/*
+ * using a marker image, calculate the watershed segmentation
+ * a marker image can be created from the panorama retrieved by using the
+ * --dump-pano option
+ */
+cv::Mat calculateWatershed(vector<vector<cv::Vec3f>> &segmented_points,
+        string &marker, cv::Mat &img, vector<vector<vector<cv::Vec3f> > > extendedMap)
+{
+    int i, j, idx;
+    cv::Mat markerMask = cv::imread(marker, 0);
+    vector<vector<cv::Point> > contours;
+    vector<cv::Vec4i> hierarchy;
+    cv::findContours(markerMask, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
+    if (contours.empty())
+        throw std::runtime_error("empty marker mask");
+    cv::Mat markers(markerMask.size(), CV_32S);
+    markers = cv::Scalar::all(0);
+    int compCount = 0;
+    for (int idx = 0; idx >= 0; idx = hierarchy[idx][0], compCount++ )
+        cv::drawContours(markers, contours, idx,
+					cv::Scalar::all(compCount+1), -1, 8, hierarchy, INT_MAX);
+    if (compCount == 0)
+        throw std::runtime_error("no component found");
+    cv::Mat imgGray;
+    cvtColor(img, imgGray, CV_GRAY2BGR);
+    cv::watershed(imgGray, markers);
+
+    segmented_points.resize(compCount);
+
+    for (i = 0; i < markers.rows; i++) {
+        for (j = 0; j < markers.cols; j++) {
+            idx = markers.at<int>(i,j);
+            if (idx > 0 && idx <= compCount) {
+                segmented_points[idx-1].insert(segmented_points[idx-1].end(),
+									  extendedMap[i][j].begin(),
+									  extendedMap[i][j].end());
+            }
+        }
+    }
+
+    return markers;
+}
+
+/*
+ * given a vector of segmented 3d points, write them out as uos files
+ */
+void write3dfiles(vector<vector<cv::Vec3f>> &segmented_points, string &segdir)
+{
+    unsigned int i;
+
+    vector<ofstream*> outfiles(segmented_points.size());
+    for (i = 0; i < segmented_points.size(); i++) {
+        std::stringstream outfilename;
+        outfilename << segdir << "/scan" << std::setw(3) << std::setfill('0') << i << ".3d";
+        outfiles[i] = new ofstream(outfilename.str());
+    }
+
+    for (i = 0; i < segmented_points.size(); i++) {
+        for (vector<cv::Vec3f>::iterator it=segmented_points[i].begin() ;
+		   it < segmented_points[i].end();
+		   it++) {
+            (*(outfiles[i])) << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << endl;
+        }
+    }
+
+    for (i = 0; i < segmented_points.size(); i++) {
+        outfiles[i]->close();
+    }
+}
+
+// write .pose files
+// .frames files can later be generated from them using ./bin/pose2frames
+void writeposefiles(int num, string &segdir, const double* rPos, const double* rPosTheta)
+{
+    for (int i = 0; i < num; i++) {
+        std::stringstream posefilename;
+        posefilename << segdir << "/scan" << std::setw(3) << std::setfill('0') << i << ".pose";
+        ofstream posefile(posefilename.str());
+        posefile << rPos[0] << " " << rPos[1] << " " << rPos[2] << endl;
+        posefile << deg(rPosTheta[0]) << " "
+			  << deg(rPosTheta[1]) << " "
+			  << deg(rPosTheta[2]) << endl;
+        posefile.close();
+    }
+}
+
+void createdirectory(string segdir)
+{
+    int success = mkdir(segdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
+
+    if (success == 0 || errno == EEXIST) {
+        cout << "Writing segmentations to " << segdir << endl;
+    } else {
+        cerr << "Creating directory " << segdir << " failed" << endl;
+        exit(1);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    // commandline arguments
+    int start, end;
+    bool scanserver;
+    image_type itype;
+    int width, height;
+    int maxDist, minDist;
+    int nImages, pParam;
+    fbr::projection_method ptype;
+    bool logarithm;
+    float cutoff;
+    string dir;
+    IOType iotype;
+    segment_method stype;
+    string marker;
+    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, pyrlinks, pyrcluster, pyrlevels);
+
+    Scan::openDirectory(scanserver, dir, iotype, start, end);
+
+    if(Scan::allScans.size() == 0) {
+      cerr << "No scans found. Did you use the correct format?" << endl;
+      exit(-1);
+    }
+
+    cv::Mat img, res;
+    string segdir;
+
+    for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
+        Scan* scan = *it;
+
+        // apply optional filtering
+        scan->setRangeFilter(maxDist, minDist);
+
+        // create target directory
+        segdir = dir + "segmented" + scan->getIdentifier();
+        createdirectory(segdir);
+
+        // create panorama
+        fbr::panorama fPanorama(width, height, ptype, nImages, pParam, fbr::EXTENDED);
+        fPanorama.createPanorama(scan2mat(scan));
+
+        if (itype == M_RANGE) {
+            // the range image has to be converted from float to uchar
+            img = fPanorama.getRangeImage();
+            img = float2uchar(img, logarithm, cutoff);
+        } else {
+            // intensity image
+            img = fPanorama.getReflectanceImage();
+        }
+
+        // output panorama image
+        if (dump_pano)
+            imwrite(segdir+"/panorama.png", img);
+
+        // will store the result of the segmentation
+        vector<vector<cv::Vec3f>> segmented_points;
+
+        if (stype == THRESHOLD) {
+            res = calculateThreshold(segmented_points, img, fPanorama.getExtendedMap(), thresh);
+        } 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) {
+            res = calculateWatershed(segmented_points, marker, img, fPanorama.getExtendedMap());
+        }
+
+        // output segmentation image
+        if (dump_seg)
+            imwrite(segdir+"/segmentation.png", res);
+
+        // write .3d and .pose files
+        write3dfiles(segmented_points, segdir);
+        writeposefiles(segmented_points.size(), segdir, scan->get_rPos(), scan->get_rPosTheta());
+    }
+}

.svn/pristine/f1/f127b01bfac80b6078ad9ad6d09ff516c2479da6.svn-base

@@ -0,0 +1,50 @@
+/**
+  * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm
+  *
+  * Copyright (C) Jacobs University Bremen
+  *
+  * Released under the GPL version 3.
+  *
+  * @author Mihai-Cotizo Sima
+  */
+
+
+#ifndef __FHGRAPH_H_
+#define __FHGRAPH_H_
+
+#include <vector>
+#include <list>
+
+#include <slam6d/point.h>
+#include <slam6d/scan.h>
+#include <segmentation/segment-graph.h>
+#include <ANN/ANN.h>
+
+class FHGraph {
+public:
+    FHGraph(std::vector< Point >& ps, double weight(Point, Point), double sigma, double eps, int neighbors, float radius);
+    edge* getGraph();
+    Point operator[](int index);
+    int getNumPoints();
+    int getNumEdges();
+
+    void dispose();
+private:
+    void compute_neighbors(double weight(Point, Point), double eps);
+    void do_gauss(double sigma);
+    void without_gauss();
+
+    std::vector<edge> edges;
+    std::vector<Point>& points;
+    int V;
+    int E;
+
+    int nr_neighbors;
+    float radius;
+
+
+    struct he{ int x; float w; };
+    std::vector< std::list<he> > adjency_list;
+};
+
+#endif

.svn/pristine/f5/f5984da835350005c283abf5b1b259e056026afd.svn-base

@@ -0,0 +1,36 @@
+FIND_PACKAGE(OpenCV REQUIRED)
+
+SET(FBR_IO_SRC scan_cv.cc)
+add_library(fbr_cv_io STATIC ${FBR_IO_SRC})
+
+SET(FBR_PANORAMA_SRC panorama.cc)
+#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC})
+add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc)
+
+
+SET(FBR_FEATURE_SRC feature.cc)
+add_library(fbr_feature STATIC ${FBR_FEATURE_SRC})
+
+SET(FBR_FEATURE_MATCHER_SRC feature_matcher.cc)
+add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC})
+
+SET(FBR_REGISTRATION_SRC registration.cc)
+add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC})
+
+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})
+
+add_executable(featurebasedregistration feature_based_registration.cc fbr_global.cc)
+#target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS})
+target_link_libraries(featurebasedregistration fbr ${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)

.svn/pristine/f6/f6689d2e558bed60b90f9bf5a6a989aac6928a7e.svn-base

@@ -0,0 +1,420 @@
+cmake_minimum_required (VERSION 2.8.2)
+SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH})
+project (Slam6D)
+
+#include_directories(OPENGL_INCLUDE_DIR)
+IF(WIN32)
+  set(Boost_USE_STATIC_LIBS TRUE)
+ELSE(WIN32)
+  set(Boost_USE_STATIC_LIBS FALSE)
+ENDIF(WIN32)
+
+SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49")
+IF(WIN32)
+  # for some unknown reason no one variant works on all windows platforms
+  find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
+ELSE(WIN32)
+  find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
+ENDIF(WIN32)
+
+if(Boost_FOUND)
+  link_directories(${BOOST_LIBRARY_DIRS})
+  include_directories(${Boost_INCLUDE_DIRS})
+  add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
+endif()
+
+#################################################
+# Declare Options and modify build accordingly ##
+#################################################
+
+## FreeGLUT
+OPTION(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON)
+
+IF(WITH_FREEGLUT)
+  MESSAGE(STATUS "With freeglut")
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT")
+ELSE(WITH_FREEGLUT)
+  MESSAGE(STATUS "Without freeglut")
+ENDIF(WITH_FREEGLUT)
+
+## Show
+OPTION(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON)
+
+IF(WITH_SHOW)
+  FIND_PACKAGE(OpenGL REQUIRED)
+  FIND_PACKAGE(GLUT REQUIRED)
+  MESSAGE(STATUS "With show")
+ELSE(WITH_SHOW)
+  #  SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE)
+  MESSAGE(STATUS "Without show")
+ENDIF(WITH_SHOW)
+
+## WXShow
+OPTION(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF)
+
+IF(WITH_WXSHOW)
+  FIND_PACKAGE(OpenGL REQUIRED)
+  FIND_PACKAGE(GLUT REQUIRED)
+  find_package(wxWidgets COMPONENTS core base gl REQUIRED)
+  # set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration
+  SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE)
+  # wxWidgets include (this will do all the magic to configure everything)
+  include( ${wxWidgets_USE_FILE})
+  MESSAGE(STATUS "With wxshow")
+ELSE(WITH_XWSHOW)
+  MESSAGE(STATUS "Without wxshow")
+ENDIF(WITH_WXSHOW)
+
+## Shapes
+OPTION(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF)
+IF(WITH_SHAPE_DETECTION)
+  MESSAGE(STATUS "With shape detection")
+ELSE(WITH_SHAPE_DETECTION)
+  MESSAGE(STATUS "Without shape detection")
+ENDIF(WITH_SHAPE_DETECTION)
+
+## Interior reconstruction
+option(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF)
+
+if(WITH_MODEL)
+  message(STATUS "With interior reconstruction")
+else(WITH_MODEL)
+  message(STATUS "Without interior reconstruction")
+endif(WITH_MODEL)
+
+## Thermo
+OPTION(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF)
+IF(WITH_THERMO)
+  FIND_PACKAGE(OpenCV REQUIRED)
+  add_subdirectory(3rdparty/cvblob)
+  include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
+  link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
+  MESSAGE(STATUS "With thermo")
+ELSE(WITH_THERMO)
+  MESSAGE(STATUS "Without thermo")
+ENDIF(WITH_THERMO)
+
+## Octree
+OPTION(WITH_OCTREE_DISPLAY "Whether to use octree display for efficiently culling scans ON/OFF" ON)
+
+IF(WITH_OCTREE_DISPLAY)
+  MESSAGE(STATUS "Using octree display")
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_GL_POINTS")
+ELSE(WITH_OCTREE_DISPLAY)
+  MESSAGE(STATUS "Using displaylists: Warning potentially much slower")
+ENDIF(WITH_OCTREE_DISPLAY)
+#SET (WITH_OCTREE_DISPLAY ${WITH_OCTREE_DISPLAY} CACHE BOOL
+#"Whether to use octree display for efficiently culling scans ON/OFF" FORCE)
+
+
+## Octree
+OPTION(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF)
+
+IF(WITH_COMPACT_OCTREE)
+  MESSAGE(STATUS "Using compact octrees")
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE")
+ELSE(WITH_COMPACT_OCTREE)
+  MESSAGE(STATUS "Not using compact octreees: Warning uses more memory")
+ENDIF(WITH_COMPACT_OCTREE)
+
+## Glee?
+OPTION(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF)
+
+IF(WITH_GLEE)
+  MESSAGE(STATUS "Using opengl extensions")
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE")
+ELSE(WITH_GLEE)
+  MESSAGE(STATUS "Not using opengl extensions")
+ENDIF(WITH_GLEE)
+
+## Gridder
+OPTION(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF)
+
+IF(WITH_GRIDDER)
+  MESSAGE(STATUS "With 2DGridder")
+ELSE(WITH_GRIDDER)
+  MESSAGE(STATUS "Without 2DGridder")
+ENDIF(WITH_GRIDDER)
+
+## Dynamic VELOSLAM
+OPTION(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF)
+
+IF(WITH_VELOSLAM)
+  MESSAGE(STATUS "With VELOSLAM")
+ELSE(WITH_VELOSLAM)
+  MESSAGE(STATUS "Without VELOSLAM")
+ENDIF(WITH_VELOSLAM)
+
+## Home-made Laserscanner
+OPTION(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF)
+
+IF(WITH_DAVID_3D_SCANNER)
+  MESSAGE(STATUS "With David scanner")
+ELSE(WITH_DAVID_3D_SCANNER)
+  MESSAGE(STATUS "Without David scanner")
+ENDIF(WITH_DAVID_3D_SCANNER)
+
+## Tools
+
+OPTION(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF)
+
+IF(WITH_TOOLS)
+  MESSAGE(STATUS "With Tools")
+ELSE(WITH_TOOLS)
+  MESSAGE(STATUS "Without Tools")
+ENDIF(WITH_TOOLS)
+
+## Segmentation
+
+OPTION(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF)
+
+IF(WITH_SEGMENTATION)
+  MESSAGE(STATUS "With segmentation")
+  find_package (Boost COMPONENTS program_options REQUIRED)
+ELSE(WITH_SEGMENTATION)
+  MESSAGE(STATUS "Without segmentation")
+ENDIF(WITH_SEGMENTATION)
+
+## CAD matching
+OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)
+
+IF (WITH_CAD)
+  MESSAGE (STATUS "With CAD import")
+  find_package (Boost COMPONENTS program_options filesystem REQUIRED)
+ELSE (WITH_CAD)
+  MESSAGE (STATUS "Without CAD import")
+ENDIF (WITH_CAD)
+
+## RivLib
+OPTION(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF)
+
+IF(WITH_RIVLIB)
+  MESSAGE(STATUS "Compiling a scan IO for RXP files")
+  include_directories(${CMAKE_SOURCE_DIR}/3rdparty)
+  link_directories(${CMAKE_SOURCE_DIR}/3rdparty)
+  SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/)
+  IF(WIN32)
+    SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib)
+  ELSE(WIN32)
+    SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread)
+  ENDIF(WIN32)
+  FIND_PACKAGE(LibXml2 )
+
+ELSE(WITH_RIVLIB)
+  MESSAGE(STATUS "Do NOT compile a scan IO for RXP")
+ENDIF(WITH_RIVLIB)
+
+## CUDA support, TODO depend on CUDA_FIND
+OPTION(WITH_CUDA "Compile with CUDA support" OFF)
+IF(WITH_CUDA)
+  MESSAGE(STATUS "Compiling WITH CUDA support")
+  FIND_PACKAGE(CUDA)
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA")
+ELSE(WITH_CUDA)
+  MESSAGE(STATUS "Compiling WITHOUT CUDA support")
+ENDIF(WITH_CUDA)
+
+## PMD
+OPTION(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF)
+
+IF(WITH_PMD)
+  FIND_PACKAGE(OpenGL REQUIRED)
+  MESSAGE(STATUS "With Tools")
+ELSE(WITH_PMD)
+  MESSAGE(STATUS "Without Tools")
+ENDIF(WITH_PMD)
+
+## FBR
+OPTION(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF)
+
+IF(WITH_FBR)
+  MESSAGE(STATUS "With FBR ")
+ELSE(WITH_FBR)
+  MESSAGE(STATUS "Without FBR")
+ENDIF(WITH_FBR)
+
+## Special treatment for system specifics
+IF(APPLE)
+add_definitions(-Dfopen64=fopen)
+ENDIF(APPLE)
+
+## Multiple Cores
+IF(APPLE)
+  SET(PROCESSOR_COUNT 2)
+ELSE(APPLE)
+  INCLUDE(CountProcessors)
+  SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" )
+ENDIF(APPLE)
+
+# OPEN
+FIND_PACKAGE(OpenMP)
+IF(OPENMP_FOUND)
+  OPTION(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON)
+ENDIF(OPENMP_FOUND)
+
+IF(OPENMP_FOUND AND WITH_OPENMP)
+  MESSAGE(STATUS "With OpenMP ")
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${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")
+ENDIF(OPENMP_FOUND AND WITH_OPENMP)
+
+## TORO
+OPTION(WITH_TORO "Whether to use TORO. ON/OFF" OFF)
+
+IF(WITH_TORO)
+  IF(WIN32)
+    SET(Subversion_SVN_EXECUTABLE "svn.exe")
+  ENDIF(WIN32)
+  cmake_minimum_required (VERSION 2.8)
+  include(ExternalProject)
+  ExternalProject_Add(toro3d
+    SVN_REPOSITORY https://www.openslam.org/data/svn/toro/trunk
+    SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/toro"
+    CONFIGURE_COMMAND ""
+    BUILD_COMMAND make
+    BUILD_IN_SOURCE 1
+    INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/toro/toro3d ${CMAKE_SOURCE_DIR}/bin/
+    )
+  MESSAGE(STATUS "With TORO ")
+ELSE(WITH_TORO)
+  MESSAGE(STATUS "Without TORO")
+ENDIF(WITH_TORO)
+
+
+## HOGMAN
+OPTION(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF)
+
+IF(WITH_HOGMAN)
+  # dependant on libqt4-devi
+  find_package( Qt4 REQUIRED)
+  # CMake of earlier versions do not have external project capabilities
+  cmake_minimum_required (VERSION 2.8)
+  include(ExternalProject)
+  ExternalProject_Add(hogman
+    SVN_REPOSITORY https://svn.openslam.org/data/svn/hog-man/trunk
+    SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/hogman"
+    CONFIGURE_COMMAND <SOURCE_DIR>/configure --prefix=<INSTALL_DIR>
+    BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make
+    BUILD_IN_SOURCE 1
+    INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
+    cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/
+    )
+  MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH")
+ELSE(WITH_HOGMAN)
+  MESSAGE(STATUS "Without HOGMAN")
+ENDIF(WITH_HOGMAN)
+
+OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF)
+
+IF(EXPORT_SHARED_LIBS)
+  MESSAGE(STATUS "exporting additional libraries")
+ELSE(EXPORT_SHARED_LIBS)
+  MESSAGE(STATUS "not exporting libraries")
+ENDIF(EXPORT_SHARED_LIBS)
+
+OPTION(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF)
+
+IF(WITH_METRICS)
+  MESSAGE(STATUS "With metrics in slam6d.")
+  SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS")
+ELSE(WITH_METRICS)
+  MESSAGE(STATUS "Without metrics in slam6d.")
+ENDIF(WITH_METRICS)
+
+IF(WIN32)
+  SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" )
+  include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/)
+  link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows)
+  link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64)
+  add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp)
+  SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib")
+ELSE(WIN32)
+  SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" )
+  # Add include path for OpenGL without GL/-prefix
+  # to avoid the include incompatibility between MACOS
+  # and linux
+    FIND_PATH(OPENGL_INC gl.h /usr/include/GL)
+    include_directories(${OPENGL_INC})
+ENDIF(WIN32)
+
+# Add OpenGL includes for MACOS if needed
+# The OSX OpenGL frameworks natively supports freeglut extensions
+IF(APPLE)
+  include_directories(/System/Library/Frameworks/GLUT.framework/Headers)
+  include_directories(/System/Library/Frameworks/OpenGL.framework/Headers)
+ENDIF(APPLE)
+
+SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}")
+
+# Hide CMake variables
+SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE)
+SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE)
+
+
+# Set output directories for libraries and executables
+SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib )
+SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj )
+SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin )
+
+# Set include and link dirs ...
+include_directories(${CMAKE_SOURCE_DIR}/include)
+include_directories(${CMAKE_SOURCE_DIR}/3rdparty/)
+include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui)
+include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/)
+include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include)
+include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src)
+link_directories(${CMAKE_SOURCE_DIR}/obj)
+link_directories(${CMAKE_SOURCE_DIR}/lib)
+
+add_subdirectory(3rdparty)
+add_subdirectory(src/slam6d)
+add_subdirectory(src/scanio)
+add_subdirectory(src/scanserver)
+add_subdirectory(src/segmentation)
+add_subdirectory(src/veloslam)
+add_subdirectory(src/show)
+add_subdirectory(src/grid)
+add_subdirectory(src/pmd)
+add_subdirectory(src/shapes)
+add_subdirectory(src/thermo)
+IF(WITH_FBR)
+  add_subdirectory(src/slam6d/fbr)
+ENDIF(WITH_FBR)
+add_subdirectory(src/scanner)
+add_subdirectory(src/model)
+
+IF(EXPORT_SHARED_LIBS)
+## Compiling a shared library containing all of the project
+add_library(slam SHARED src/slam6d/icp6D.cc)
+target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s)
+ENDIF(EXPORT_SHARED_LIBS)
+
+MESSAGE (STATUS "Build environment is set up!")
+
+
+# hack to "circumvent" Debug and Release folders that are created under visual studio
+# this is why the INSTALL target has to be used in visual studio
+IF(MSVC)
+  INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
+  IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
+	  INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
+  ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
+	  INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
+  ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
+
+  INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
+  IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
+    INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
+  ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
+    INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
+  ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
+ENDIF(MSVC)

.svn/pristine/f8/f88eab7718ced1859f76d8ca97e665745cb78707.svn-base

@@ -0,0 +1,11 @@
+IF(WITH_SEGMENTATION)
+  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)

.svn/pristine/fb/fb21832fa26a23fca92bb898aaaec41ef38a05d1.svn-base

@@ -0,0 +1,122 @@
+/*
+ * kdMeta implementation
+ *
+ * Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/** @file 
+ *  @brief An optimized k-d tree implementation. MetaScan variant.
+ *  @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
+ *  @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
+ */
+
+#ifdef _MSC_VER
+#define  _USE_MATH_DEFINES
+#endif
+
+#include "slam6d/kdMeta.h"
+#include "slam6d/globals.icc"
+#include "slam6d/scan.h"
+
+#include <iostream>
+using std::cout;
+using std::cerr;
+using std::endl;
+#include <algorithm>
+using std::swap;
+#include <cmath>
+#include <cstring>
+
+// KDtree class static variables
+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)
+{
+  // create scan pointer and data pointer arrays
+  m_size = scans.size();
+  m_scans = new Scan*[m_size];
+  for(unsigned int i = 0; i < m_size; ++i)
+    m_scans[i] = scans[i];
+  m_data = new DataXYZ*[m_size];
+  
+  lock();
+  create(m_data, prepareTempIndices(scans), getPointsSize(scans));
+  unlock();
+  
+  // allocate in prepareTempIndices, deleted here
+  delete[] m_temp_indices;
+}
+
+KDtreeMetaManaged::~KDtreeMetaManaged()
+{
+  delete[] m_scans;
+  delete[] m_data;
+}
+
+Index* KDtreeMetaManaged::prepareTempIndices(const vector<Scan*>& scans)
+{
+  unsigned int n = getPointsSize(scans);
+
+  m_temp_indices = new Index[n];
+  unsigned int s = 0, j = 0;
+  unsigned int scansize = scans[s]->size<DataXYZ>("xyz reduced");
+  for(unsigned int i = 0; i < n; ++i) {
+    m_temp_indices[i].set(s, j++);
+    // switch to next scan
+    if(j >= scansize) {
+      ++s;
+      j = 0;
+      if(s < scans.size())
+        scansize = scans[s]->size<DataXYZ>("xyz reduced");
+    }
+  }
+  return m_temp_indices;
+}
+
+unsigned int KDtreeMetaManaged::getPointsSize(const vector<Scan*>& scans)
+{
+  unsigned int n = 0;
+  for(vector<Scan*>::const_iterator it = scans.begin(); it != scans.end(); ++it) {
+    n += (*it)->size<DataXYZ>("xyz reduced");
+  }
+  return n;
+}
+
+double* KDtreeMetaManaged::FindClosest(double *_p, double maxdist2, int threadNum) const
+{
+  params[threadNum].closest = 0;
+  params[threadNum].closest_d2 = maxdist2;
+  params[threadNum].p = _p;
+  _FindClosest(m_data, threadNum);
+  return params[threadNum].closest;
+}
+
+void KDtreeMetaManaged::lock()
+{
+  boost::lock_guard<boost::mutex> lock(m_mutex_locking);
+  if(m_count_locking == 0) {
+    // lock all the contained scans, metascan uses the transformed points
+    for(unsigned int i = 0; i < m_size; ++i) {
+      m_data[i] = new DataXYZ(m_scans[i]->get("xyz reduced"));
+    }
+  }
+  ++m_count_locking;
+}
+
+void KDtreeMetaManaged::unlock()
+{
+  boost::lock_guard<boost::mutex> lock(m_mutex_locking);
+  --m_count_locking;
+  if(m_count_locking == 0) {
+    // delete each locking object
+    for(unsigned int i = 0; i < m_size; ++i) {
+      delete m_data[i];
+    }
+  }
+}

.svn/pristine/fe/fec41303bc005ae264f9cb242d23dfb77bcf2163.svn-base

@@ -0,0 +1,170 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+
+
+#ifdef _MSC_VER
+#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 <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 */
+extern float frustum[6][4];
+
+/** the modelview * projection matrix to map a model point to an onscreen coordinate */
+extern float matrix[16];
+/** some useful variables for faster calculation of the pixel coordinates */
+extern short VP[4];
+/** a unit vector pointing to the right of the screen */
+extern float right[3];
+/** how much detail is shown, 0 means everything is plotted */
+extern short DETAIL;
+
+extern double SX, SY, SZ, EX, EY, EZ;
+extern float origin[3], dir[3];   /*ray */
+extern float dist;
+extern int rayX,rayY;
+extern float  rayVP[4];
+
+#define NUMDIM  3
+#define RIGHT 0
+#define LEFT  1
+#define MIDDLE  2
+
+extern float minB[NUMDIM], maxB[NUMDIM];    /*box */
+extern float coord[NUMDIM];       /* hit point */
+
+
+template <class T> bool HitBoundingBox(const T center[3], T size )
+{
+  minB[0] = center[0] - size;
+  minB[1] = center[1] - size;
+  minB[2] = center[2] - size;
+
+  maxB[0] = center[0] + size;
+  maxB[1] = center[1] + size;
+  maxB[2] = center[2] + size;
+
+  bool inside = true;
+  char quadrant[NUMDIM];
+  register int i;
+  int whichPlane;
+  float maxT[NUMDIM];
+  float candidatePlane[NUMDIM];
+
+  // Find candidate planes; this loop can be avoided if
+  //  rays cast all from the eye(assume perpsective view) 
+  for (i=0; i<NUMDIM; i++)
+    if(origin[i] < minB[i]) {
+      quadrant[i] = LEFT;
+      candidatePlane[i] = minB[i];
+      inside = false;
+    }else if (origin[i] > maxB[i]) {
+      quadrant[i] = RIGHT;
+      candidatePlane[i] = maxB[i];
+      inside = false;
+    }else {
+      candidatePlane[i] = 0.0;
+      quadrant[i] = MIDDLE;
+    }
+
+  // Ray origin inside bounding box 
+  if(inside)  {
+    return (true);
+  }
+
+
+  // Calculate T distances to candidate planes
+  for (i = 0; i < NUMDIM; i++)
+    if (quadrant[i] != MIDDLE && dir[i] !=0.)
+      maxT[i] = (candidatePlane[i]-origin[i]) / dir[i];
+    else
+      maxT[i] = -1.;
+
+  // Get largest of the maxT's for final choice of intersection 
+  whichPlane = 0;
+  for (i = 1; i < NUMDIM; i++)
+    if (maxT[whichPlane] < maxT[i])
+      whichPlane = i;
+
+  // Check final candidate actually inside box 
+  if (maxT[whichPlane] < 0.) return (false);
+  for (i = 0; i < NUMDIM; i++)
+    if (whichPlane != i) {
+      coord[i] = origin[i] + maxT[whichPlane] *dir[i];
+      if (coord[i] < minB[i] || coord[i] > maxB[i])
+        return (false);
+    } else {
+      coord[i] = candidatePlane[i];
+    }
+  return (true);        // ray hits box
+} 
+
+void calcRay(int x, int y, double znear, double zfar);  
+
+#include <float.h>
+template <class T> 
+float RayDist(T *point)
+{
+  return point[0] * dir[0] + point[1] * dir[1] + point[2] * dir[2] - dist;
+}
+
+template <class T> 
+short ScreenDist(T *point) {
+  float pn[3];
+  // x coordinate on screen, not normalized
+  pn[0] = point[0] * matrix[0] + point[1] * matrix[4] + point[2] * matrix[8]  + matrix[12];
+  pn[1] = point[0] * matrix[1] + point[1] * matrix[5] + point[2] * matrix[9]  + matrix[13];
+  // normalization
+  pn[2] = point[0] * matrix[3] + point[1] * matrix[7] + point[2] * matrix[11] + matrix[15];
+
+  // normalized x coordinate on screen
+  pn[0] /= pn[2];
+  pn[1] /= pn[2];
+
+  // true x coordinate in viewport coordinate system
+  //Xi = pn[0]*VP[0] + VP[1];
+  //fTempo[4]*0.5+0.5)*viewport[2]+viewport[0];
+  
+  float XX = ( (pn[0])*rayVP[0] + rayVP[1]);
+  float YY = ( (pn[1])*rayVP[2] + rayVP[3]);
+
+  short dx, dy;
+  if (XX > rayX) dx = XX-rayX;
+  else dx = rayX-XX;
+  
+  if (YY > rayY) dy = YY-rayY;
+  else dy = rayY-YY;
+
+  // for the benefit of visual studio's compiler cast to float
+  return sqrt((float)(dx*dx + dy*dy));
+}
+
+void ExtractFrustum(short detail);
+void ExtractFrustum(float *frust[6]);
+
+
+bool CubeInFrustum( float x, float y, float z, float size );
+int  CubeInFrustum2( float x, float y, float z, float size );
+char PlaneAABB( float x, float y, float z, float size, float *plane );
+
+void remViewport();
+bool LOD(float x, float y, float z, float size);
+int LOD2(float x, float y, float z, float size);
+
+}
+#endif