diff --git a/.svn/pristine/02/0266ada4f3e0365a9d424292ae86bd3186b49387.svn-base b/.svn/pristine/02/0266ada4f3e0365a9d424292ae86bd3186b49387.svn-base new file mode 100644 index 0000000..11318b0 --- /dev/null +++ b/.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 + +#include +using std::vector; +#include +using std::deque; +#include +using std::set; +#include +using std::list; +#include +#include +#include + +#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 &vp, double center[3], double size) { + double ccenter[3]; + cbitunion *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&c, cbitoct &node, double *center, double size) { + double ccenter[3]; + cbitunion *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 *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 *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 *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 *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 *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 *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 *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 *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 *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 *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 *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 *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 +void compactTree::selectRay(vector &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&c) { GetOctTreeCenter(c, *root, center, size); } +void compactTree::AllPoints(vector &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::root, BOctTree::center, BOctTree::size, BOctTree::size/ pow(2, min( (int)(ratio * BOctTree::max_depth ), BOctTree::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 (POINTDIM*length); + return points; +} +void compactTree::deserialize(std::string filename ) { + char buffer[sizeof(float) * 20]; + float *p = reinterpret_cast(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(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(fmins), POINTDIM * sizeof(float)); + file.read(reinterpret_cast(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(); + 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 *children = new cbitunion[n_children]; + cbitunion *children = alloc->allocate >(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(&length), sizeof(lint)); + shortpointrep *points = createPoints(length); + + f.read(reinterpret_cast(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(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(&(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 *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(&length), sizeof(lint) ); + + of.write(reinterpret_cast(points), POINTDIM*length*sizeof(tshort) ); + + } else { // write child + serialize(of, children->node); + } + ++children; // next child + } + } +} diff --git a/.svn/pristine/04/048d5ca5d56fbbc8f62b96fad293aa3647b1b764.svn-base b/.svn/pristine/04/048d5ca5d56fbbc8f62b96fad293aa3647b1b764.svn-base new file mode 100644 index 0000000..94eaac8 --- /dev/null +++ b/.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 +#include +#include +#include + +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 > + * 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 0 && nr_neighbors < nret ) + { + random_shuffle(n, n+nret); + nret = nr_neighbors; + } + + for (int j=0; j& v) +{ + double s = 0; + for (size_t i=0; i::iterator k, j; + vector gauss_weight, edge_weight; + edge e; +#pragma omp parallel for private(j, k, e, gauss_weight, edge_weight) schedule(dynamic) + for (int i=0; iw, 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::iterator j; + edge e; + + for (int i=0; ix; e.w = j->w; + edges.push_back(e); + } + } +} + +edge* FHGraph::getGraph() +{ + edge* ret = new edge[edges.size()]; + for (size_t i=0; i +void vectorFree(T& t) { + T tmp; + t.swap(tmp); +} + +void FHGraph::dispose() { + vectorFree(edges); + vectorFree(points); + vectorFree(adjency_list); +} + diff --git a/.svn/pristine/0b/0b791769e2124bae1b4af44cbb37f799db11c8d1.svn-base b/.svn/pristine/0b/0b791769e2124bae1b4af44cbb37f799db11c8d1.svn-base new file mode 100644 index 0000000..8d5e7f8 --- /dev/null +++ b/.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) diff --git a/.svn/pristine/13/137cef42436c2c4ae9351f5632a48c0ed06ca884.svn-base b/.svn/pristine/13/137cef42436c2c4ae9351f5632a48c0ed06ca884.svn-base new file mode 100644 index 0000000..c32d71d --- /dev/null +++ b/.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 +#include +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#include +using std::ofstream; +using std::flush; +using std::cout; +using std::string; +using std::cerr; +using std::endl; +#include + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#include +#include +#else +#include +#include +#include +#include +#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=)" << endl + << endl + << bold << " -O" << normal << " NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << 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 points; + CollisionPlane * plane; + plane = new CollisionPlane(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(); +} + diff --git a/.svn/pristine/1a/1a4914ee1edbcfa2063c6bbb823444a5fec55e6b.svn-base b/.svn/pristine/1a/1a4914ee1edbcfa2063c6bbb823444a5fec55e6b.svn-base new file mode 100644 index 0000000..cea3058 --- /dev/null +++ b/.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 +using std::cout; +using std::cerr; +using std::endl; +#include +using std::swap; +#include +#include + +// KDtree class static variables +template +KDParams KDTreeImpl::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; +} diff --git a/.svn/pristine/1b/1bc82d9ef314c49f5f3477cfcd5b20f21d500a62.svn-base b/.svn/pristine/1b/1bc82d9ef314c49f5f3477cfcd5b20f21d500a62.svn-base new file mode 100644 index 0000000..3c53adb --- /dev/null +++ b/.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 +#include + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#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 +{ +public: + KDtreeMetaManaged(const vector& 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& scans); + unsigned int getPointsSize(const vector& scans); +}; + +#endif diff --git a/.svn/pristine/1f/1ff694340f907a92134585612918621390a04042.svn-base b/.svn/pristine/1f/1ff694340f907a92134585612918621390a04042.svn-base new file mode 100644 index 0000000..2825089 --- /dev/null +++ b/.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 + * @author Mihai-Cotizo Sima + * @file fhsegmentation.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +/// validate IO types +void validate(boost::any& v, const std::vector& 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 where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library 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(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1), "neglegt all data points with a distance smaller than 'units") + ("K,k", po::value(&k)->default_value(1), " value of K value used in the FH segmentation") + ("neighbors,N", po::value(&neighbors)->default_value(1), "use approximate -nearest neighbors search or limit the number of points") + ("sigma,v", po::value(&sigma)->default_value(1.0), "Set the Gaussian variance for smoothing to ") + ("radius,r", po::value(&radius)->default_value(-1.0), "Set the range of radius search to ") + ("eps,E", po::value(&eps)->default_value(1.0), "Set error threshold used by the AKNN algorithm to ") + ("minsize,z", po::value(&min_size)->default_value(0), "Keep segments of size at least ") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&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* > cloud) +{ + for (int i = outnum, j = 0; i < (int)cloud.size() && j < (int)cloud.size(); i++, j++) { + vector* 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::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 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* > clouds; + clouds.reserve(nr); + for (int i=0; i ); + + map 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; +} + diff --git a/.svn/pristine/21/21c8eebf31843a931b8d4277e259942c97b42530.svn-base b/.svn/pristine/21/21c8eebf31843a931b8d4277e259942c97b42530.svn-base new file mode 100644 index 0000000..8c4ad8f --- /dev/null +++ b/.svn/pristine/21/21c8eebf31843a931b8d4277e259942c97b42530.svn-base @@ -0,0 +1,2497 @@ +#include +#include "thermo/thermo.h" +#include "newmat/newmatap.h" +using namespace NEWMAT; + +#include "cvblob.h" +using namespace cvb; + +#include + +#ifndef _MSC_VER +#include +#include +#else +#include "XGetopt.h" +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#include +#include +#endif + +#ifdef _EiC +#define WIN32 +#endif + +Float2D data1; +Float2D data2; + + +unsigned int BLOB_SIZE = 65; +double AVG_THRES = 0.8; +unsigned int GRAY_TH = 65; +/** + * Calculates the PCA of a two-dimensional point cloud + * @param x x coordinate of the axis + * @param y y coordinate of the axis + * @param pc true if the principal axis is wanted, false for the least dominant + * @param cx center x of the point cloud + * @param cy center y of the point cloud + */ + +void calcBoard(double point_array[][2], int board_n, double &x, double &y, double &cx, double &cy, bool pc) { + cx = cy = 0; + for (int a = 0; a < board_n; a++) { + cx += point_array[a][0]; + cy += point_array[a][1]; + } + + cx /= board_n; + cy /= board_n; + + SymmetricMatrix A(2); + A = 0; + + for(int a = 0; a < board_n; a++) { + A(1,1) += (point_array[a][0] - cx)*(point_array[a][0] - cx); + A(2,2) += (point_array[a][1] - cy)*(point_array[a][1] - cy); + A(1,2) += (point_array[a][0] - cx)*(point_array[a][1] - cy); + } + DiagonalMatrix D; + Matrix V; + try { + Jacobi(A,D,V); + } catch (ConvergenceException) { + cout << "couldn't find board..." << endl; + } + + int min, max; + + D.MaximumAbsoluteValue1(max); + D.MinimumAbsoluteValue1(min); + + // return eigenvector with highest eigenvalue + if(pc) { + x = V(1,max); + y = V(2,max); + // return eigenvector with lowest eigenvalue + } else { + x = V(1,min); + y = V(2,min); + } + +} + +/** + * Sorts the detected light bulbs on the board. + * @param point_array list of detected points + * @param board_n number of lightbulbs + * @param board_h number of rows + * @param board_w number of columns + * @param quiet if true, debug information is printed + */ +void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, bool quiet) { + double x, y, cx, cy; + // align board using PCA + calcBoard(point_array, board_n, x, y, cx, cy, board_h <= board_w); + double point_array2[board_n][2]; + double angle = -atan2(y,x); + for(int i = 0; i < board_n; i++) { + double tmpx = point_array[i][0] - cx; + double tmpy = point_array[i][1] - cy; + point_array2[i][0] = tmpx * cos(angle) - tmpy * sin(angle); + point_array2[i][1] = tmpx * sin(angle) + tmpy * cos(angle); + } + // sorting the points on the basis of y coordinate////// + int swapped1 = 0; + do { + swapped1 = 0; + + for (int a = 1; a <= board_n - 1; a++) { + if (point_array2[a][1] < point_array2[a - 1][1]) { + //rotated points + double tempx = point_array2[a][0]; + double tempy = point_array2[a][1]; + point_array2[a][0] = point_array2[a - 1][0]; + point_array2[a][1] = point_array2[a - 1][1]; + point_array2[a - 1][0] = tempx; + point_array2[a - 1][1] = tempy; + + //original points + double tmpx = point_array[a][0]; + double tmpy = point_array[a][1]; + point_array[a][0] = point_array[a - 1][0]; + point_array[a][1] = point_array[a - 1][1]; + point_array[a - 1][0] = tmpx; + point_array[a - 1][1] = tmpy; + swapped1 = 1; + } + } + } while (swapped1 == 1); + + if(!quiet) { + cout << "sorted array:" << endl; + for (int f = 0; f < board_n; f++) { + cout << point_array2[f][0] << " " << point_array2[f][1] << endl; + } + } + // sorting the array rows now + for (int x = 0; x < board_h; x++) { + double row_points[board_w][2]; + double row_points2[board_w][2]; + for (int y = 0; y < board_w; y++) { + row_points[y][0] = point_array[x * board_w + y][0]; + row_points[y][1] = point_array[x * board_w + y][1]; + row_points2[y][0] = point_array2[x * board_w + y][0]; + row_points2[y][1] = point_array2[x * board_w + y][1]; + if(!quiet) cout << row_points[y][0] << " " << row_points[y][1] << " "; + } + if(!quiet) cout << endl; + int swapped = 0; + do { + swapped = 0; + for (int a = 1; a <= board_w - 1; a++) { + if (row_points2[a][0] < row_points2[a - 1][0]) { + // original points + double tempx = row_points[a][0]; + double tempy = row_points[a][1]; + row_points[a][0] = row_points[a - 1][0]; + row_points[a][1] = row_points[a - 1][1]; + row_points[a - 1][0] = tempx; + row_points[a - 1][1] = tempy; + // rotated points + double tmpx = row_points2[a][0]; + double tmpy = row_points2[a][1]; + row_points2[a][0] = row_points2[a - 1][0]; + row_points2[a][1] = row_points2[a - 1][1]; + row_points2[a - 1][0] = tmpx; + row_points2[a - 1][1] = tmpy; + swapped = 1; + } + } + } while (swapped == 1); + if(!quiet) cout << "sorted:" << endl; + for (int z = 0; z < board_w; z++) { + point_array2[x * board_w + z][0] = row_points2[z][0]; + point_array2[x * board_w + z][1] = row_points2[z][1]; + if(!quiet) cout << point_array[x * board_w + z][0] << " " << point_array[x * board_w + z][1] << " "; + point_array[x * board_w + z][0] = row_points[z][0]; + point_array[x * board_w + z][1] = row_points[z][1]; + } + if(!quiet) cout << endl; + + } + +} + +/** + * Detects the blobs. + */ +IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]) { + + IplImage *gray_image = cvCloneImage(org_image); + //cvThreshold(gray_image, gray_image, 100, 255, CV_THRESH_BINARY); + cvThreshold(gray_image, gray_image, GRAY_TH, 255, CV_THRESH_BINARY); + IplImage *labelImg = cvCreateImage(cvGetSize(gray_image), IPL_DEPTH_LABEL, 1); + + // detect blobs + CvBlobs blobs; + cvLabel(gray_image, labelImg, blobs); + double average_size = 0; + int count = 0; + for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { + if (it->second->area < BLOB_SIZE) { + count++; + average_size += it->second->area; + } + } + if(!quiet) cout << "centroid:" << average_size << endl; + + // refine blobs + average_size = average_size / count; + double average_size_min = average_size * (1.0 - AVG_THRES); + double average_size_max = average_size * (1.0 + AVG_THRES); + int blob_count = 0; + + for (CvBlobs::const_iterator it2 = blobs.begin(); it2 != blobs.end(); ++it2) { + if (it2->second->area >= average_size_min && + it2->second->area <= average_size_max && + blob_count < corner_exp) { + + point_array2[blob_count][0] = it2->second->centroid.x; + point_array2[blob_count][1] = it2->second->centroid.y; + double sumx = 0.0; + double sumy = 0.0; + double sum = 0.0; + + /* + int step = 5; + int minx = ((int)it2->second->minx - step) > -1 ? (it2->second->minx - step) : 0; + int maxx = ((it2->second->maxx + step) < gray_image->width) ? (it2->second->maxx + step) : (gray_image->width - 1); + int miny = ((int)it2->second->miny - step) > -1 ? (it2->second->miny - step) : 0; + int maxy = ((it2->second->maxy + step) < gray_image->height) ? (it2->second->maxy + step) : (gray_image->height - 1); + */ + + int minx = it2->second->minx; + int miny = it2->second->miny; + int maxx = it2->second->maxx; + int maxy = it2->second->maxy; + + for(int x = minx; x <= maxx; x++) { + for(int y = miny; y <= maxy; y++) { + if(cvGet2D(gray_image, y, x).val[0] > 0) { + CvScalar c; + c = cvGet2D(org_image, y, x); + sum += c.val[0]; + sumx += c.val[0]*x; + sumy += c.val[0]*y; + } + } + } + + sumx /= sum; + sumy /= sum; + point_array2[blob_count][0] = sumx; + point_array2[blob_count][1] = sumy; + + blob_count++; + } + } + + if(!quiet) cout << "Refined number of blobs=" << blob_count << endl; + // sorting the points + sortBlobs(point_array2, corner_exp, board_h, board_w, true); + cvReleaseImage(&labelImg); + corner_exp = blob_count; + return gray_image; +} + +/** + * Connects the detected calibration features in the image with lines. + */ +void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color) { + for (int i = 4; i <= corner_exp - 2; i++) { + CvPoint pt1; + CvPoint pt2; + CvScalar s; + if(color) { + s = CV_RGB(255,0,0); + } else { + s.val[0] = 100; + } + double temp1 = point_array2[i][0] - floor(point_array2[i][0]); + if (temp1 < .5) { + pt1.x = floor(point_array2[i][0]); + } else { + pt1.x = floor(point_array2[i][0]) + 1; + } + double temp2 = point_array2[i][1] - floor(point_array2[i][1]); + if (temp2 < .5) { + pt1.y = floor(point_array2[i][1]); + } else { + pt1.y = floor(point_array2[i][1]) + 1; + } + double temp3 = point_array2[i + 1][0] - floor( + point_array2[i + 1][0]); + if (temp3 < .5) { + pt2.x = floor(point_array2[i + 1][0]); + } else { + pt2.x = floor(point_array2[i + 1][0]) + 1; + } + double temp4 = point_array2[i + 1][1] - floor( + point_array2[i + 1][1]); + if (temp4 < .5) { + pt2.y = floor(point_array2[i + 1][1]); + } else { + pt2.y = floor(point_array2[i + 1][1]) + 1; + } + cvLine(image, pt1, pt2, s, 3, 8); + } + cvShowImage("Final Result", image); + +} + +/** + * Resizes the image + */ +IplImage* resizeImage(IplImage *source, int scale) { + int width, height; + IplImage *image; + switch(scale) { + case 2: + width = 1200; + height = 900; + break; + case 3: + width = 800; + height = 600; + break; + case 4: + width = 400; + height = 300; + break; + case 5: + width = 160; + height = 120; + break; + case 1: + default: + return cvCloneImage(source); + } + + image = cvCreateImage(cvSize(width,height),source->depth,source->nChannels); + cvResize(source,image); + return image; +} + +/** + * Detects the corners of the chessboard pattern. + */ +IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale) { + + cout << "Scale: " << scale << endl; + IplImage *image = resizeImage(orgimage, scale); + CvSize size = cvGetSize(image); + CvPoint2D32f* corners = new CvPoint2D32f[corner_exp]; + CvSize board_sz = cvSize(board_w, board_h); + IplImage *gray_image = cvCreateImage(size,8,1); + + if (image->nChannels == 3) { + cvCvtColor(image, gray_image, CV_BGR2GRAY); + } else { + gray_image = image; + } + + int found = cvFindChessboardCorners(image, board_sz, corners, &corner_exp, + CV_CALIB_CB_ADAPTIVE_THRESH + | CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS); + + cout << "found corners:" << corner_exp << endl; + if (found != 0) {//if all corners found successfully + //if (corner_exp != 0) {//if all corners found successfully + //Get Subpixel accuracy on those corners + if(size.width > 400) { + cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(11, 11), cvSize(-1, -1), + cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + } + else { + cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(3, 3), cvSize(-1, -1), + cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + } + } + cout << "blub " << found << endl; + + for (int i = 0; i < corner_exp; i++) { + point_array2[i][0] = corners[i].x; + point_array2[i][1] = corners[i].y; + } + return gray_image; + +} + +/** + * Writes the intrinsic calibration parameters to files. + */ +void writeCalibParam(int images, int corner_exp, int board_w, CvMat* +image_points, CvSize size, string dir, string substring) { + CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1); + CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1); + //ALLOCATE MATRICES ACCORDING TO HOW MANY CHESSBOARDS FOUND + CvMat* object_points2 = cvCreateMat(images * corner_exp, 3, CV_32FC1); + CvMat* image_points2 = cvCreateMat(images * corner_exp, 2, CV_32FC1); + CvMat* point_counts2 = cvCreateMat(images, 1, CV_32SC1); + CvMat* Rotation = cvCreateMat(images, 3, CV_32FC1); + CvMat* Translation = cvCreateMat(images, 3, CV_32FC1); + //TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES + int j; + for (int i = 0; i < images * corner_exp; ++i) { + j = i % corner_exp; + CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0); + CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1); + CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; + CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; + //CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; + //CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; + CV_MAT_ELEM( *object_points2, float, i, 2) = 0.0f; + } + for (int i = 0; i < images; ++i) { //These are all the same number + CV_MAT_ELEM( *point_counts2, int, i, 0) = corner_exp; + } + + // Initialize the intrinsic matrix with focal length = 1.0 + CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f; + CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f; + //CALIBRATE THE CAMERA! + cvCalibrateCamera2(object_points2, image_points2, point_counts2, size, + intrinsic_matrix, distortion_coeffs, Rotation, Translation, 0 //CV_CALIB_FIX_ASPECT_RATIO + ); + // SAVE AND PRINT THE INTRINSICS AND DISTORTIONS + + string file = dir + "Intrinsics" + substring + ".xml"; + cvSave(file.c_str(), intrinsic_matrix); + file = dir + "Distortion" + substring + ".xml"; + cvSave(file.c_str(), distortion_coeffs); + cout << "Camera Intrinsic Matrix is:" << endl; + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + cout << CV_MAT_ELEM( *intrinsic_matrix, float, row, col ) << "\t"; + } + cout << endl; + } + cout << "Distortion Coefficients are:" << endl; + for (int row = 0; row < 5; row++) { + for (int col = 0; col < 1; col++) { + cout << CV_MAT_ELEM( *distortion_coeffs, float, row, col ) << "\t"; + } + } + cout << endl; + CvMat *intrinsic = intrinsic_matrix; + CvMat *distortion = distortion_coeffs; + + // CLEANUP + cvReleaseMat(&object_points2); + cvReleaseMat(&image_points2); + cvReleaseMat(&point_counts2); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); +} + +/** + * Main function for intrinsic calibration + */ +void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool +chess, bool quiet, string dir, int scale) { + cvNamedWindow("Original Image", 0); + cvNamedWindow("Final Result", 0); + cvResizeWindow( "Original Image", 480, 640 ); + cvResizeWindow( "Final Result", 480, 640 ); + /* + cvNamedWindow("Final Result", 0); + cvResizeWindow( "Final Result", 320, 240 ); + */ + int nr_img = end - start + 1; + if (nr_img == 0) { + cout << "ImageCount is zero!" << endl; + return; + } + + int corner_exp = board_w * board_h; + CvSize board_sz = cvSize(board_w, board_h); + CvSize size; + //ALLOCATE STORAGE(depending upon the number of images in(in case if command line arguments are given ) + //not on the basis of number of images in which all corner extracted/while in the other case the number is the same ) + CvMat* image_points = cvCreateMat(nr_img * corner_exp, 2, CV_32FC1); + //TODO CvPoint2D32f* corners = new CvPoint2D32f[ board_n ]; + + int successes = 0; + int step = 0; + + for (int count = start; count <= end; count++) { + cout << "count : " << count << endl; + string t; + string t1; + + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + t = dir + "/photo" + to_string(count, 3) + ".jpg"; + //t1 = dir + "/cimage" + to_string(count, 3) + ".ppm"; + //t = dir + to_string(count, 3) + "/photo" + to_string(count, 3) + ".ppm"; + //t1 = dir + to_string(count, 3) + "/cimage" + to_string(count, 3) + ".ppm"; + } else { + //t = dir + to_string(count, 3) + "/image" + to_string(count, 3) + ".ppm"; + //t1 = dir + to_string(count, 3) + "/timage" + to_string(count, 3) + ".ppm"; + t = dir + "/image" + to_string(count, 3) + ".ppm"; + //t1 = dir + "/timage" + to_string(count, 3) + ".ppm"; + } + cout << t << endl; + //loading images and finding corners + IplImage* image1 = cvLoadImage(t.c_str(), -1); + if (!image1) { + cout << "image cannot be loaded" << endl; + return; + } + cvShowImage("Original Image", image1); + + ///////////////////////////////////////////////////////////// + + double point_array2[corner_exp][2]; + IplImage *image; + + int tmp_corners = corner_exp; + if(chess) { + cout << "detect corners" << endl; + image = detectCorners(image1, tmp_corners, board_h, board_w, quiet, point_array2, scale); + } else { + cout << "detect blob" << endl; + image = detectBlobs(image1, tmp_corners, board_h, board_w, quiet, point_array2); + } + + /* + for(int i = 0; i < corner_exp; i++) { + cout << (float) point_array2[i][0] << " " << (float) point_array2[i][1] << + endl; + } + + //drawing the lines on the image now + */ + drawLines(point_array2, corner_exp, image); + //cvDrawChessboardCorners(image, size, point_array2, tmp_corners, true); + //cvShowImage("Final Result", image); + char in; + if(tmp_corners == corner_exp) { + cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; + in = 'y'; + /* + int c = cvWaitKey(100); + if (c == 27) { + break; + } + cin >> in; + */ + if (in == 'y') { + //cvSaveImage(t1.c_str(), image); + size = cvGetSize(image); + step = successes * corner_exp; + //appending corner data to a generic data structure for all images + for (int i = step, j = 0; j < corner_exp; ++i, ++j) { + CV_MAT_ELEM(*image_points, float,i,0) = (float) point_array2[j][0]; + CV_MAT_ELEM(*image_points, float,i,1) = (float) point_array2[j][1]; + } + successes++; + } else if(in == 'x') { + break; + } + } + cvReleaseImage(&image); + cvReleaseImage(&image1); + + } + cout << "Images for which all corners were found successfully=" + << successes << endl; + if (successes == 0) { + cout << "No successful corners found from any image" << endl; + return; + } + + string substring = optical? "Optical" : ""; + writeCalibParam(successes, corner_exp, board_w, image_points, size, dir, substring); + + cvReleaseMat(&image_points); + +} + +/** + * Reads the 3D information of the features from a file. + */ +bool readPoints(string filename, CvPoint3D32f *corners, int size) { + ifstream infile(filename.c_str(), ios::in); + if (!infile) { + cout << "3Ddata file cannot be loaded" << endl; + return false; + } + + string verify; + infile >> verify; + if(strcmp(verify.c_str(), "failed") == 0) return false; + for(int l = 0; l < size; l++) { + infile >> corners[l].y; + infile >> corners[l].z; + infile >> corners[l].x; + corners[l].y = -corners[l].y; + } + return true; +} + +/** + * Calculates the median of a set of translation vectors, i.e., the translation + * that has the smallest distance to all other translation. + */ +int realMedian(CvMat * vectors, int nr_vectors) { + double distances[nr_vectors]; + + for(int i = 0; i < nr_vectors; i++) { + double sum = 0; + double x1 = (CV_MAT_ELEM(*vectors,float,i,0)); + double y1 = (CV_MAT_ELEM(*vectors,float,i,1)); + double z1 = (CV_MAT_ELEM(*vectors,float,i,2)); + for(int j = 0; j < nr_vectors; j++) { + double x2 = (CV_MAT_ELEM(*vectors,float,j,0)); + double y2 = (CV_MAT_ELEM(*vectors,float,j,1)); + double z2 = (CV_MAT_ELEM(*vectors,float,j,2)); + double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); + sum += sqrt(tmp); + } + distances[i] = sum; + } + int min_pos = -1; + double min_dist = DBL_MAX; + for(int i = 0; i < nr_vectors; i++) { + if(distances[i] < min_dist) { + min_pos = i; + min_dist = distances[i]; + } + } + + return min_pos; +} + +/* + * Calculates the median of a set of vectors by iteratively calculating the + * median and cropping outliers. + */ +void filterMedian(CvMat * vectors, int nr_vectors, int thres, CvMat * mean) { + int threshold = thres; + // calculate Median + int min_pos = realMedian(vectors, nr_vectors); + if(thres == 4) { + (CV_MAT_ELEM(*mean,float,0,0)) = (CV_MAT_ELEM(*vectors,float,min_pos,0)); + (CV_MAT_ELEM(*mean,float,0,1)) = (CV_MAT_ELEM(*vectors,float,min_pos,1)); + (CV_MAT_ELEM(*mean,float,0,2)) = (CV_MAT_ELEM(*vectors,float,min_pos,2)); + (CV_MAT_ELEM(*mean,float,0,3)) = (CV_MAT_ELEM(*vectors,float,min_pos,3)); + (CV_MAT_ELEM(*mean,float,0,4)) = (CV_MAT_ELEM(*vectors,float,min_pos,4)); + (CV_MAT_ELEM(*mean,float,0,5)) = (CV_MAT_ELEM(*vectors,float,min_pos,5)); + return; + } + + // crop outliers + double x1 = (CV_MAT_ELEM(*vectors,float,min_pos,0)); + double y1 = (CV_MAT_ELEM(*vectors,float,min_pos,1)); + double z1 = (CV_MAT_ELEM(*vectors,float,min_pos,2)); + + int count = 0; + for(int i = 0; i < nr_vectors; i++) { + double x2 = (CV_MAT_ELEM(*vectors,float,i,0)); + double y2 = (CV_MAT_ELEM(*vectors,float,i,1)); + double z2 = (CV_MAT_ELEM(*vectors,float,i,2)); + double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); + if(sqrt(tmp) < 1.0/threshold) count++; + } + + CvMat* some_vectors = cvCreateMat(count, 6, CV_32FC1); + count = 0; + for(int i = 0; i < nr_vectors; i++) { + double x2 = (CV_MAT_ELEM(*vectors,float,i,0)); + double y2 = (CV_MAT_ELEM(*vectors,float,i,1)); + double z2 = (CV_MAT_ELEM(*vectors,float,i,2)); + double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); + if(sqrt(tmp) < 1.0/threshold) { + for(int j = 0; j < 6; j++) { + (CV_MAT_ELEM(*some_vectors,float,count,j)) = (CV_MAT_ELEM(*vectors,float,i,j)); + cout << (CV_MAT_ELEM(*some_vectors,float,count,j)) << " "; + } + cout << endl; + count++; + } + } + cout << "min_pos " << min_pos << endl; + // recurse + if(thres < 3) { + filterMedian(some_vectors, count, ++threshold, mean); + cvReleaseMat(&some_vectors); + // determine result + } else { + /* + x1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,0)); + y1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,1)); + z1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,2)); + */ + double x2 = 0; + double y2 = 0; + double z2 = 0; + double r1 = 0; + double r2 = 0; + double r3 = 0; + for(int i = 0; i < count; i++) { + x2 += (CV_MAT_ELEM(*some_vectors,float,i,0)); + y2 += (CV_MAT_ELEM(*some_vectors,float,i,1)); + z2 += (CV_MAT_ELEM(*some_vectors,float,i,2)); + r1 += (CV_MAT_ELEM(*some_vectors,float,i,3)); + r2 += (CV_MAT_ELEM(*some_vectors,float,i,4)); + r3 += (CV_MAT_ELEM(*some_vectors,float,i,5)); + } + (CV_MAT_ELEM(*mean,float,0,0)) = x2/count; + (CV_MAT_ELEM(*mean,float,0,1)) = y2/count; + (CV_MAT_ELEM(*mean,float,0,2)) = z2/count; + (CV_MAT_ELEM(*mean,float,0,3)) = r1/count; + (CV_MAT_ELEM(*mean,float,0,4)) = r2/count; + (CV_MAT_ELEM(*mean,float,0,5)) = r3/count; + } +} + +/** + * Sorts vectors element by element, enables one to calculate the median of + * each element separately. + */ +void sortElementByElement(CvMat * vectors, int nr_elems, int nr_vectors) { + bool swapped; + for (int i = 0; i < nr_elems; i++) { + do { + swapped = false; + for (int j = 1; j <= nr_vectors - 1; j++) { + if (CV_MAT_ELEM(*vectors,float,j,i) < CV_MAT_ELEM(*vectors,float,j-1,i)) { + float temp = CV_MAT_ELEM(*vectors,float,j,i); + CV_MAT_ELEM(*vectors,float,j,i) = CV_MAT_ELEM(*vectors,float,j-1,i); + CV_MAT_ELEM(*vectors,float,j-1,i) = temp; + swapped = true; + } + } + } while (swapped); + } +} + +/** + * Calculates the extrinsic parameters of a set of matches. Find the best match + * by calculating the reprojection error of each set of calibration parameters. + */ +void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat * + points3D, CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, CvMat + * distortion, CvMat * intrinsics, int corners, int successes, string dir, bool quiet, string substring) { + double reprojectionError[successes]; + for(int i = 0; i < successes; i++) { + reprojectionError[i] = 0.0; + } + + cout << "reprojectionError" << endl; + for(int i = 0; i < successes + 4; i++) { + reprojectionError[i] = 0.0; + CvMat * rotation = cvCreateMat(1, 3, CV_32FC1); + CvMat * translation = cvCreateMat(1, 3, CV_32FC1); + if(i==successes) cout << "now other stuff" << endl; + for(int k = 0; k < 3; k++) { + CV_MAT_ELEM(*rotation, float, 0, k) = CV_MAT_ELEM(*rotation_vectors_temp, float, i, k); + CV_MAT_ELEM(*translation, float, 0, k) = CV_MAT_ELEM(*translation_vectors_temp, float, i, k); + cerr << CV_MAT_ELEM(*translation, float, 0, k)<< " "; + } + for(int k = 0; k < 3; k++) { + cerr << CV_MAT_ELEM(*rotation, float, 0, k)<< " "; + } + cerr << endl; + for(int j = 0; j < successes; j++) { + double tmp = 0; + //calculate reprojection error + CvMat * point_3Dcloud = cvCreateMat(corners, 3, CV_32FC1); + CvMat * point_2Dcloud = cvCreateMat(corners, 2, CV_32FC1); + for(int l = 0; l < corners; l++) { + CV_MAT_ELEM(*point_2Dcloud,float,l,0) = 0.0; + CV_MAT_ELEM(*point_2Dcloud,float,l,1) = 1.0; + CV_MAT_ELEM(*point_3Dcloud,float,l,0) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).x; + CV_MAT_ELEM(*point_3Dcloud,float,l,1) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).y; + CV_MAT_ELEM(*point_3Dcloud,float,l,2) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).z; + } + cvProjectPoints2(point_3Dcloud, rotation, translation, intrinsics, + distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + for(int l = 0; l < corners; l++) { + double x = CV_MAT_ELEM(*point_2Dcloud,float,l,0) - CV_MAT_ELEM(*points2D,CvPoint2D32f,j,l).x; + double y = CV_MAT_ELEM(*point_2Dcloud,float,l,1) - CV_MAT_ELEM(*points2D,CvPoint2D32f,j,l).y; + tmp += sqrt(x*x + y*y); + } + cvReleaseMat(&point_2Dcloud); + reprojectionError[i] += tmp; + cvReleaseMat(&point_3Dcloud); + } + cout << reprojectionError[i]/successes/30 << endl; + cvReleaseMat(&rotation); + cvReleaseMat(&translation); + } + + int maxindex = -1; + double max = DBL_MAX; + for(int i = 0; i < successes + 4; i++) { + if(reprojectionError[i] < max) { + maxindex = i; + max = reprojectionError[i]; + } + } + cerr << "Reprojection error: " << max/successes << endl; + CvMat * rotation = cvCreateMat(1, 3, CV_32FC1); + CvMat * translation = cvCreateMat(1, 3, CV_32FC1); + + for(int i = 0; i < 3; i++) { + CV_MAT_ELEM(*rotation, float, 0, i) = CV_MAT_ELEM(*rotation_vectors_temp, float, maxindex, i); + CV_MAT_ELEM(*translation, float, 0, i) = CV_MAT_ELEM(*translation_vectors_temp, float, maxindex, i); + } + string file = dir + "Rotation" + substring + ".xml"; + cvSave(file.c_str(), rotation); + file = dir + "Translation" + substring + ".xml"; + cvSave(file.c_str(), translation); + +} + +/** + * Calculates the extrinsic parameters given a set of feature matches using the + * mean and median method. + */ +void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, int successes, string dir, bool quiet, string substring) { + + CvMat* rotation_vectors = cvCreateMat(successes, 3, CV_32FC1); + CvMat* translation_vectors = cvCreateMat(successes, 3, CV_32FC1); + CvMat* vectors = cvCreateMat(successes, 6, CV_32FC1); + CvMat* rotation_vector_mean = cvCreateMat(1, 3, CV_32FC1); + CvMat* translation_vector_mean = cvCreateMat(1, 3, CV_32FC1); + CvMat* rotation_vector_median = cvCreateMat(1, 3, CV_32FC1); + CvMat* translation_vector_median = cvCreateMat(1, 3, CV_32FC1); + CvMat* median = cvCreateMat(1, 6, CV_32FC1); + for (int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) = 0; + CV_MAT_ELEM(*translation_vector_mean,float,0,t) = 0; + CV_MAT_ELEM(*rotation_vector_median,float,0,t) = 0; + CV_MAT_ELEM(*translation_vector_median,float,0,t) = 0; + CV_MAT_ELEM(*median,float,0,t) = 0; + CV_MAT_ELEM(*median,float,0,t + 3) = 0; + } + + for (int h = 0; h < successes; h++) { + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vectors,float,h,t) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) += CV_MAT_ELEM(*rotation_vectors,float,h,t); + + CV_MAT_ELEM(*translation_vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t); + CV_MAT_ELEM(*translation_vector_mean,float,0,t) += CV_MAT_ELEM(*translation_vectors,float,h,t); + + CV_MAT_ELEM(*vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t); + CV_MAT_ELEM(*vectors,float,h,t + 3) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + cout << CV_MAT_ELEM(*translation_vectors,float,h,t) << " "; + } + cout << endl; + } + + // mean + cout << "Getting mean vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) /= successes; + CV_MAT_ELEM(*rotation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*rotation_vector_mean,float,0,t); + CV_MAT_ELEM(*translation_vector_mean,float,0,t) /= successes; + CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*translation_vector_mean,float,0,t); + cout << CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) << " "; + } + cout << endl; + + // getting the median vectors + // median + cout << "Getting median vector" << endl; + filterMedian(vectors, successes, 1, median); + cout << "Got median vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*translation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t); + CV_MAT_ELEM(*rotation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t+3); + CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*rotation_vector_median,float,0,t); + CV_MAT_ELEM(*translation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*translation_vector_median,float,0,t); + } + + + // filtered median + cout << "Getting filtered median vector" << endl; + filterMedian(vectors, successes, 4, median); + cout << "Got filtered median vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t+3); + CV_MAT_ELEM(*translation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t); + } + + // elementwise median + // finding the median of rotation and translation + // sorting the rotation vectors element by element + + sortElementByElement(vectors, 6, successes); + /* + sortElementByElement(translation_vectors, 3, successes); + + if(!quiet) { + cout << "number of successes : " << successes << endl; + cout << "rotation vectors are" << endl; + for (int i = 0; i < successes; i++) { + cout << CV_MAT_ELEM(*rotation_vectors,float,i,0) << " " + < -30) + if(CV_MAT_ELEM(*Translation, float, 0, 0) < -20) + if(CV_MAT_ELEM(*Translation, float, 0, 1) > -4 ) + if(CV_MAT_ELEM(*Translation, float, 0, 1) < -00) + if(CV_MAT_ELEM(*Translation, float, 0, 2) > -12) + if(CV_MAT_ELEM(*Translation, float, 0, 2) < -8 ) { + */ + if(!quiet) cout << "Rotation is:" << endl; + for (int row = 0; row < 1; row++) { + for (int col = 0; col < 3; col++) { + if(!quiet) cout << CV_MAT_ELEM( *Rotation, float, row, col ) << " "; + CV_MAT_ELEM( *rotation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Rotation, float, row, col ); + } + if(!quiet) cout << endl; + } + if(!quiet) cout << "Translation is:" << endl; + for (int row = 0; row < 1; row++) { + for (int col = 0; col < 3; col++) { + if(!quiet) cout << CV_MAT_ELEM( *Translation, float, row, col ) << " "; + CV_MAT_ELEM( *translation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Translation, float, row, col ); + } + if(!quiet) cout << endl; + } + + cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; + int c = cvWaitKey(100); + cin >> in; + if (c == 27) { + break; + } + + if (in == 'y') { + for (int j = 0; j < corner_exp; ++j) { + CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).x = (float)point_array2[j][0]; + CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).y = (float)point_array2[j][1]; + CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).x = corners[j].x; + CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).y = corners[j].y; + CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).z = corners[j].z; + } + successes++; + // } + + } else if(in == 'x') { + break; + } + //cvReleaseImage(&image); + cvReleaseMat(&image_points); + cvReleaseMat(&object_points); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + } + cvReleaseImage(&image); + cvReleaseImage(&image1); + cvReleaseImage(&image2); + }//for loop for imagecount + cvDestroyWindow("Original Image"); + cvDestroyWindow("Final Result"); + + cout << "Number of successes: " << successes << endl; + // Now calculating mean and median rotation and trans + calculateExtrinsics(rotation_vectors_temp, translation_vectors_temp, successes, dir, quiet, substring); + calculateExtrinsicsWithReprojectionCheck(points2D, points3D, rotation_vectors_temp, translation_vectors_temp, distortion, intrinsic, corner_exp, successes, dir, quiet, substring); + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&translation_vectors_temp); + cvReleaseMat(&rotation_vectors_temp); + cvReleaseMat(&points2D); + cvReleaseMat(&points3D); +} + +//bool readFrames(char * dir, int index, double * rPos, rPosTheta) { +//bool readFrames(char * dir, int index, double * inMatrix, double * rPos) { +bool readFrames(const char * dir, int index, double * tMatrix, CvMat * inMatrix, CvMat * rPos) { + char frameFileName[255]; + snprintf(frameFileName,255,"%sscan%.3d.frames",dir,index); + ifstream pose_in; + pose_in.open(frameFileName); + + if (!pose_in.good()) return false; // no more files in the directory + + cout << "Reading frame " << frameFileName << "..." << endl; + double tmpMatrix[17]; + while(pose_in.good()) { + for (unsigned int i = 0; i < 17; pose_in >> tMatrix[i++]); + } + + M4inv(tMatrix, tmpMatrix); + + CV_MAT_ELEM(*inMatrix, float,0,0) = tmpMatrix[10]; + CV_MAT_ELEM(*inMatrix, float,0,1) = -tmpMatrix[2]; + CV_MAT_ELEM(*inMatrix, float,0,2) = tmpMatrix[6]; + CV_MAT_ELEM(*inMatrix, float,1,0) = -tmpMatrix[8]; + CV_MAT_ELEM(*inMatrix, float,1,1) = tmpMatrix[0]; + CV_MAT_ELEM(*inMatrix, float,1,2) = -tmpMatrix[4]; + CV_MAT_ELEM(*inMatrix, float,2,0) = tmpMatrix[9]; + CV_MAT_ELEM(*inMatrix, float,2,1) = -tmpMatrix[1]; + CV_MAT_ELEM(*inMatrix, float,2,2) = tmpMatrix[5]; + + CV_MAT_ELEM(*rPos, float,0,0) = tmpMatrix[14]; + CV_MAT_ELEM(*rPos, float,1,0) = -tmpMatrix[12]; + CV_MAT_ELEM(*rPos, float,2,0) = tmpMatrix[13]; + + return true; +} + +void writeGlobalCameras(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) { + + string file; + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + string substring = optical? "Optical" : ""; + switch(method) { + case 0: + file = dir + "Rotation" + substring + ".xml"; + break; + case 1: + file = dir + "RotationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "RotationMean" + substring + ".xml"; + break; + } + CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Translation" + substring + ".xml"; + break; + case 1: + file = dir + "TranslationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "TranslationMean" + substring + ".xml"; + break; + } + CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + + for (int count = start; count <= end; count++) { + // filling the rotation matrix + + // reading the frame files 3D points and projecting them back to 2d + CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1); + CvMat* rPos = cvCreateMat(3,1,CV_32FC1); + double * tMatrix = new double[17]; + readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos); + + // TODO make sure correct points are printed + /* + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + */ + + // write colored data + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + double angle = rot_angle * (p%nrP360); + int count0 = count * nrP360 + p; + string outname = outdir + "/image" + to_string(count0, 3) + ".mat"; + + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + /* + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + */ + } + cout << endl; + + // transform into global coordinate system (inMatrix, rPos) + CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* rodglob = cvCreateMat(3,1,CV_32FC1); + cvRodrigues2(inMatrix, rodglob); + cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI); + //cvRodrigues2(r_globalI, rot_tmp); + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + CvMat* rotmatrix = cvCreateMat(3,3,CV_32FC1); + cvRodrigues2(r_globalI, rotmatrix); + + //cvSave(outname.c_str(), rotmatrix); + + for(int i = 0; i < 3; i++) { + for(int j = 0; j < 3; j++) { + outfile << CV_MAT_ELEM(*rotmatrix,float,i,j) << " "; + } + + outfile << CV_MAT_ELEM(*t_globalI,float,i,0) << endl; + } + outfile << "0 0 0 1" << endl; + // checking whether projection lies within the image boundaries + cvReleaseMat(&rot_tmp); + + outfile.close(); + outfile.flush(); + + } + + } + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + +} +/** + * Read scans + * Read frames + */ +void calculateGlobalCameras(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) { + + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + string substring = optical? "Optical" : ""; + string file = dir + "Intrinsics" + substring + ".xml"; + CvMat *intrinsic = (CvMat*) cvLoad(file.c_str()); + file = dir + "Distortion" + substring + ".xml"; + CvMat *distortion = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Rotation" + substring + ".xml"; + break; + case 1: + file = dir + "RotationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "RotationMean" + substring + ".xml"; + break; + } + CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Translation" + substring + ".xml"; + break; + case 1: + file = dir + "TranslationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "TranslationMean" + substring + ".xml"; + break; + } + CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; + } + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + int pointcnt = 0; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + for (int count = start; count <= end; count++) { + // filling the rotation matrix + CvMat* point_3Dcloud; + CvMat* point_2Dcloud; + CvMat* undistort_2Dcloud; + + // reading the 3D points and projecting them back to 2d + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); + + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + cout << "Points: " << red_size << endl; + point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + cout << "readScans done" << endl; + // read frames + //double * rPos = new double[3]; + //double * rPosTheta= new double[3]; + //readFrames(dir, count, rPos, rPosTheta); + + CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1); + CvMat* rPos = cvCreateMat(3,1,CV_32FC1); + double * tMatrix = new double[17]; + readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos); + Scan::allScans[0]->transform(tMatrix, Scan::INVALID, 0); + + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + // TODO make sure correct points are printed + + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + + } + int nr_points = red_size; + cout << "Number of points read: " << red_size << endl; + delete Scan::allScans[0]; + Scan::allScans.clear(); + + // write colored data + string outname = outdir + "/scan" + to_string(count, 3) + ".3d"; + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + //for(int p = 4; p < 5; p++) { + //double angle = rot_angle * (p%nrP360) + 2.0; + double angle = rot_angle * (p%nrP360); + cout << angle << endl; + // loading images + int count0 = count * nrP360 + p; + + string t, t0; + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + //t = dir + "/photo" + to_string(count0, 3) + "_2.jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_90.jpg"; + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_1.jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + IplImage* image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + return; + } + CvSize size = cvGetSize(image); + + image = resizeImage(image, scale); + + + /** TODO!!! + * Transform rPos and rPosTheta into OpenCV (Rodrigues) + * cvComposeRT with rod_comI and t_comI + */ + // rotate Rotation and Translation + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + cout << "tmp2" << endl; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + cout << "tmp3" << endl; + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + cout << endl; + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + } + cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + + // transform into global coordinate system (inMatrix, rPos) + CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* rodglob = cvCreateMat(3,1,CV_32FC1); + cvRodrigues2(inMatrix, rodglob); + cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI); + //TODO verify order + cvRodrigues2(r_globalI, rot_tmp); + + //cvComposeRT(rod_comI, t_comI, rodglob, rPos, r_globalI, t_globalI); + + + cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + + + // END TODO + // Project Points + /* + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + */ + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + //for counting how many points get mapped to first and second image file + int point_map1 = 0; // #points mapped to first image + int point_map2 = 0; // " " " second image + int point_map3 = 0; // " " " second image + + // checking whether projection lies within the image boundaries + cout << "Now project points" << endl; + for (int k = 0; k < nr_points; k++) { + float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); + float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) { + point_map1++; + px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); + py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { + point_map2++; + CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3); + CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2); + + cvTransform(tmp1, tmp2, rot_tmp); + cvReleaseMat(&tmp1); + + //double tmpz = CV_MAT_ELEM(*t_globalI, CV_32FC1, 2,0); + double tmpz = -CV_MAT_ELEM(*t_globalI, float, 2,0); + //double tmpz = CV_MAT_ELEM(*TranslationI, float, 2,0); + + if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < tmpz) { + cvReleaseMat(&tmp2); + continue; + } + + //cout << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).x << " " + // << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).y << " " + // << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).z << endl; + + cvReleaseMat(&tmp2); + /* + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl; + */ + point_map3++; + int ppx = 0; + int ppy = 0; + if (px - int(px) < .5) { + ppx = int(px); + } else { + ppx = int(px) + 1; + } + if (py - int(py) < .5) { + ppy = int(py); + } else { + ppy = int(py) + 1; + } + + CvScalar c; + c = cvGet2D(image, ppy, ppx); + // check for overlap + /* + if(correction) { + vector temp_vec; + float p_id = 1; // 1 for pixel, 0 for neighboring pixel + temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); + temp_vec.push_back(c.val[2]); + temp_vec.push_back(c.val[1]); + temp_vec.push_back(c.val[0]); + temp_vec.push_back(p_id); + if(neighborhood > 1) { + int limit = neighborhood / 2; + + int lower_y = ppy - limit > 0 ? ppy - limit : 0; + int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; + int lower_x = ppx - limit > 0 ? ppx - limit : 0; + int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; + + for (int y = lower_y; y < upper_y; y++) { + for (int x = lower_x; x < upper_x; x++) { + if(x == ppx && y == ppy) { + temp_vec[6] = 1; + } else { + temp_vec[6] = 0; + } + data1[y][x].push_back(temp_vec); + } + } + + } else { + data1[ppy][ppx].push_back(temp_vec); + } + temp_vec.clear(); + } else { + */ + // write all the data + + outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outdat << c.val[2] <<" "<< c.val[1]<<" "< 100) { + outfile.write(outdat.str().c_str(), outdat.str().size()); + pointcnt = 0; + outdat.clear(); + outdat.str(""); + } + + /* + outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { + if(size.width > 0 && size.height > 0) { + CorrectErrorAndWrite(data2, outfile, size, optical); + } + } + } + + cout << "Done correction" << endl; + // clean up + outfile.flush(); + + double endtime = GetCurrentTimeInMilliSec(); + double time = endtime - starttime; + time = time/1000.0; + cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl; + + cout << point_map1 << " " << point_map2 << " " << point_map3 << endl; + cvReleaseImage(&image); + if(correction) { + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + data1[i][j].clear(); + if(fabs(rot_angle) > 1) { + data2[i][j].clear(); + } + } + } + } + cvReleaseMat(&rot_tmp); + } + outfile.close(); + + cvReleaseMat(&point_2Dcloud); + cvReleaseMat(&point_3Dcloud); + cvReleaseMat(&undistort_2Dcloud); + + } + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&undistort); + +} + +/** + * Main function for projecting the 3D points onto the corresponding image and + * associating temperature values to the data points. + */ +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) { + + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + string substring = optical? "Optical" : ""; + string file = dir + "Intrinsics" + substring + ".xml"; + CvMat *intrinsic = (CvMat*) cvLoad(file.c_str()); + file = dir + "Distortion" + substring + ".xml"; + CvMat *distortion = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Rotation" + substring + ".xml"; + break; + case 1: + file = dir + "RotationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "RotationMean" + substring + ".xml"; + break; + } + CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Translation" + substring + ".xml"; + break; + case 1: + file = dir + "TranslationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "TranslationMean" + substring + ".xml"; + break; + } + CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; + } + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + int pointcnt = 0; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + for (int count = start; count <= end; count++) { + // filling the rotation matrix + CvMat* point_3Dcloud; + CvMat* point_2Dcloud; + CvMat* undistort_2Dcloud; + + // reading the 3D points and projecting them back to 2d + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); + + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + cout << "Points: " << red_size << endl; + point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + cout << "readScans done" << endl; + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + // TODO make sure correct points are printed + + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + + } + int nr_points = red_size; + cout << "Number of points read: " << red_size << endl; + delete Scan::allScans[0]; + Scan::allScans.clear(); + + // write colored data + string outname = outdir + "/scan" + to_string(count, 3) + ".3d"; + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + //for(int p = 0; p < 9; p++) { + //double angle = rot_angle * (p%nrP360) + 2.0; + double angle = rot_angle * (p%nrP360); + cout << angle << endl; + // loading images + int count0 = count * nrP360 + p; + + string t, t0; + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + //t = dir + "/photo" + to_string(count0, 3) + "_2.jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_90.jpg"; + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_1.jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + IplImage* image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + return; + } + CvSize size = cvGetSize(image); + + image = resizeImage(image, scale); + + // rotate Rotation and Translation + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + cout << "tmp2" << endl; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + cout << "tmp3" << endl; + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + cout << endl; + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + } + cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + + // Project Points + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + //for counting how many points get mapped to first and second image file + int point_map1 = 0; // #points mapped to first image + int point_map2 = 0; // " " " second image + int point_map3 = 0; // " " " second image + + // checking whether projection lies within the image boundaries + cout << "Now project points" << endl; + for (int k = 0; k < nr_points; k++) { + float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); + float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) { + point_map1++; + px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); + py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { + point_map2++; + CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3); + CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2); + + cvTransform(tmp1, tmp2, rot_tmp); + cvReleaseMat(&tmp1); + + + if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < 0) { + cvReleaseMat(&tmp2); + continue; + } + cvReleaseMat(&tmp2); + /* + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl; + */ + point_map3++; + int ppx = 0; + int ppy = 0; + if (px - int(px) < .5) { + ppx = int(px); + } else { + ppx = int(px) + 1; + } + if (py - int(py) < .5) { + ppy = int(py); + } else { + ppy = int(py) + 1; + } + + CvScalar c; + c = cvGet2D(image, ppy, ppx); + // check for overlap + /* + if(correction) { + vector temp_vec; + float p_id = 1; // 1 for pixel, 0 for neighboring pixel + temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); + temp_vec.push_back(c.val[2]); + temp_vec.push_back(c.val[1]); + temp_vec.push_back(c.val[0]); + temp_vec.push_back(p_id); + if(neighborhood > 1) { + int limit = neighborhood / 2; + + int lower_y = ppy - limit > 0 ? ppy - limit : 0; + int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; + int lower_x = ppx - limit > 0 ? ppx - limit : 0; + int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; + + for (int y = lower_y; y < upper_y; y++) { + for (int x = lower_x; x < upper_x; x++) { + if(x == ppx && y == ppy) { + temp_vec[6] = 1; + } else { + temp_vec[6] = 0; + } + data1[y][x].push_back(temp_vec); + } + } + + } else { + data1[ppy][ppx].push_back(temp_vec); + } + temp_vec.clear(); + } else { + */ + // write all the data + + outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outdat << c.val[2] <<" "<< c.val[1]<<" "< 100) { + outfile.write(outdat.str().c_str(), outdat.str().size()); + pointcnt = 0; + outdat.clear(); + outdat.str(""); + } + + /* + outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { + if(size.width > 0 && size.height > 0) { + CorrectErrorAndWrite(data2, outfile, size, optical); + } + } + } + + cout << "Done correction" << endl; + // clean up + outfile.flush(); + + double endtime = GetCurrentTimeInMilliSec(); + double time = endtime - starttime; + time = time/1000.0; + cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl; + + cout << point_map1 << " " << point_map2 << " " << point_map3 << endl; + cvReleaseImage(&image); + if(correction) { + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + data1[i][j].clear(); + if(fabs(rot_angle) > 1) { + data2[i][j].clear(); + } + } + } + } + cvReleaseMat(&rot_tmp); + } + outfile.close(); + + cvReleaseMat(&point_2Dcloud); + cvReleaseMat(&point_3Dcloud); + cvReleaseMat(&undistort_2Dcloud); + + } + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&undistort); + +} + +/** + * Sorts a number of float array according to their distance to the origin. + */ +void sortDistances(float ** points, int size) { + int swapped1 = 0; + do { + swapped1 = 0; + for(int a = 1; a <= size - 1; a++) { + if(Len(points[a]) < Len(points[a - 1])) { + float * tmp = points[a-1]; + points[a-1] = points[a]; + points[a] = tmp; + swapped1 = 1; + } + } + } while (swapped1 == 1); +} + +/** + * Performs clustering on all points that are projected onto one pixel. + * Writes only the points from the largest closest cluster. + */ +void clusterSearch(float ** points, int size, double thresh1, double thres2, fstream &outfile, bool optical) { + int position = 0; + int cluster_count = 0; + + double max_cluster = 0; + int max_position = 0; + vector clusters; + while (position < size) { + double sum = 0.0; + int j = position + 1; + while(j < size && (Len(points[j]) < (Len(points[j-1]) + thresh1))) { + j++; + cluster_count++; + sum+=Len(points[j-1]); + } + double * tmp = new double[4]; + tmp[0] = position; + tmp[1] = j - 1; + // tmp[2] = sum / (j - position); + // weird heuristic ;-) (clustersize/(rank of the cluster)) + tmp[2] = (double)(j - position) / (clusters.size() + 1.0); + tmp[3] = (double)(j - position); + if(tmp[3] > max_cluster) { + max_position = clusters.size(); + max_cluster = tmp[3]; + } + clusters.push_back(tmp); + position = j; + } + + /* + max_position = 0; + for(int p = clusters.size() - 1; p > -1; p--) { + max_position = p; + break; + } + */ + + for(int p = clusters[max_position][0]; p <= clusters[max_position][1]; p++) { + if(points[p][6] == 1) { + outfile << points[p][0] << " " << points[p][1] << " " << points[p][2] << " "; + if(optical) { + outfile << points[p][3] << " " << points[p][4] << " " << points[p][5] << endl; + } else { + outfile << (points[p][5] - 1000.0)/10.0 << endl; + } + } + } + + for(unsigned int i = 0; i < clusters.size(); i++) { + delete[] clusters[i]; + } +} + +void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical) { + double thresh1 = 4; + double thresh2 = 5; + + cout << size.height << " " << size.width << endl; + // getting points mapping to one pixel + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + int tmp_size = data[i][j].size(); + if (tmp_size > 0) { + float ** points = new float*[tmp_size]; + for (int k = 0; k < tmp_size; k++) { + points[k] = new float[7]; + for(int l = 0; l < 7; l++) { + points[k][l] = data[i][j][k][l]; + } + } + + //sorting the points now in ascending order wrt distance + sortDistances(points, tmp_size); + //look for clusters + clusterSearch(points, tmp_size, thresh1, thresh2, outfile, optical); + for (int k = 0; k < tmp_size; k++) { + delete[] points[k]; + } + delete[] points; + } + + } + } + +} + +/** + * Prints out usage message + */ +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 << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl + << " start at scan NR (i.e., neglects the first NR scans)" << endl + << " [ATTENTION: counting naturally starts with 0]" << endl + << endl + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end at scan NR" << endl + << endl + << bold << " -x" << normal << " NR, " << bold << "--width=" << normal << "NR" << endl + << " NR of lamps/corners in x direction" << endl + << endl + << bold << " -y" << normal << " NR, " << bold << "--height=" << normal << "NR" << endl + << " NR of lamps/corners in y direction" << endl + << endl + << bold << " -o --=optical" << normal << endl + << " use optical camera instead of thermal camera" << endl + << endl + << bold << " -c --=chess" << normal << endl + << " use chessboard pattern for calibration instead of lightbulb pattern" << endl + << endl + << bold << " -I --=intrinsic" << normal << endl + << " perform intrinsic calibration" << endl + << endl + << bold << " -E --=extrinsic" << normal << endl + << " perform extrinsic calibration" << endl + << endl + << bold << " -P --=mapping" << normal << endl + << " perform mapping of image data to point cloud" << endl + << endl + << bold << " -q --=quiet" << normal << endl + << " " << endl + << endl << endl; + + cout << bold << "EXAMPLES " << normal << endl + << " " << prog << " -s 2 -e 10 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, int &start, int &end, double +&maxDist, double &minDist, IOType &type, bool &optical, bool &chess, int +&width, int &height, bool &intrinsic, bool &extrinsic, bool &mapping, bool +&correction, int &scale, int &neighborhood, double &angle, bool &quiet ) { + // from unistd.h: + int c; + extern char *optarg; + extern int optind; + + /* options descriptor */ + // 0: no arguments, 1: required argument, 2: optional argument + static struct option longopts[] = { + { "correction", no_argument, 0, 'C' }, + { "scale", required_argument, 0, 'S' }, + { "neighborhood", required_argument, 0, 'n' }, + { "angle", required_argument, 0, 'a' }, + { "format", required_argument, 0, 'f' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "width", required_argument, 0, 'x' }, + { "height", required_argument, 0, 'y' }, + { "quiet", no_argument, 0, 'q' }, + { "optical", no_argument, 0, 'o' }, + { "intrinsic", no_argument, 0, 'I' }, + { "extrinsic", no_argument, 0, 'E' }, + { "mapping", no_argument, 0, 'P' }, + { "chess", no_argument, 0, 'c' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + cout << endl; + while ((c = getopt_long(argc, argv, "f:s:e:x:y:m:M:qoIEPcCS:n:a:", longopts, NULL)) != -1) { + switch (c) + { + 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: cannot be smaller than .\n"; exit(1); } + break; + case 'f': + try { + type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'q': + quiet = true; + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'o': + optical = true; + break; + case 'I': + intrinsic = true; + break; + case 'E': + extrinsic = true; + break; + case 'P': + mapping = true; + break; + case 'c': + chess = true; + break; + case 'x': + width = atoi(optarg); + break; + case 'y': + height = atoi(optarg); + break; + case 'S': + scale = atoi(optarg); + break; + case 'a': + angle = atof(optarg); + break; + case 'n': + neighborhood = atoi(optarg); + break; + case 'C': + correction = true; + break; + case '?': + usage(argv[0]); + return 1; + default: + cout << "Abort" << endl; + 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 + + if(!(intrinsic || extrinsic || mapping)) { + cerr << "\n*** Please choose at least one method (intrinsic calibration, " + << "extrinsic calibration, mapping of image data to point data! ***\n" << + endl; + usage(argv[0]); + } + return 0; + +} + +/** + * Main function. Calls either function for color mapping or function for + * intrinsic and/or extrinsic calibration. + */ +int main(int argc, char** argv) { + string dir; + int start = 0; + int end = -1; + int width = 5; + int height = 6; + double maxDist = -1; + double minDist = -1; + IOType type = UOS; + bool optical = false; + bool chess = false; + bool intrinsic = false; + bool extrinsic = false; + bool mapping = false; + bool quiet = false; + int scale = 1; + //double rot_angle = -40; + double rot_angle = 0; + bool correction = false; + int neighborhood = 1; + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, type, optical, chess, + width, height, intrinsic, extrinsic, mapping, correction, scale, neighborhood, + rot_angle, quiet); + + // either mapping + if(mapping) { + if(!quiet) cout << "Starting projecting and mapping image data to point cloud..." << endl; + //TODO ProjectAndMap(start, end, optical, quiet, dir, type, scale, rot_angle, minDist, maxDist, correction, neighborhood); + + //calculateGlobalCameras(start, end, optical, quiet, dir, type, scale, + writeGlobalCameras(start, end, optical, quiet, dir, type, scale, + rot_angle, minDist, maxDist, correction, neighborhood, 0); + if(!quiet) cout << "\nDONE" << endl; + return 0; + } + + // or calibration + if(intrinsic) { + if(!quiet) { + cout << "Starting intrinsic calibration using "; + if(chess) cout << "chessboard pattern..." << endl; + else cout << "lightbulb pattern..." << endl; + } + CalibFunc(width, height, start, end, optical, chess, quiet, dir, scale); + if(!quiet) cout << "\nDONE" << endl; + } + + if(extrinsic) { + if(!quiet) { + cout << "Starting extrinsic calibration using "; + if(chess) cout << "chessboard pattern..." << endl; + else cout << "lightbulb pattern..." << endl; + } + ExtrCalibFunc(width, height, start, end, optical, chess, quiet, dir, scale); + if(!quiet) cout << "\nDONE" << endl; + } + + return 0; +} + diff --git a/.svn/pristine/2c/2ce0f84a30216373bcc91d1d3a176c840d15e404.svn-base b/.svn/pristine/2c/2ce0f84a30216373bcc91d1d3a176c840d15e404.svn-base new file mode 100644 index 0000000..dea4d44 --- /dev/null +++ b/.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 +using std::ifstream; +using std::ofstream; + +#include +using std::cout; +using std::cerr; +using std::endl; + +#include +using std::stringstream; + +#include "veloslam/veloscan.h" + +#ifdef _OPENMP +#include +#endif + +#ifdef _MSC_VER +#include +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +#include +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 +#endif + +#ifdef _MSC_VER +#include +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +#include +using std::flush; + +#include +#include + +#ifdef _MSC_VER +#include +#else +#include +#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 tanv; + vector::iterator result; + + for(i=0; i=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; ix; + 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_xx) f.max_x=cellobj[i]->x; + + if(f.min_x>cellobj[i]->x) f.min_x=cellobj[i]->x; + + if(f.max_zz) f.max_z=cellobj[i]->z; + + if(f.min_z>cellobj[i]->z) f.min_z=cellobj[i]->z; + + if(f.max_yy) + { + 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; iy - 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 120) +// { +// scanCellFeatureArray[j][i].cellType |= CELL_TYPE_STATIC; +// for(int k=0;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; iave_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_xmax_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_zmax_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_ymax_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 *oct = new BOctTree(PointerArray(xyz_t).get(), + xyz_t.size(), reduction_voxelSize, reduction_pointtype); + + vector 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:"<0.5) + cout<<"intersection"< 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_x4)) + { + 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; isize(); + 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; + } diff --git a/.svn/pristine/2d/2d3538d03392fb9bafc74ab0394be867d69fa0dd.svn-base b/.svn/pristine/2d/2d3538d03392fb9bafc74ab0394be867d69fa0dd.svn-base new file mode 100644 index 0000000..3479ed8 --- /dev/null +++ b/.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 + +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--; +} diff --git a/.svn/pristine/2d/2db59ca156a4091e122b11689b22d2a62022b330.svn-base b/.svn/pristine/2d/2db59ca156a4091e122b11689b22d2a62022b330.svn-base new file mode 100644 index 0000000..022fa73 --- /dev/null +++ b/.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 . + + + +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 diff --git a/.svn/pristine/2f/2fe18e6c7cf19c30fdfb8ff8d953bf7138a65c31.svn-base b/.svn/pristine/2f/2fe18e6c7cf19c30fdfb8ff8d953bf7138a65c31.svn-base new file mode 100644 index 0000000..389bdeb --- /dev/null +++ b/.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 +#endif + +#include +#include + +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 + +{ +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 diff --git a/.svn/pristine/30/30a170f1cfcc358de8779570e93c8768d2668763.svn-base b/.svn/pristine/30/30a170f1cfcc358de8779570e93c8768d2668763.svn-base new file mode 100644 index 0000000..13e043f --- /dev/null +++ b/.svn/pristine/30/30a170f1cfcc358de8779570e93c8768d2668763.svn-base @@ -0,0 +1,2048 @@ +/* + * show_gl implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#include +#include +#include "show/viewcull.h" +#include "show/scancolormanager.h" + +using namespace show; +bool fullydisplayed = true; // true if all points have been drawn to the screen +bool mousemoving = false; // true iff a mouse button has been pressed inside a window, + // but hs not been released +bool keypressed = false; // true iff a key button has been pressed inside a window, + // but hs not been released +double ptstodisplay = 100000; +double lastfps = idealfps; // last frame rate +int pointmode = -1; + +bool smallfont = true; +bool label = true; + +/** + * Displays all data (i.e., points) that are to be displayed + * @param mode spezification for drawing to screen or in selection mode + */ +void DrawPoints(GLenum mode, bool interruptable) +{ + long time = GetCurrentTimeInMilliSec(); + double min = 0.000000001; + double max = 1.0; + LevelOfDetail *= 1.0 + adaption_rate*(lastfps - idealfps)/idealfps; + if (LevelOfDetail > max) LevelOfDetail = max; + else if (LevelOfDetail < min) LevelOfDetail = min; + + // In case of animation + if(frameNr != 0) { + cm->setMode(ScanColorManager::MODE_ANIMATION); + +#ifdef USE_GL_POINTS + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { +#else + for(int iterator = (int)Scan::allScans.size()-1; iterator >= 0; iterator--) { +#endif + // ignore scans that don't have any frames associated with them + if((unsigned int)iterator >= MetaMatrix.size()) continue; + // set usable frame + double* frame; + Scan::AlgoType type; + if((unsigned int)frameNr >= MetaMatrix[iterator].size()) { + // use last possible frame + frame = MetaMatrix[iterator].back(); + type = MetaAlgoType[iterator].back(); + } else { + frame = MetaMatrix[iterator][frameNr]; + type = MetaAlgoType[iterator][frameNr]; + } + if(type == Scan::INVALID) continue; + cm->selectColors(type); + glPushMatrix(); + glMultMatrixd(frame); + + + glPointSize(pointsize); +#ifdef USE_GL_POINTS + ExtractFrustum(pointsize); + cm->selectColors(type); + if (pointmode == 1 ) { + octpts[iterator]->display(); + } else { + octpts[iterator]->displayLOD(LevelOfDetail); + } +#else + for (unsigned int jterator = 0; jterator < vvertexArrayList[iterator].size(); jterator++) { + + if ((jterator == 0) && vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { + cm->selectColors(type); + } + + if (vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { + glCallList(vvertexArrayList[iterator][jterator]->name); + } + } +#endif + glPopMatrix(); + } + + setScansColored(0); + } else { + + if(mode == GL_SELECT){ + // select points mode + // ------------------ + GLuint name = 0; + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + glPushMatrix(); + glMultMatrixd(MetaMatrix[iterator].back()); + + glColor4f(1.0, 0.0, 0.0,1.0); + glPointSize(pointsize + 2.0); + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + glLoadName(name++); + glBegin(GL_POINTS); + glVertex3d((*it)[0], (*it)[1], (*it)[2]); + glEnd(); + } + glPointSize(pointsize); + + glFlush(); + glPopMatrix(); + } + + } else { + + // draw point is normal mode + // ------------------------- + + if (interruptable) { + glDrawBuffer (GL_FRONT); + } + glPointSize(pointsize); + + vector sequence; + calcPointSequence(sequence, current_frame); +#ifdef USE_GL_POINTS + //for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + for(unsigned int i = 0; i < sequence.size(); i++) { + int iterator = sequence[i]; +#else + for(int iterator = (int)Scan::allScans.size()-1; iterator >= 0; iterator--) { +#endif + // ignore scans that don't have any frames associated with them + if((unsigned int)iterator >= MetaMatrix.size()) continue; + // set usable frame + double* frame; + Scan::AlgoType type; + if((unsigned int)current_frame >= MetaMatrix[iterator].size()) { + // use last possible frame + frame = MetaMatrix[iterator].back(); + type = MetaAlgoType[iterator].back(); + } else { + frame = MetaMatrix[iterator][current_frame]; + type = MetaAlgoType[iterator][current_frame]; + } + if (type == Scan::INVALID) continue; + glPushMatrix(); + if (invert) // default: white points on black background + glColor4d(1.0, 1.0, 1.0, 0.0); + else // black points on white background + glColor4d(0.0, 0.0, 0.0, 0.0); + + // glMultMatrixd(MetaMatrix[iterator].back()); + if (current_frame != (int)MetaMatrix.back().size() - 1) { + cm->setMode(ScanColorManager::MODE_ANIMATION); + cm->selectColors(type); + } + glMultMatrixd(frame); + +#ifdef USE_GL_POINTS + //cout << endl << endl; calcRay(570, 266, 1.0, 40000.0); + /* // for height mapped color in the vertex shader + GLfloat v[16]; + for (unsigned int l = 0; l < 16; l++) + v[l] = MetaMatrix[iterator].back()[l]; + glUniformMatrix4fvARB(glGetUniformLocationARB(p, "MYMAT"), 1, 0, v); + */ + ExtractFrustum(pointsize); + if (pointmode == 1 ) { + octpts[iterator]->display(); + } else if (interruptable) { + checkForInterrupt(); + glFlush(); + glFinish(); + if (isInterrupted()) { + glPopMatrix(); + return; + } + octpts[iterator]->display(); + } else { + octpts[iterator]->displayLOD(LevelOfDetail); + } + if (!selected_points[iterator].empty()) { + glColor4f(1.0, 0.0, 0.0, 1.0); + glPointSize(pointsize + 2.0); + glBegin(GL_POINTS); + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + glVertex3d((*it)[0], (*it)[1], (*it)[2]); + } + glEnd(); + glPointSize(pointsize); + } + +#else + for (unsigned int jterator = 0; jterator < vvertexArrayList[iterator].size(); jterator++) { + if (vvertexArrayList[iterator][jterator]->numPointsToRender > 0) { + glCallList(vvertexArrayList[iterator][jterator]->name); + } + } +#endif + glPopMatrix(); + } + } + } + + + if (pointmode == 1 ) { + fullydisplayed = true; + } else { + unsigned long td = (GetCurrentTimeInMilliSec() - time); + if (td > 0) + lastfps = 1000.0/td; + else + lastfps = 1000.0; + fullydisplayed = false; + } + if (interruptable) + fullydisplayed = true; +} + + +void DrawObjects(GLenum mode) { + for (unsigned int i = 0; i < displays.size(); i++) + displays[i]->displayAll(); + +} + +/** + * Draw a smooth path passing from all the camera points. + * + */ +void DrawPath() +{ + + glLineWidth(10.0); + // draw path + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < path_vectorX.size(); j++){ + // set the color + glColor4f(0.0, 1.0, 0.0, 1.0); + // set the points + glVertex3f(path_vectorX.at(j).x,path_vectorX.at(j).y,path_vectorZ.at(j).y); + } + glEnd(); + + // draw lookat path + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < lookat_vectorX.size(); j++){ + //set the color + glColor4d(1.0, 1.0, 0.0, 1.0); + //set the points + glVertex3f(lookat_vectorX.at(j).x,lookat_vectorX.at(j).y,lookat_vectorZ.at(j).y); + } + glEnd(); + + // draw up path + /* + glBegin(GL_LINE_STRIP); + for(unsigned int j = 0; j < ups_vectorX.size(); j++){ + //set the color + glColor4d(0.0, 1.0, 0.0, 0.7); + //set the points + glVertex3f(ups_vectorX.at(j).x,ups_vectorX.at(j).y,ups_vectorZ.at(j).y); + } + glEnd(); + */ +} + +/** + * Draw the camera boxes in the viewer + * + */ +void DrawCameras(void) +{ + for (unsigned int i = 0; i < cams.size(); i++) { + glPushMatrix(); + + // TODO improve upon this primitive camera + Point p = cams[i]; + Point l = Point::norm( lookats[i] - p ); // forward vector + Point u = Point::norm( ups[i] - p ); // up vector + Point r = Point::cross(l,u); // right vector + l = 5 * l; + r = 5 * r; + u = 5 * u; + + Point cube[8]; + cube[0] = p + l - r - u; + cube[1] = p + l + r - u; + cube[2] = p - l + r - u; + cube[3] = p - l - r - u; + cube[4] = p + l - r + u; + cube[5] = p + l + r + u; + cube[6] = p - l + r + u; + cube[7] = p - l - r + u; + + int sides[6][4] = {{0,1,2,3}, {4,5,6,7}, {0,1,5,4}, + {3,2,6,7}, {1,2,6,5}, {0,3,7,4}}; + + if (i+1 == cam_choice) { + glColor4f(1, 0, 1, 1); + } else { + glColor4f(0, 1, 0, 1); + } + // camera cube + glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glBegin(GL_QUADS); + for (int j = 0; j < 6; j++) { + if (j == 2) continue; + for (int k = 0; k < 4; k++) { + int index = sides[j][k]; + glVertex3d(cube[index].x, cube[index].y, cube[index].z); + } + } + glEnd(); + + r = 5 * r; + u = 5 * u; + + glColor4f(1, 1, 0, 1); + if (i+1 == cam_choice) { + glPointSize(10); + } else { + glPointSize(5); + } + glBegin(GL_POINTS); + glVertex3d( lookats[i].x, lookats[i].y, lookats[i].z); + glEnd(); + + Point fcube[8]; + fcube[0] = cube[4]; + fcube[1] = cube[5]; + fcube[2] = cube[1]; + fcube[3] = cube[0]; + fcube[4] = lookats[i] - r + u; + fcube[5] = lookats[i] + r + u; + fcube[6] = lookats[i] + r - u; + fcube[7] = lookats[i] - r - u; + + glLineWidth(2.0); + glLineStipple(1, 0x0C0F); + glEnable(GL_LINE_STIPPLE); + glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); + // camera FOV + glBegin(GL_QUADS); + for (int j = 0; j < 6; j++) { + for (int k = 0; k < 4; k++) { + int index = sides[j][k]; + glVertex3d(fcube[index].x, fcube[index].y, fcube[index].z); + } + } + glEnd(); + glDisable(GL_LINE_STIPPLE); + + + /* + if (i+1 == cam_choice) { + glColor3f(1, 1, 0); + glPointSize(20); + } else { + glColor3f(0, 0, 1); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d(p.x, p.y, p.z); + glEnd(); + + if (i+1 == cam_choice) { + glColor3f(0, 1, 1); + glPointSize(20); + } else { + glColor3f(1, 0, 0); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d( l.x, l.y, l.z); + glEnd(); + + if (i+1 == cam_choice) { + glColor3f(1, 0, 1); + glPointSize(20); + } else { + glColor3f(0, 1, 0); + glPointSize(10); + } + glBegin(GL_POINTS); + glVertex3d( u.x, u.y, u.z); + glEnd(); +*/ + + glPopMatrix(); + } +} + +//----------------------------------------------------------------------------------- + + +/** + * Display function + */ +void DisplayItFunc(GLenum mode, bool interruptable) +{ + /** + * Color of the fog + */ + + GLfloat fogColor[4]; + + if(!invert) { + glEnable(GL_COLOR_LOGIC_OP); + glLogicOp(GL_COPY_INVERTED); + } + + // set the clear color buffer in case of + // both invert and non invert mode + if (invert) + glClearColor(0.0, 0.0, 0.0, 0.0); + else + glClearColor(1.0, 1.0, 1.0, 1.0); + + // clear the color and depth buffer bit + if (!interruptable) { // single buffer mode, we need the depth buffer + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + } + + glPushMatrix(); + + // set the matrix mode + glMatrixMode(GL_MODELVIEW); + + // init modelview matrix + glLoadIdentity(); + + // set the polygon mode + glPolygonMode(GL_FRONT/*_AND_BACK*/, GL_LINE); + + // do the model-transformation + if (haveToUpdate == 6 && path_iterator < path_vectorX.size() ) { + gluLookAt(path_vectorX.at(path_iterator).x, + path_vectorX.at(path_iterator).y, + path_vectorZ.at(path_iterator).y, + lookat_vectorX.at(path_iterator).x, + lookat_vectorX.at(path_iterator).y, + lookat_vectorZ.at(path_iterator).y, + ups_vectorX.at(path_iterator).x - path_vectorX.at(path_iterator).x, + ups_vectorX.at(path_iterator).y - path_vectorX.at(path_iterator).y, + ups_vectorZ.at(path_iterator).y - path_vectorZ.at(path_iterator).y); + } else { + if (cameraNavMouseMode == 1) { + glRotated( mouseRotX, 1, 0, 0); + glRotated( mouseRotY, 0, 1, 0); + glRotated( mouseRotZ, 0, 0, 1); + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + update_view_rotate(0); + } else { + double t[3] = {0,0,0}; + double mat[16]; + QuatToMatrix4(quat, t, mat); + glMultMatrixd(mat); + + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + //glRotated(angle, axis[0], axis[1], axis[2]); // rotate the camera + double rPT[3]; + Matrix4ToEuler(mat, rPT); + mouseRotX = deg(rPT[0]); + mouseRotY = deg(rPT[1]); + mouseRotZ = deg(rPT[2]); + + } + updateControls(); + + glTranslated(X, Y, Z); // move camera + } + +// cout << "Position :" << X << " " << Y << " " << Z << endl; +// cout << "Quaternion:" << quat[0] << " " << quat[1] << " " << quat[2] << " " << quat[3] << endl; +// cout << "Axis/Angle:" << axis[0] << " " << axis[1] << " " << axis[2] << " " << angle << endl; +// cout << "Apex angle:" << cangle << endl; +// cout << endl; + + // process fog + if (show_fog > 0) { + if (show_fog > 3) + fogColor[0] = fogColor[1] = fogColor[2] = fogColor[3] = 1.0; + else + fogColor[0] = fogColor[1] = fogColor[2] = fogColor[3] = 0.0; + glEnable(GL_FOG); + { + // ln(1/2^8) = -5.54517744 -> threshold at which the last color bit is gone due to fog + if (show_fog==1) {fogMode = GL_EXP; fardistance = min(5.54517744 / fogDensity, (double)maxfardistance);} + else if (show_fog==2) {fogMode = GL_EXP2; fardistance = min(sqrt(5.54517744) / fogDensity, (double)maxfardistance);} + else if (show_fog==3) {fogMode = GL_LINEAR; fardistance = 32000.0;} + else if (show_fog==4) {fogMode = GL_EXP; fardistance = (double)maxfardistance; } + else if (show_fog==5) {fogMode = GL_EXP2; fardistance = (double)maxfardistance; } + else if (show_fog==6) {fogMode = GL_LINEAR; fardistance = (double)maxfardistance;} + glFogi(GL_FOG_MODE, fogMode); + glFogfv(GL_FOG_COLOR, fogColor); + glFogf(GL_FOG_DENSITY, fogDensity); + glHint(GL_FOG_HINT, GL_FASTEST); + glFogf(GL_FOG_START, neardistance); + glFogf(GL_FOG_END, maxfardistance); + } + } else { + glDisable(GL_FOG); + fardistance = maxfardistance; + } + if (fardistance > maxfardistance) fardistance = maxfardistance; + if ( fabs(oldfardistance - fardistance) > 0.00001 || fabs(oldneardistance - neardistance) > 0.00001 ) { + oldfardistance = fardistance; + oldneardistance = neardistance; + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + } + + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // + // show the objects __after__ the model-transformation + // for all status variables we show the appropiated thing + // using the drawing functions + // + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + if (show_path == 1) { + double *pose; + glColor4d(1.0, 0.0, 0.0, 1.0); + glLineWidth(5); + glBegin(GL_LINE_STRIP); + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + // set usable type + Scan::AlgoType type; + if((unsigned int)frameNr >= MetaMatrix[i].size()) { + type = MetaAlgoType[i].back(); + } else { + type = MetaAlgoType[i][frameNr]; + } + if(frameNr >= 1 && frameNr < (int)MetaMatrix[i].size()) { + if(type == Scan::INVALID) continue; + // avoid incomplete frames in a scan + if((unsigned int)frameNr >= MetaMatrix[i].size()) + pose = MetaMatrix[i].back(); + else + pose = MetaMatrix[i][frameNr]; + } else { + //pose = MetaMatrix[i].back(); + // avoid incomplete frames in a scan + if((unsigned int)current_frame >= MetaMatrix[i].size()) + pose = MetaMatrix[i].back(); + else + pose = MetaMatrix[i][current_frame]; + } + if(showTopView) { + glVertex3f(pose[12], 2000, pose[14]); + } else { + glVertex3f(pose[12], pose[13], pose[14]); + } + } + glEnd(); + } + + // if show camera is true then draw cameras. + if (show_cameras == 1) { + DrawCameras(); + } + + // if show path is true the draw path. + if (show_path == 1) { + DrawPath(); + } + DrawObjects(mode); + + if (label) DrawUrl(); + + // if show points is true the draw points + if (show_points == 1) DrawPoints(mode, interruptable); + + + glPopMatrix(); + + if (!invert) { + glDisable(GL_COLOR_LOGIC_OP); + } + + // force draw the scene + glFlush(); + glFinish(); +} + +void DrawUrl() { + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + glLoadIdentity(); + glMatrixMode(GL_PROJECTION); + + // Save the current projection matrix + glPushMatrix(); + // Make the current matrix the identity matrix + glLoadIdentity(); + + // Set the projection (to 2D orthographic) + glOrtho(0.0,100.0,0.0,100.0,-1.5,1.5); + + glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // TODO + + glColor4d(0.0,0.0,0.0,0.7); + + glBegin(GL_QUADS); + glVertex3f(0,0,1.49); + glVertex3f(0,6,1.49); + if(smallfont) { + glVertex3f(22,6,1.49); + glVertex3f(22,0,1.49); + } else { + glVertex3f(25,6,1.49); + glVertex3f(25,0,1.49); + } + glEnd(); + + glBlendFunc(GL_ONE, GL_ZERO); + glColor3f(1,1,1); + if(smallfont) { + glRasterPos3f(1,3.5,1.5); + _glutBitmapString(GLUT_BITMAP_8_BY_13, "created by 3DTK"); + glRasterPos3f(1,1,1.5); + _glutBitmapString(GLUT_BITMAP_8_BY_13, "http://threedtk.de"); + } else { + glRasterPos3f(1,3.5,1.5); + _glutBitmapString(GLUT_BITMAP_9_BY_15, "created by 3DTK"); + glRasterPos3f(1,1,1.5); + _glutBitmapString(GLUT_BITMAP_9_BY_15, "http://threedtk.de"); + } + + // Restore the original projection matrix + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + glPopMatrix(); + +} +/** + * Function topview. Set the screen for top view. + */ +void topView() +{ + static GLdouble save_qx, save_qy, save_qz, save_qangle, save_X, save_Y, save_Z; + static GLdouble saveMouseRotX, saveMouseRotY, saveMouseRotZ; + + if (!showTopView) // set to top view + { + showTopView = true; + // save current pose + save_X = X; + save_Y = Y; + save_Z = Z; + save_qx = quat[0]; + save_qy = quat[1]; + save_qz = quat[2]; + save_qangle = quat[3]; + saveMouseRotX = mouseRotX; + saveMouseRotY = mouseRotY; + saveMouseRotZ = mouseRotZ; + + Y = Y - 350.0; + Z = Z + 500.0; + quat[0] = quat[1] = sqrt(0.5); + quat[2] = quat[3] = 0.0; + mouseRotX = 90; + mouseRotY = 0; + mouseRotZ = 0; + + haveToUpdate = 2; + } else { + showTopView = false; + + // restore old settings + X = save_X; + Y = save_Y; + Z = save_Z; + quat[0] = save_qx; + quat[1] = save_qy; + quat[2] = save_qz; + quat[3] = save_qangle; + mouseRotX = saveMouseRotX; + mouseRotY = saveMouseRotY; + mouseRotZ = saveMouseRotZ; + + haveToUpdate = 2; + } +} + +//--------------------------------------------------------------------------- +/** + * This function is called when the user wants to + * delete a camera. + */ + +void callDeleteCamera(int dummy){ + + //iterator for the position of camera + //in the camera list + vector::iterator position; + vector::iterator positionL; + vector::iterator positionU; + + //calculate the position of the camera. we are referring + //to the selected camera + position = cams.begin()+ (cam_choice-1); + positionL = lookats.begin()+ (cam_choice-1); + positionU = ups.begin()+ (cam_choice-1); + + //if no camera present then return + if(cam_choice == 0) + return; + + //if the list is not empty then + if(!cams.empty()){ + //delete the camera from the position + cams.erase(position); + lookats.erase(positionL); + ups.erase(positionU); + //reset the cam_choice spinner values + } + + updateCamera(); +} + + +//--------------------------------------------------------------------------- +/** + * Function to reset the viewer window. + */ + +void resetView(int dummy) +{ + cangle = 60.0; + pzoom = defaultZoom; + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + haveToUpdate = 2; + mouseRotX = 0; + mouseRotY = 0; + mouseRotZ = 0; + + resetRotationButton(); +} + +/** + * Function to set the viewer window back to a previously saved state. + */ + +void setView(double pos[3], double new_quat[4], + double newMouseRotX, double newMouseRotY, double newMouseRotZ, + double newCangle, + bool sTV, bool cNMM, double pzoom_new, + bool s_points, bool s_path, bool s_cameras, double ps, int + sf, double fD, bool inv) +{ + X = pos[0]; + Y = pos[1]; + Z = pos[2]; + for(int i = 0; i < 4; i++) { + quat[i] = new_quat[i]; + } + cangle = newCangle; + mouseRotX = newMouseRotX; + mouseRotY = newMouseRotY; + mouseRotZ = newMouseRotZ; + showTopView = sTV, + cameraNavMouseMode = cNMM; + pzoom = pzoom_new; + updateTopViewControls(); + + show_points = s_points; + show_path = s_path; + show_cameras = s_cameras; + pointsize = ps; + show_fog = sf; + fogDensity = fD; + invert = inv; + + haveToUpdate = 2; +} + + +/** + * This function is called when the viewer is created. This + * acts as the display function. + */ +void CallBackDisplayFunc() +{ + if ((cangle_spinner != 0 && (fabs(cangle_old - cangle) > 0.5)) || + (pzoom_spinner != 0 && (fabs(pzoom_old - pzoom) > 0.5))) { + + cangle_old = cangle; + pzoom_old = pzoom; + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); +#ifdef _MSC_VER + Sleep(25); +#else + usleep(250000); +#endif + } + + glDrawBuffer(buffermode); + // delete framebuffer and z-buffer + + //Call the display function + DisplayItFunc(GL_RENDER ); + + // show the rednered scene + glutSwapBuffers(); + +} + +/** + * This function is called when there is nothing to be done + * in the screen. + */ +void CallBackIdleFunc(void) +{ + +#ifdef _MSC_VER + Sleep(1); +#else + usleep(1000); +#endif + + if(glutGetWindow() != window_id) + glutSetWindow(window_id); + + // return as nothing has to be updated + if (haveToUpdate == 0) { + if (!fullydisplayed && !mousemoving && !keypressed && pointmode == 0 + ) { + glDrawBuffer(buffermode); + //Call the display function + DisplayItFunc(GL_RENDER, true); + } + return; + } + + // case: display is invalid - update it + if (haveToUpdate == 1) { + glutPostRedisplay(); + haveToUpdate = 0; + return; + } + // case: display is invalid - update it with all points +/* if (haveToUpdate == 7) { + showall = true; + glutPostRedisplay(); + haveToUpdate = 0; + return; + }*/ + + // case: camera angle is changed - instead of repeating code call Reshape, + // since these OpenGL commands are the same + if (haveToUpdate == 2) { + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + CallBackReshapeFunc(viewport[2],viewport[3]); + glutPostRedisplay(); + haveToUpdate = 0; + return; + } + + // case: animation + if(haveToUpdate == 3 ){ + frameNr += 1; + if(!(MetaMatrix.size() > 1 && frameNr < (int) MetaMatrix[1].size())){ + frameNr = 0; + haveToUpdate = 4; + return; + } + glutPostRedisplay(); + + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(frameNr,5) + ".ppm"; + cout << "write " << filename << endl; + int tmpUpdate = haveToUpdate; + glWriteImagePPM(filename.c_str(), factor, 0); + haveToUpdate = tmpUpdate; + + string jpgname = scan_dir + "animframe" + to_string(frameNr,5) + ".jpg"; + string systemcall = "convert -quality 100 -type TrueColor " + filename + " " + jpgname; + // cout << systemcall << endl; + system(systemcall.c_str()); + systemcall = "rm " + filename; + system(systemcall.c_str()); + // cout << systemcall << endl; + // for f in *ppm ; do convert -quality 100 -type TrueColor $f `basename $f ppm`jpg; done + } + + } +#ifdef _MSC_VER + Sleep(300); + Sleep(anim_delay); +#else + usleep(anim_delay * 10000); +#endif + + if (haveToUpdate == 4) { // stop animation + frameNr = 0; // delete these lines if you want a 'continue' functionality. + haveToUpdate = 1; + } + + // case: path animation + if(haveToUpdate == 6){ + + if (path_iterator == 0) { + oldcamNavMode = cameraNavMouseMode; // remember state of old mousenav + cameraNavMouseMode = 0; + } + + // check if the user wants to animate both + // scan matching and the path at the same + //time + + // cout << "path_iterator: " << path_iterator << endl; + if(path_iterator < path_vectorX.size()){ // standard animation case + + // call the path animation function + // hide both the cameras and the path + show_cameras = 0; + show_path = 0; + // increase the iteration count + + path_iterator += 1; + // repaint the screen + glutPostRedisplay(); + + // save the animation + if(save_animation){ + string filename = scan_dir + "animframe" + to_string(path_iterator,5) + ".ppm"; + string jpgname = scan_dir + "animframe" + to_string(path_iterator,5) + ".jpg"; + cout << "written " << filename << " of " << path_vectorX.size() << " files" << endl; + glWriteImagePPM(filename.c_str(), factor, 0); + string systemcall = "convert -quality 100 " + filename + " " + jpgname; + system(systemcall.c_str()); + systemcall = "rm " + filename; + system(systemcall.c_str()); + haveToUpdate = 6; + + } + }else{ // animation has just ended + cameraNavMouseMode = oldcamNavMode; + show_cameras = 1; + show_path = 1; + haveToUpdate = 0; + } + } + +} + + +/** + * This function handles the rotation of the view + */ + +void update_view_rotate(int t) +{ + double view_rotate_button_quat[4]; + + // convert the rotate button matrix to quaternion + //Matrix4ToQuaternion(view_rotate_button, view_rotate_button_quat); + double mat[16]; + for (int i = 0; i < 16; i++) + mat[i] = view_rotate_button[i]; + Matrix4ToQuat(mat, view_rotate_button_quat); + + // normalize the quartenion + QuatNormalize(view_rotate_button_quat); + + // copy it to the global quartenion quat + memcpy(quat, view_rotate_button_quat, sizeof(quat)); +} + +/** + * This function handles the translation of view. + */ +void update_view_translation(int t) +{ + double obj_pos_button1[3]; + + for (int i = 0; i < 3; i++) { + if (fabs(obj_pos_button_old[i] - obj_pos_button[i]) > COMPARE_EPSILON) { + obj_pos_button1[i] = obj_pos_button[i] - obj_pos_button_old[i]; + obj_pos_button_old[i] = obj_pos_button[i]; + } else obj_pos_button1[i] = 0.0; + } + + X = X + obj_pos_button1[0] * view_rotate_button[0] + obj_pos_button1[1] * view_rotate_button[1] + obj_pos_button1[2] * view_rotate_button[2]; + Y = Y + obj_pos_button1[0] * view_rotate_button[4] + obj_pos_button1[1] * view_rotate_button[5] + obj_pos_button1[2] * view_rotate_button[6]; + Z = Z + obj_pos_button1[0] * view_rotate_button[8] + obj_pos_button1[1] * view_rotate_button[9] + obj_pos_button1[2] * view_rotate_button[10]; + +} + + +/** + * handles the animation button + * @param dummy not needed necessary for glui + */ +void startAnimation(int dummy) +{ + if (MetaMatrix.size() > 0) { + if (haveToUpdate != 3) { + haveToUpdate = 3; + } + else // stop animation + haveToUpdate = 4; + } +} + +/** + * calls the resetView function + * @param dummy not needed necessary for glui + */ +void callResetView(int dummy) +{ + if (showTopView) callTopView(dummy); + resetView(0); +} + +/** + * calls the resetView function + * @param dummy not needed necessary for glui + */ +void invertView(int dummy) +{ + invert = !invert; +} + +/** + * calls the topView function + * @param dummy not needed necessary for glui + */ +void callTopView(int dummy) +{ + topView(); + if (showTopView) { + rotButton->disable(); + cangle_spinner->disable(); + pzoom_spinner->enable(); + } else { + rotButton->enable(); + cangle_spinner->enable(); + pzoom_spinner->disable(); + } +} + +/** + * calls the cameraView function + * @param dummy not needed necessary for glui + */ +void callAddCamera(int dummy) +{ + Point campos(-X, -Y, -Z); + + // calculate lookat point + Point lookat; + Point up(0, 0, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = view_rotate_button[i]; + + lookat.x = -50*tmat[2] -X; + lookat.y = -50*tmat[6] -Y; + lookat.z = -50*tmat[10] -Z; + + up.x = 50*tmat[1] -X; + up.y = 50*tmat[5] -Y; + up.z = 50*tmat[9] -Z; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + + updateCamera(); + + // signal to repaint screen + haveToUpdate = 1; +} + +void selectPoints(int x, int y) { + + GLuint selectBuf[BUFSIZE]; + GLint hits; + GLint viewport[4]; + if (selectOrunselect) { + // set the matrix mode + glMatrixMode(GL_MODELVIEW); + // init modelview matrix + glLoadIdentity(); + + // do the model-transformation + if (cameraNavMouseMode == 1) { + glRotated( mouseRotX, 1, 0, 0); + glRotated( mouseRotY, 0, 1, 0); + glRotated( mouseRotZ, 0, 0, 1); + } else { + double t[3] = {0,0,0}; + double mat[16]; + QuatToMatrix4(quat, t, mat); + glMultMatrixd(mat); + + glGetFloatv(GL_MODELVIEW_MATRIX, view_rotate_button); + double rPT[3]; + Matrix4ToEuler(mat, rPT); + mouseRotX = deg(rPT[0]); + mouseRotY = deg(rPT[1]); + mouseRotZ = deg(rPT[2]); + } + updateControls(); + glTranslated(X, Y, Z); // move camera + + static sfloat *sp2 = 0; +/* for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + if (!selected_points[iterator].empty()) sp2 = *selected_points[iterator].begin(); + + // selected_points[iterator].clear(); + }*/ + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + glPushMatrix(); + glMultMatrixd(MetaMatrix[iterator].back()); + calcRay(x, y, 1.0, 40000.0); + if (select_voxels) { + octpts[iterator]->selectRay(selected_points[iterator], selection_depth); + } else if (brush_size == 0) { + sfloat *sp = 0; + octpts[iterator]->selectRay(sp); + if (sp != 0) { + cout << "Selected point: " << sp[0] << " " << sp[1] << " " << sp[2] << endl; + + if (sp2 != 0) { + cout << "Distance to last point: " + << sqrt( sqr(sp2[0] - sp[0]) + sqr(sp2[1] - sp[1]) + sqr(sp2[2] - sp[2]) ) << endl; + } + sp2 = sp; + + selected_points[iterator].insert(sp); + } + } else { // select multiple points with a given brushsize + octpts[iterator]->selectRayBrushSize(selected_points[iterator], brush_size); + } + + glPopMatrix(); + } + + } else { + // unselect points + glGetIntegerv(GL_VIEWPORT, viewport); + + glSelectBuffer(BUFSIZE, selectBuf); + (void) glRenderMode(GL_SELECT); + + glInitNames(); + glPushName(0); + + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + +// gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), 10.0, 10.0, viewport); + gluPickMatrix((GLdouble)x, (GLdouble)(viewport[3]-y), brush_size*2, brush_size*2, viewport); + gluPerspective(cangle, aspect, neardistance, fardistance); + glMatrixMode(GL_MODELVIEW); + DisplayItFunc(GL_SELECT); + + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + + hits = glRenderMode(GL_RENDER); // get hits + ProcessHitsFunc(hits, selectBuf); + } + glPopMatrix(); + glutPostRedisplay(); +} + +void CallBackMouseFuncMoving(int button, int state, int x, int y) +{ + + if( state == GLUT_DOWN) { + mousemoving = true; + } else { + mousemoving = false; + } +} + + +/** + * This function is called after a mousebutton has been pressed. + */ +void CallBackMouseFunc(int button, int state, int x, int y) +{ + // Are we selecting points or moving the camera? + if(cameraNavMouseMode != 1) { // selecting points + if (state == GLUT_DOWN && (button == GLUT_LEFT_BUTTON || button == GLUT_RIGHT_BUTTON)) { + selectPoints(x,y); + } + } else { + if( state == GLUT_DOWN) { + + mouseNavX = x; + mouseNavY = y; + mouseNavButton = button; + + mousemoving = true; + } else { + mouseNavButton = -1; + mousemoving = false; + } + } +} + + +void moveCamera(double x, double y, double z, double rotx, double roty, double rotz) { + interruptDrawing(); + double mat[9]; + + double xr = M_PI * mouseRotX / 180; + double yr = M_PI * mouseRotY / 180; + double zr = M_PI * mouseRotZ / 180; + double c1,c2,c3,s1,s2,s3; + s1 = sin(xr); c1 = cos(xr); + s2 = sin(yr); c2 = cos(yr); + s3 = sin(zr); c3 = cos(zr); + mat[0] = c2*c3; + mat[1] = -c2*s3; + mat[2] = s2; + mat[3] = c1*s3+c3*s1*s2; + mat[4] = c1*c3-s1*s2*s3; + mat[5] = -c2*s1; + mat[6] = s1*s3-c1*c3*s2; + mat[7] = c1*s2*s3+c3*s1; + mat[8] = c1*c2; + + double transX, transY, transZ; + transX = transY = transZ = 0.0; + + mouseRotX += rotx; + mouseRotY -= roty; + mouseRotZ -= rotz; + + if (mouseRotX < -90) mouseRotX=-90; + else if (mouseRotX > 90) mouseRotX=90; + if (mouseRotY > 360) mouseRotY-=360; + else if (mouseRotY < 0) mouseRotY+=360; + if (mouseRotZ > 360) mouseRotZ-=360; + else if (mouseRotZ < 0) mouseRotZ+=360; + + transX += x * mat[0] + y * mat[3] + z * mat[6]; + transY += x * mat[1] + y * mat[4] + z * mat[7]; + transZ += x * mat[2] + y * mat[5] + z * mat[8]; + + + X += transX; + Y += transY; + Z += transZ; + haveToUpdate = 1; + +} + +void KeyboardFunc(int key, bool control, bool alt, bool shift) { + double stepsize = movementSpeed; + if (shift) stepsize *= 10.0; + if (control) stepsize *= 0.1; + + double rotsize = 0.2 * stepsize; + + switch (key) { + case 'w': + case 'W': + moveCamera(0,0,stepsize,0,0,0); + break; + case 'a': + case 'A': + moveCamera(stepsize,0,0,0,0,0); + break; + case 's': + case 'S': + moveCamera(0,0,-stepsize,0,0,0); + break; + case 'd': + case 'D': + moveCamera(-stepsize,0,0,0,0,0); + break; + case 'c': + case 'C': + moveCamera(0,stepsize,0,0,0,0); + break; + case 32: // WXK_SPACE + moveCamera(0,-stepsize,0,0,0,0); + break; + case 314: // WXK_LEFT + moveCamera(0,0,0,0,rotsize,0); + break; + case 315: // WXK_UP + moveCamera(0,0,0,rotsize,0,0); + break; + case 316: // WXK_RIGHT + moveCamera(0,0,0,0,-rotsize,0); + break; + case 317: // WXK_DOWN + moveCamera(0,0,0,-rotsize,0,0); + break; + case 'q': + case 'Q': + case 366: // WXK_PAGEUP + moveCamera(0,0,0,0,0,rotsize); + break; + case 'e': + case 'E': + case 367: // WXK_PAGEDOWN + moveCamera(0,0,0,0,0,-rotsize); + break; + case 'f': + if (!fullscreen) { + fullscreen = true; + glutFullScreen(); + } else { + fullscreen = false; + glutReshapeWindow(current_width, current_height); + } + break; + default: + break; + } +} + +void CallBackMouseMotionFunc(int x, int y) { + double deltaMouseX = x - mouseNavX; + double deltaMouseY = mouseNavY - y; + mouseNavX = x; + mouseNavY = y; + + if(cameraNavMouseMode == 1) { + if( mouseNavButton == GLUT_RIGHT_BUTTON){ + if ( showTopView ) { + deltaMouseX *= 5; + deltaMouseY *= 5; + } + deltaMouseX *= movementSpeed/10.0; // moving 10 pixels is equivalent to one key stroke + deltaMouseY *= movementSpeed/10.0; + moveCamera(deltaMouseX, deltaMouseY, 0, 0,0,0); + } else if( mouseNavButton == GLUT_MIDDLE_BUTTON ){ + if ( !showTopView ) { + deltaMouseY *= -5; + } + deltaMouseX *= movementSpeed/10.0; // moving 10 pixels is equivalent to one key stroke + deltaMouseY *= movementSpeed/10.0; + moveCamera(deltaMouseX, 0, deltaMouseY, 0,0,0); + } else if ( mouseNavButton == GLUT_LEFT_BUTTON ){ + moveCamera(0, 0, 0, deltaMouseY,deltaMouseX,0); + } else { + return; + } + } else { + selectPoints(x,y); + } +} + + + +void initScreenWindow() +{ + // init display + glutInitDisplayMode(GLUT_DEPTH | GLUT_RGBA | GLUT_DOUBLE); + + // define the window position and size + glutInitWindowPosition(START_X, START_Y); + glutInitWindowSize( START_WIDTH, START_HEIGHT ); + + // create window and append callback functions + window_id = glutCreateWindow("3D_Viewer"); + + glutDisplayFunc( CallBackDisplayFunc ); + glutReshapeFunc( CallBackReshapeFunc ); + + glutMouseFunc ( CallBackMouseFunc ); + glutKeyboardFunc ( CallBackKeyboardFunc); + glutKeyboardUpFunc ( CallBackKeyboardUpFunc); + glutMotionFunc ( CallBackMouseMotionFunc); + glutSpecialFunc ( CallBackSpecialFunc); + // glutEntryFunc ( CallBackEntryFunc); + GLUI_Master.set_glutReshapeFunc( CallBackReshapeFunc ); + GLUI_Master.set_glutIdleFunc( CallBackIdleFunc ); + + update_view_rotate(0); + glClearColor(0.0, 0.0, 0.0, 0.0); + // glClearColor(1.0, 1.0, 1.0, 1.0); +} + + +/* +++++++++-------------++++++++++++ + * NAME + * glDumpWindowPPM + * DESCRIPTION + * writes an ppm file of the window + * content + * PARAMETERS + * filename + * RESULT + * writes the framebuffer content + * to a ppm file ++++++++++-------------++++++++++++ */ +void glDumpWindowPPM(const char *filename, GLenum mode) +{ + int win_height, win_width; + int i,j,k,l; // Counter variables + GLubyte *buffer; // The GL Frame Buffer + unsigned char *ibuffer; // The PPM Output Buffer + ofstream fp; // The PPM File + + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + win_height = viewport[3]; + win_width = viewport[2]; + + // Allocate memory for the the frame buffer and output buffer + buffer = new GLubyte[win_width * win_height * RGBA]; + ibuffer = new unsigned char[win_width * win_height * RGB]; + + // Read window contents from GL frame buffer with glReadPixels + glFinish(); + glReadBuffer(buffermode); + glReadPixels(0, 0, win_width, win_height, + GL_RGBA, GL_UNSIGNED_BYTE, buffer); + + // Open the output file + fp.open(filename, ios::out); + + // Write a proper P6 PPM header + fp << "P6" << endl << "# CREATOR: 3D_Viewer by Andreas Nuechter, University of Osnabrueck" + << endl << win_width << " " << win_height << " " << UCHAR_MAX << endl; + + // Loop through the frame buffer data, writing to the PPM file. Be careful + // to account for the frame buffer having 4 bytes per pixel while the + // output file has 3 bytes per pixel + l = 0; + for (i = 0; i < win_height; i++) { // For each row + for (j = 0; j < win_width; j++) { // For each column + for (k = 0; k < RGB; k++) { // For each RGB component + //cout << (RGBA*((win_height-1-i)*win_width+j)+k) << endl; + ibuffer[l++] = (unsigned char) + *(buffer + (RGBA*((win_height-1-i)*win_width+j)+k)); + } // end RGB + } // end column + } // end row + + // to make a video do: + // for f in *ppm ; do convert -quality 100 $f `basename $f ppm`jpg; done + // mencoder "mf://*.jpg" -mf fps=10 -o test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800 + // Write output buffer to the file */ + fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * win_width * win_height)); + fp.close(); + fp.clear(); + delete [] buffer; + delete [] ibuffer; +} + +/* +++++++++-------------++++++++++++ + * NAME + * glDumpWindowPPM + * DESCRIPTION + * writes an ppm file of the window + * content + * size is scale times the window size + * PARAMETERS + * filename + * RESULT + * writes the framebuffer content + * to a ppm file ++++++++++-------------++++++++++++ */ +void glWriteImagePPM(const char *filename, int scale, GLenum mode) +{ + //if(!showTopView) { + int m,o,k; // Counter variables + // Get viewport parameters + double left, right, top, bottom; + double tmp = 1.0/tan(rad(cangle)/2.0); + + // Save camera parameters + GLdouble savedMatrix[16]; + glGetDoublev(GL_PROJECTION_MATRIX,savedMatrix); + GLdouble savedModel[16]; + glGetDoublev(GL_MODELVIEW_MATRIX,savedModel); + //glMatrixMode(GL_PROJECTION); + //glPushMatrix(); + //glMatrixMode(GL_MODELVIEW); + //glPushMatrix(); + top = 1.0/tmp; + bottom = -top; + right = (aspect)/tmp; + left = -right; + + double part_height, part_width; + if(!showTopView) { + part_width = (right - left)/(double)scale; + part_height = (top - bottom)/(double)scale; + } else { + part_height = (2*pzoom)/scale; + part_width = (2*pzoom*aspect)/scale; + cout << part_width << " " << part_height << endl; + } + // Calculate part parameters + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + int win_width = viewport[2]; + int win_height = viewport[3]; + int image_width = scale * win_width; + int image_height = scale * win_height; + + // Allocate memory for the the frame buffer and output buffer + GLubyte *buffer; // The GL Frame Buffer + unsigned char *ibuffer; // The PPM Output Buffer + buffer = new GLubyte[win_width * win_height * RGBA]; + ibuffer = new unsigned char[image_width * image_height * RGB]; + + smallfont = (scale==1); + double height; + if(!showTopView) { + height = bottom; + } else { + height = -pzoom; + } + for(int i = 0; i < scale; i++) { + double width; + if(!showTopView) { + width = left; + } else { + width = -pzoom*aspect; + } + for(int j = 0; j < scale; j++) { + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + label = false; + if(!showTopView) { + glFrustum(neardistance*width, neardistance*(width + part_width), + neardistance*(height), + neardistance*(height + part_height), + neardistance, fardistance); + glMatrixMode(GL_MODELVIEW); + if(i==0 && j==0) { + label = true; + } + DisplayItFunc(mode); + } else { + glOrtho( width, width + part_width, + height, height + part_height, + 1.0, 32000.0 ); + glMatrixMode(GL_MODELVIEW); + if(i==0 && j==0) { + label = true; + } + DisplayItFunc(mode); + } + + // Read window contents from GL frame buffer with glReadPixels + glFinish(); + glReadBuffer(buffermode); + glReadPixels(0, 0, win_width, win_height, GL_RGBA, GL_UNSIGNED_BYTE, buffer); + + // Loop through the frame buffer data, writing to the PPM file. Be careful + // to account for the frame buffer having 4 bytes per pixel while the + // output file has 3 bytes per pixel + // end row + for (m = 0; m < win_height; m++) { // For each row + for (o = 0; o < win_width; o++) { // For each column + for (k = 0; k < RGB; k++) { // For each RGB component + int l = (k+RGB*(image_width*((scale - 1 - i)*win_height + m) + j*win_width + o)); + ibuffer[l] = (unsigned char) *(buffer + (RGBA*((win_height-1-m)*win_width+o)+k)); + } // end RGB + } // end column + } + width += part_width; + } + height += part_height; + } + + // show the starting scene + + // Restore the original projection matrix + glMatrixMode(GL_PROJECTION); + glLoadMatrixd(savedMatrix); + glMatrixMode(GL_MODELVIEW); + glLoadMatrixd(savedModel); + // show the rednered scene + label = true; + smallfont = true; + haveToUpdate=2; + DisplayItFunc(mode); + + ofstream fp; // The PPM File + + // Open the output file + fp.open(filename, ios::out); + + // Write a proper P6 PPM header + fp << "P6" << endl << "# CREATOR: 3D_Viewer by Dorit Borrmann, Jacobs University Bremen gGmbH" + << endl << image_width << " " << image_height << " " << UCHAR_MAX << endl; + + // Write output buffer to the file + fp.write((const char*)ibuffer, sizeof(unsigned char) * (RGB * image_width * image_height)); + fp.close(); + fp.clear(); + delete [] buffer; + delete [] ibuffer; +} + +/** Reshape Function + * TODO: have to describe it. + * + */ +void CallBackReshapeFunc(int width, int height) +{ + if (!fullscreen) { + current_height = height; + current_width = width; + } + aspect = (double)width/(double)height; + if (!showTopView) { + // usage of the vsize of a structiewport + glViewport(0, 0, (GLint)width, (GLint)height); + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + + // angle, aspect, near clip, far clip + // get matrix + gluPerspective(cangle, aspect, neardistance, fardistance); + + // now use modelview-matrix as current matrix + glMatrixMode(GL_MODELVIEW); + + haveToUpdate = 1; + + } else { + + // usage of the viewport + glViewport ( 0, 0, width, height); + + glMatrixMode ( GL_PROJECTION ); + glLoadIdentity (); + + // get matrix + glOrtho ( -aspect * pzoom, aspect * pzoom, + -1 * pzoom, pzoom, + 1.0, 32000.0 ); + + // now use modelview-matrix as current matrix + glMatrixMode(GL_MODELVIEW); + + haveToUpdate = 1; + + } + // glDepthMask(false); + glEnable(GL_BLEND); // TODO + glBlendFunc(GL_ONE, GL_ZERO); // TODO + // glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // TODO + // glBlendFunc(GL_ONE, GL_ONE); // TODO + // glBlendFunc(GL_SRC_COLOR, GL_DST_COLOR); + glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); + // TODO glDepthFunc(GL_LEQUAL); + glDepthFunc(GL_LESS); //TODO + glEnable(GL_DEPTH_TEST); + glEnable (GL_POINT_SMOOTH); +} + +/** + * Prints out which points were clicked on + */ +void ProcessHitsFunc(GLint hits, GLuint buffer[]) +{ + //cout << "SIZE " << selected_points[0].size() << endl; + //cout << "processing " << endl; + set names; + set unsel_points; + + GLuint *ptr, nr_names; + + ptr = (GLuint *)buffer; + + for(int i = 0 ; i < hits ; i++) { + nr_names = *ptr; + ptr+=3; // skip 2 z values + for(unsigned int j = 0;j < nr_names ; j++){ // iterate over all names + names.insert(*ptr); + + ptr++; + } + } + //cout << "number of names " << names.size() << endl; + if (names.empty()) return; + + int index = 0; + set::iterator nit = names.begin(); + // find the respective name + + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { + // iterate over the selected points as in DrawPoints + for ( set::iterator it = selected_points[iterator].begin(); + it != selected_points[iterator].end(); it++) { + if (index == *nit) { // if the current index is the current name + unsel_points.insert(*it); + nit++; + } + if (nit == names.end()) goto Done; // break out of the loop + index++; + } + } + + Done: + + cout << "Erasing " << endl; + for (set::iterator it = unsel_points.begin(); + it != unsel_points.end(); it++) { // iterate to the index as indicated by the name *ptr + for(int iterator = (int)octpts.size()-1; iterator >= 0; iterator--) { // erase for all scans + selected_points[iterator].erase(*it); + } + } + + + cout << "processing done" << endl; + +} + + +//------------------------------------------------------------------ +/** + * This function deals with all our keyboard activities + */ + +void InterfaceFunc(unsigned char key){ + + strncpy(path_file_name, path_filename_edit->get_text(), 1024); + strncpy(pose_file_name, pose_filename_edit->get_text(), 1024); + return; +} + + +void CallBackSpecialFunc(int key , int x, int y) { + //KeyboardFunc(key + 214, false, false, false); + // return; +} + +/** + * Function drawRobotPath + * \brief This functions draws the path where the + * robot has travelled along while taking the scans + */ +void drawRobotPath(int dummy){ + // clear the camera list as we are going to add the cameras + // in the path where the robot travelled. + + // lets loop through the entire frame files to extract the + // total number of places where the robot has taken the scans from + for(unsigned int i = 0; i < MetaMatrix.size(); i++){ + //temp variable + double *temp; + // Now, lets go to the last of each frame file to + // extract the transformation matrix obtained + // after scan matching has been done. + glMultMatrixd(MetaMatrix[i].back()); + + // temp is final transformation matrix + temp = MetaMatrix[i].back(); + + Point campos(temp[12], temp[13] + 100, temp[14]); + + // calculate lookat point + Point lookat(0, 0, 50); + Point up(0, 50, 0); + double tmat[16]; + for (int i =0;i<16;i++) tmat[i] = temp[i]; + lookat.transform(tmat); + lookat.x = lookat.x ; + lookat.y = lookat.y + 100; + lookat.z = lookat.z ; + + up.transform(tmat); + up.x = up.x ; + up.y = up.y + 100; + up.z = up.z ; + + cams.push_back(campos); + lookats.push_back(lookat); + ups.push_back(up); + } + updateCamera(); + + // signal for the update of scene + haveToUpdate = 1; +} + +/** + * Calculates the positions of the interpolated camera path positions on the + * Nurbs path. There will be an equal number of intermediate positions between + * neighboring cameras. + */ +void calcInterpolatedCameras(vector vec1, vector vec2) { + NurbsPath::camRatio.clear(); + double distance = 0.0; + double dx, dy, dz; + for(unsigned int i=0;i vec1, vector vec2) +{ + double distance = 0.0; + double dx, dy, dz; + for(unsigned int i=0;i= 'A' && key <= 'Z') { + keymap[key+ ('a'-'A')] = false; + } + if (key >= 'a' && key <= 'z') { + keymap[key+ ('A'-'a')] = false; + } + + for (unsigned int i = 0; i < 256; i++) { + if (keymap[i]) { + keypressed = true; + return; + } + } + keypressed = false; +} + +void CallBackKeyboardFunc(unsigned char key, int x, int y) { + keymap[key] = true; + keypressed = true; + bool cmd,alt,shift; + cmd = glutGetModifiers() & GLUT_ACTIVE_CTRL; + alt = glutGetModifiers() & GLUT_ACTIVE_ALT; + shift = glutGetModifiers() & GLUT_ACTIVE_SHIFT; + if (cmd) { + key += 96; + } + KeyboardFunc(key, cmd, alt, shift); +} + +void mapColorToValue(int dummy) { + switch (listboxColorVal) { + case 0: + cm->setCurrentType(PointType::USE_HEIGHT); + break; + case 1: + cm->setCurrentType(PointType::USE_REFLECTANCE); + break; + case 2: + cm->setCurrentType(PointType::USE_TEMPERATURE); + break; + case 3: + cm->setCurrentType(PointType::USE_AMPLITUDE); + break; + case 4: + cm->setCurrentType(PointType::USE_DEVIATION); + break; + case 5: + cm->setCurrentType(PointType::USE_TYPE); + break; + case 6: + cm->setCurrentType(PointType::USE_COLOR); + break; + default: + break; + }; + resetMinMax(0); +} + +void changeColorMap(int dummy) { + ColorMap c; + GreyMap gm; + HSVMap hsv; + SHSVMap shsv; + JetMap jm; + HotMap hot; + DiffMap diff; + TempMap temp; + + switch (listboxColorMapVal) { + case 0: + // TODO implement no color map + cm->setColorMap(c); + break; + case 1: + cm->setColorMap(gm); + break; + case 2: + cm->setColorMap(hsv); + break; + case 3: + cm->setColorMap(jm); + break; + case 4: + cm->setColorMap(hot); + break; + case 5: + cm->setColorMap(diff); + break; + case 6: + cm->setColorMap(shsv); + break; + case 7: + cm->setColorMap(temp); + break; + default: + break; + } +} + +void minmaxChanged(int dummy) { + cm->setMinMax(mincolor_value, maxcolor_value); +} + +void resetMinMax(int dummy) { + mincolor_value = cm->getMin(); + maxcolor_value = cm->getMax(); + minmaxChanged(0); +} + +void setScansColored(int dummy) { + switch(colorScanVal) { + case 0: + cm->setMode(ScanColorManager::MODE_STATIC); + break; + case 1: + cm->setMode(ScanColorManager::MODE_COLOR_SCAN); + break; + case 2: + cm->setMode(ScanColorManager::MODE_POINT_COLOR); + break; + default: + break; + } +} + + +void changePointMode(int dummy) { + if (dummy == 0) { // always display + if (pointmode != 1) { // standard mode + pointmode = 1; + //never_box->set_int_val(0); + } else { + pointmode = 0; + } + } else if (dummy == 1) { // never display + if (pointmode != -1) { // standard mode + pointmode = -1; + //always_box->set_int_val(0); + } else { + pointmode = 0; + } + } + updatePointModeControls(); +} + + +void callCameraUpdate(int dummy) { + updateCamera(); +} + +void calcPointSequence(vector &sequence, int frameNr) { + sequence.clear(); + vector > dists; + double x,y,z; + + for (unsigned int i = 0; i < octpts.size(); i++) { + // stop at scans that don't have any frames associated with them + if(i >= MetaMatrix.size()) break; + // set usable frame + double* frame; + if((unsigned int)frameNr >= MetaMatrix[i].size()) { + // use last possible frame + frame = MetaMatrix[i].back(); + } else { + frame = MetaMatrix[i][frameNr]; + } + x = frame[12]; + y = frame[13]; + z = frame[14]; + dists.push_back( pair(sqr(X + x) + sqr(Y + y) + sqr(Z + z), i) ); + } + + sort( dists.begin(), dists.end()); + + for (unsigned int i = 0; i < dists.size(); i++) { + sequence.push_back( dists[i].second); + } +} diff --git a/.svn/pristine/45/459a8ac2a115f52554eef1c357fdebd3c79bf177.svn-base b/.svn/pristine/45/459a8ac2a115f52554eef1c357fdebd3c79bf177.svn-base new file mode 100644 index 0000000..d2562b0 --- /dev/null +++ b/.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 +#include +#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 diff --git a/.svn/pristine/49/494f2fcff752c79b0981b59126fa3ed5bd01acf7.svn-base b/.svn/pristine/49/494f2fcff752c79b0981b59126fa3ed5bd01acf7.svn-base new file mode 100644 index 0000000..91a79ec --- /dev/null +++ b/.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 \ No newline at end of file diff --git a/.svn/pristine/65/6566191d235553599f77174b9192fa4cb776e8ca.svn-base b/.svn/pristine/65/6566191d235553599f77174b9192fa4cb776e8ca.svn-base new file mode 100644 index 0000000..4badc29 --- /dev/null +++ b/.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::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 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 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 *oct = new BOctTree(PointerArray(xyz).get(), + xyz.size(), reduction_voxelSize, reduction_pointtype); + + vector 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(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(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 &diff, + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2) +{ + DataXYZ xyz_r(Source->get("xyz reduced")); + KDtree* kd = new KDtree(PointerArray(Target->get("xyz reduced")).get(), Target->size("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 *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(Source->get("xyz reduced")).get(), Source->size("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 *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 *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(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::iterator it = scans.begin(); it != scans.end(); ++it) { + unsigned int count = (*it)->size("xyz reduced"); + if(count > max) + max = count; + } + return max; +} diff --git a/.svn/pristine/68/686a5a70ccec35fc247ac32bf98d72287b251237.svn-base b/.svn/pristine/68/686a5a70ccec35fc247ac32bf98d72287b251237.svn-base new file mode 100644 index 0000000..e1921b1 --- /dev/null +++ b/.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 diff --git a/.svn/pristine/71/7159b4092ea0c92cd637e4b0cec8a72f7c0089ad.svn-base b/.svn/pristine/71/7159b4092ea0c92cd637e4b0cec8a72f7c0089ad.svn-base new file mode 100644 index 0000000..7f8a298 --- /dev/null +++ b/.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 + +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; +} diff --git a/.svn/pristine/7b/7bffcb83acee8c5d40e56b49cbfbb32967b24045.svn-base b/.svn/pristine/7b/7bffcb83acee8c5d40e56b49cbfbb32967b24045.svn-base new file mode 100644 index 0000000..940b03d --- /dev/null +++ b/.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 +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 +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 +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 +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 \ No newline at end of file diff --git a/.svn/pristine/80/80e4f43c46b62200ace933c4bc8098cd8b771da6.svn-base b/.svn/pristine/80/80e4f43c46b62200ace933c4bc8098cd8b771da6.svn-base new file mode 100644 index 0000000..1a02b0f --- /dev/null +++ b/.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 +void Ransac(CollisionShape &shape, Scan *scan, vector *best_points = 0) { + int best_score = 0; + CollisionShape *best = shape.copy(); + + // stores 3 sample points + vector ps; + // create octree from the points + DataXYZ xyz(scan->get("xyz reduced")); + RansacOctTree *oct = new RansacOctTree(PointerArray(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 diff --git a/.svn/pristine/80/80fa4b2f16331352477b08809318d265d9999eff.svn-base b/.svn/pristine/80/80fa4b2f16331352477b08809318d265d9999eff.svn-base new file mode 100644 index 0000000..3f5859b --- /dev/null +++ b/.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 diff --git a/.svn/pristine/89/893c94fbc7bdd37fa6d7865fcd5f04794fec237a.svn-base b/.svn/pristine/89/893c94fbc7bdd37fa6d7865fcd5f04794fec237a.svn-base new file mode 100644 index 0000000..d824b1f --- /dev/null +++ b/.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 + +#include +using std::vector; +#include +using std::deque; +#include +using std::set; +#include +using std::list; +#include +#include +#include + +#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 RansacOctTree : public BOctTree { + +public: + + template + RansacOctTree(P* const* pts, int n, T _voxelSize, PointType _pointtype = PointType() ) : BOctTree(pts, n, _voxelSize, _pointtype) {} + + // RansacOctTree(vector &pts, T voxelSize, PointType _pointtype = PointType() ) : BOctTree(pts, voxelSize, _pointtype) {} + + RansacOctTree(std::string filename) : BOctTree (filename) {} + + void DrawPoints(vector &p, unsigned char nrp) { + DrawPoints(p, *BOctTree::root, nrp); + } + + + unsigned long PointsOnShape(CollisionShape &shape) { + return PointsOnShape(*BOctTree::root, BOctTree::center, BOctTree::size, shape); + } + + void PointsOnShape(CollisionShape &shape, vector &points) { + PointsOnShape(*BOctTree::root, BOctTree::center, BOctTree::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 &shape ) { + if (! shape.isInCube(center[0], center[1], center[2], size)) { + return 0; + } + + T ccenter[3]; + bitunion *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::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 &shape, vector &vpoints) { + if (! shape.isInCube(center[0], center[1], center[2], size)) { + return; + } + + T ccenter[3]; + bitunion *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::POINTDIM]; + for (unsigned int iterator = 0; iterator < BOctTree::POINTDIM; iterator++) { + p[iterator] = point[iterator]; + } + vpoints.push_back(p); + } + point+= BOctTree::POINTDIM; + } + } + } else { // recurse + PointsOnShape( children->node, ccenter, size/2.0, shape, vpoints); + } + ++children; // next child + } + } + } + + + void DrawPoints(vector &p, bitoct &node, unsigned char nrp) { + bitunion *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::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::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 diff --git a/.svn/pristine/89/898e5bef351f7d0252d548094b5b5ae1d5042441.svn-base b/.svn/pristine/89/898e5bef351f7d0252d548094b5b5ae1d5042441.svn-base new file mode 100644 index 0000000..562ac44 --- /dev/null +++ b/.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 +using std::cout; +using std::cerr; +using std::endl; +#include +using std::swap; +#include +#include + +// KDtree class static variables +template +KDParams KDTreeImpl::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("xyz reduced original")), scan->size("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 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 lock(m_mutex_locking); + --m_count_locking; + if(m_count_locking == 0 && m_data != 0) { + delete m_data; + m_data = 0; + } +} diff --git a/.svn/pristine/8b/8bf13af2e1da80f949d9fbbedda0f35e1840884e.svn-base b/.svn/pristine/8b/8bf13af2e1da80f949d9fbbedda0f35e1840884e.svn-base new file mode 100644 index 0000000..761bae7 --- /dev/null +++ b/.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 +#include + +#include +#include + +//! SearchTree types +enum nns_type { + simpleKD, ANNTree, BOCTree +}; + +class Scan; +typedef std::vector 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 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("xyz reduced") + */ + template + 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 *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 &diff, + Scan* Source, Scan* Target, + int thread_num, + double max_dist_match2); + static void getPtPairsSimple(std::vector *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 *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 diff --git a/.svn/pristine/8c/8c20ecab50d40e311803ad7e83ce6eba44eb1821.svn-base b/.svn/pristine/8c/8c20ecab50d40e311803ad7e83ce6eba44eb1821.svn-base new file mode 100644 index 0000000..baf8cbd --- /dev/null +++ b/.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 +using std::cerr; + +#include + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#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("xyz reduced"); + int step = max / OPENMP_NUM_THREADS; + + vector 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 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("xyz reduced"); + int step = max / OPENMP_NUM_THREADS; + + vector 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 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 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); + } + } +} + diff --git a/.svn/pristine/8c/8c67cafbe8df18cdcae38de178295aceb23c703b.svn-base b/.svn/pristine/8c/8c67cafbe8df18cdcae38de178295aceb23c703b.svn-base new file mode 100644 index 0000000..98fee53 --- /dev/null +++ b/.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) diff --git a/.svn/pristine/8e/8edbc31d77e82218f78f4577d47d00a87bba870c.svn-base b/.svn/pristine/8e/8edbc31d77e82218f78f4577d47d00a87bba870c.svn-base new file mode 100644 index 0000000..a9e687a --- /dev/null +++ b/.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 Show_BOctTree : public colordisplay +{ +protected: + BOctTree* 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* tree, ScanColorManager* scm = 0) : + m_tree(tree) + { + init(scm); + } + + //! Create tree by points + template + Show_BOctTree(P * const* pts, int n, T voxelSize, PointType pointtype = PointType(), ScanColorManager *scm = 0) + { + m_tree = new BOctTree(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(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* 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 &points, int brushsize) { + selectRayBS(points, m_tree->getRoot(), m_tree->getCenter(), m_tree->getSize(), brushsize); + } + + void selectRay(set &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 *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 *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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *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::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 *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::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 &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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 &selpoints, const bitoct &node, const T* center, T size, int brushsize) { + T ccenter[3]; + bitunion *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 *children; + bitoct::getChildren(node, children); + + for (short i = 0; i < 8; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + BOctTree::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 diff --git a/.svn/pristine/92/92769b3f0cbdb909581dce5eb72ac79f3b4b593d.svn-base b/.svn/pristine/92/92769b3f0cbdb909581dce5eb72ac79f3b4b593d.svn-base new file mode 100644 index 0000000..1d7743e --- /dev/null +++ b/.svn/pristine/92/92769b3f0cbdb909581dce5eb72ac79f3b4b593d.svn-base @@ -0,0 +1,37 @@ +#ifndef __THERMO_H__ +#define __THERMO_H__ + +#include +#include +#include +#include +using namespace std; +//typedef vector > Float2D[1200][1600]; +typedef vector > 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 diff --git a/.svn/pristine/93/93ab3110d14d8d16d540a7cc2b137ba3f644dd08.svn-base b/.svn/pristine/93/93ab3110d14d8d16d540a7cc2b137ba3f644dd08.svn-base new file mode 100644 index 0000000..c3c2ded --- /dev/null +++ b/.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) diff --git a/.svn/pristine/98/98c86c975579fc3529238a3892d5ae76a563a763.svn-base b/.svn/pristine/98/98c86c975579fc3529238a3892d5ae76a563a763.svn-base new file mode 100644 index 0000000..f9e32a9 --- /dev/null +++ b/.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 +using std::cout; +using std::cerr; +using std::endl; +#include +using std::swap; +#include +#include + + + +/** + * 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&_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]; +} + diff --git a/.svn/pristine/9a/9a3d9fe78edded475f414ab692cc66858d5dfad2.svn-base b/.svn/pristine/9a/9a3d9fe78edded475f414ab692cc66858d5dfad2.svn-base new file mode 100644 index 0000000..0956967 --- /dev/null +++ b/.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 +#include +#include +using std::ifstream; +using std::ofstream; +using std::flush; +using std::string; +using std::map; +using std::pair; +using std::vector; + +#include +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 identifiers(sio->readDirectory(path.c_str(), start, end)); + + Scan::allScans.reserve(identifiers.size()); + + // for each identifier, create a scan + for(std::list::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 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(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 xyz; + vector rgb; + vector reflectance; + vector temperature; + vector amplitude; + vector type; + vector 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, + &litude, + &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(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(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(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(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(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(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(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>::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>::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>::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 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(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(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* btree = 0; + + // try to load from file, if successful return + if(octtree_loadOct && exists(scanFileName)) { + btree = new BOctTree(scanFileName); + m_data.insert( + std::make_pair( + "octtree", + std::make_pair( + reinterpret_cast(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(PointerArray(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true); + } else { // without reduction, xyz + attribute points + float** pts = octtree_pointtype.createPointArray(this); + unsigned int nrpts = size("xyz"); + btree = new BOctTree(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(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::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(frame.type); +} + +void BasicScan::addFrame(AlgoType type) +{ + m_frames.push_back(Frame(transMat, type)); +} diff --git a/.svn/pristine/a2/a270a2194e281599167e3dd2d589deb7ef689fe5.svn-base b/.svn/pristine/a2/a270a2194e281599167e3dd2d589deb7ef689fe5.svn-base new file mode 100644 index 0000000..878323e --- /dev/null +++ b/.svn/pristine/a2/a270a2194e281599167e3dd2d589deb7ef689fe5.svn-base @@ -0,0 +1,1306 @@ +/** + * @file + * @brief Globally used functions + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#ifndef __GLOBALS_ICC__ +#define __GLOBALS_ICC__ + +#ifdef _MSC_VER +#include +#define _USE_MATH_DEFINES +#include + +inline int gettimeofday (struct timeval* tp, void* tzp) +{ + unsigned long t; + t = timeGetTime(); + tp->tv_sec = t / 1000; + tp->tv_usec = t % 1000; + return 0; /* 0 indicates success. */ +} +#else +#include +#endif + +#define _USE_MATH_DEFINES +#include + +#if defined(__CYGWIN__) +# ifndef M_PI +# define M_PI 3.14159265358979323846 +# define M_PI_2 1.57079632679489661923 +# define M_PI_4 0.78539816339744830962 +# define M_1_PI 0.31830988618379067154 +# define M_2_PI 0.63661977236758134308 +# define M_SQRT2 1.41421356237309504880 +# define M_SQRT1_2 0.70710678118654752440 +# endif +#endif + +#include +using std::min; +using std::max; +#include +#include +using std::stringstream; +#include +using std::ostream; +using std::istream; +#include +using std::cout; +using std::endl; +#include +#include +using std::runtime_error; + +/** + * Set bits count + * + * @param unsigned char x + * + * @return char + * + */ +inline unsigned char _my_popcount_3(unsigned char x) { + x -= (x >> 1) & 0x55; //put count of each 2 bits into those 2 bits + x = (x & 0x33) + ((x >> 2) & 0x33); //put count of each 4 bits into those 4 bits + x = (x + (x >> 4)) & 0x0f; //put count of each 8 bits into those 8 bits + return x; +} + +/** + * Converts a class T to a string of width with padding 0 + * + * @param t output + * @param width length + * + * @return string of t + * + */ +template +inline std::string to_string(const T& t, int width) +{ + stringstream ss; + ss << std::setfill('0') << std::setw(width) << t; + return ss.str(); +} + +/** + * Converts a class T to a string of width with padding 0 + * + * @param t output + * @return string of t + * + */ +template +inline std::string to_string(const T& t) +{ + stringstream ss; + ss << t; + return ss.str(); +} + + +/** + * Overridden "<<" operator for sending a (4x4)-matrix to a stream + * + * @param os stream + * @param matrix 4x4 matrix sent to stream + * @return stream + */ +inline ostream& operator<<(ostream& os, const double matrix[16]) +{ + for (int i = 0; i < 16; os << matrix[i++] << " "); + return os; +} + +/** + * Overridden ">>" operator for reading a (4x4)-matrix from a stream.
+ * Throws a runtime error if not enough data in the stream. + * + * @param is stream + * @param matrix 4x4 matrix sent to stream + * @return stream +*/ +inline istream& operator>>(istream& is, double matrix[16]) +{ + for (int i = 0; i < 16; i++) { + if (!is.good()) throw runtime_error("Not enough elements to read for >>(istream&, double[16])"); + is >> matrix[i]; + } + return is; +} + + +/** + * Converts an angle (given in deg) to rad + * + * @param deg integer indicating, whether the figure to be drawn to show + * the clusters should be circles (0) or rectangles(1) + * + * @return the clustered image, with the clusters marked by colored figures + * + */ +template +inline T rad(const T deg) +{ + return ( (2 * M_PI * deg) / 360 ); +} + +/** + * Converts an angle (given in rad) to deg + * + * @param rad angle in rad + * @return angle in deg + */ +template +inline T deg(const T rad) +{ + return ( (rad * 360) / (2 * M_PI) ); +} + + +/** + * Calculates x^2 + * + * @param x input scalar value + * @return squared value + */ +template +static inline T sqr(const T &x) +{ + return x*x; +} + + +/** + * Computes the squared length of a 3-vector + * + * @param x input 3-vector + * @return length^2 of vector + */ +template +inline T Len2(const T *x) +{ + return sqr(x[0]) + sqr(x[1]) + sqr(x[2]); +} + + +/** + * Computes the length of a 3-vector + * + * @param x input 3-vector + * @return length of vector + */ +template +inline T Len(const T *x) +{ + return sqrt(Len2(x)); +} + + +/** + * Computes the squared Eucledian distance between two points + * in 3-space + * + * @param x1 first input vector + * @param x2 decond input vecotr + * @return Eucledian distance^2 between the two locations + */ +template +inline T Dist2(const T *x1, const F *x2) +{ + T dx = x2[0] - x1[0]; + T dy = x2[1] - x1[1]; + T dz = x2[2] - x1[2]; + + return sqr(dx) + sqr(dy) + sqr(dz); +} + +/* + * Normalization of the input 3-vector + * + * @param x input/output 3-vector + */ +template +static inline void Normalize3(T *x) +{ + T norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]); + x[0] /= norm; + x[1] /= norm; + x[2] /= norm; +} + +/* + * Normalization of the input 4-vector + * + * @param x input/output 4-vector + */ +template +static inline void Normalize4(T *x) +{ + T norm = sqrt((x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3])); + + x[0] /= norm; + x[1] /= norm; + x[2] /= norm; + x[3] /= norm; +} + +/** + * Sets a 4x4 matrix to identity + * + * @param M 4x4 matrix + */ +template +inline void M4identity( T *M ) +{ + M[0] = M[5] = M[10] = M[15] = 1.0; + M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0; +} + +/** + * Multiplies a 4x4 matrices in OpenGL + * (column-major) order + * + * @param M1 first input matrix + * @param M2 second input matrix + * @param Mout output matrix + * + */ +template +inline void MMult(const T *M1, + const T *M2, + T *Mout) +{ + Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; + Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; + Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; + Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; + Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; + Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; + Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; + Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; + Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; + Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; + Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; + Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; + Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; + Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; + Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; + Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; +} + +template +inline void MMult(const T *M1, + const T *M2, + float *Mout) +{ + Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; + Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; + Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3]; + Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3]; + Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7]; + Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7]; + Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7]; + Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7]; + Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11]; + Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11]; + Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11]; + Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11]; + Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15]; + Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15]; + Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15]; + Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15]; +} + +/** + * Transforms a vector with a matrix (P = M * V) + * @param M the transformation matrix + * @param v the initial vector + * @param p the transformt vector + */ +template +inline void VTrans(const T *M, const T *V, T *P) +{ + P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12]; + P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13]; + P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14]; +} + +/** + * Converts an Euler angle to a 3x3 matrix + * + * @param rPosTheta vector of Euler angles + * @param alignxf 3x3 matrix corresponding to the Euler angles + */ +inline void EulerToMatrix3(const double *rPosTheta, double *alignxf) +{ + double sx = sin(rPosTheta[0]); + double cx = cos(rPosTheta[0]); + double sy = sin(rPosTheta[1]); + double cy = cos(rPosTheta[1]); + double sz = sin(rPosTheta[2]); + double cz = cos(rPosTheta[2]); + + alignxf[0] = cy*cz; + alignxf[1] = sx*sy*cz + cx*sz; + alignxf[2] = -cx*sy*cz + sx*sz; + alignxf[3] = -cy*sz; + alignxf[4] = -sx*sy*sz + cx*cz; + alignxf[5] = cx*sy*sz + sx*cz; + alignxf[6] = sy; + alignxf[7] = -sx*cy; + alignxf[8] = cx*cy; +} + +/** + * Calculates the determinant of a 3x3 matrix + * + * @param M input 3x3 matrix + * @return determinant of input matrix + */ +template +inline double M3det( const T *M ) +{ + double det; + det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] ) + - M[1] * ( M[3]*M[8] - M[6]*M[5] ) + + M[2] * ( M[3]*M[7] - M[6]*M[4] )); + return ( det ); +} + +/** + * Inverts a 3x3 matrix + * + * @param Min input 3x3 matrix + * @param Mout output 3x3 matrix + */ +template +inline void M3inv( const T *Min, T *Mout ) +{ + double det = M3det( Min ); + + if ( fabs( det ) < 0.0005 ) { + M3identity( Mout ); + return; + } + + Mout[0] = (double)( Min[4]*Min[8] - Min[5]*Min[7] ) / det; + Mout[1] = (double)(-( Min[1]*Min[8] - Min[7]*Min[2] )) / det; + Mout[2] = (double)( Min[1]*Min[5] - Min[4]*Min[2] ) / det; + + Mout[3] = (double)(-( Min[3]*Min[8] - Min[5]*Min[6] )) / det; + Mout[4] = (double)( Min[0]*Min[8] - Min[6]*Min[2] ) / det; + Mout[5] = (double)(-( Min[0]*Min[5] - Min[3]*Min[2] )) / det; + + Mout[6] = (double) ( Min[3]*Min[7] - Min[6]*Min[4] ) / det; + Mout[7] = (double)(-( Min[0]*Min[7] - Min[6]*Min[1] )) / det; + Mout[8] = (double) ( Min[0]*Min[4] - Min[1]*Min[3] ) / det; +} + + +/** + * Converts a pose into a RT matrix + * @param *rPos Pointer to the position (double[3]) + * @param *rPosTheta Pointer to the angles (double[3]) + * @param *alignxf The calculated matrix + */ +inline void EulerToMatrix4(const double *rPos, const double *rPosTheta, double *alignxf) +{ + double sx = sin(rPosTheta[0]); + double cx = cos(rPosTheta[0]); + double sy = sin(rPosTheta[1]); + double cy = cos(rPosTheta[1]); + double sz = sin(rPosTheta[2]); + double cz = cos(rPosTheta[2]); + + alignxf[0] = cy*cz; + alignxf[1] = sx*sy*cz + cx*sz; + alignxf[2] = -cx*sy*cz + sx*sz; + alignxf[3] = 0.0; + alignxf[4] = -cy*sz; + alignxf[5] = -sx*sy*sz + cx*cz; + alignxf[6] = cx*sy*sz + sx*cz; + alignxf[7] = 0.0; + alignxf[8] = sy; + alignxf[9] = -sx*cy; + alignxf[10] = cx*cy; + + alignxf[11] = 0.0; + + alignxf[12] = rPos[0]; + alignxf[13] = rPos[1]; + alignxf[14] = rPos[2]; + alignxf[15] = 1; +} + +/** + * Converts a 4x4 matrix to Euler angles. + * + * @param alignxf input 4x4 matrix + * @param rPosTheta output 3-vector of Euler angles + * @param rPos output vector of trnaslation (position) if set + * + */ +static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, double *rPos = 0) +{ + + double _trX, _trY; + + if(alignxf[0] > 0.0) { + rPosTheta[1] = asin(alignxf[8]); + } else { + rPosTheta[1] = M_PI - asin(alignxf[8]); + } + // rPosTheta[1] = asin( alignxf[8]); // Calculate Y-axis angle + + double C = cos( rPosTheta[1] ); + if ( fabs( C ) > 0.005 ) { // Gimball lock? + _trX = alignxf[10] / C; // No, so get X-axis angle + _trY = -alignxf[9] / C; + rPosTheta[0] = atan2( _trY, _trX ); + _trX = alignxf[0] / C; // Get Z-axis angle + _trY = -alignxf[4] / C; + rPosTheta[2] = atan2( _trY, _trX ); + } else { // Gimball lock has occurred + rPosTheta[0] = 0.0; // Set X-axis angle to zero + _trX = alignxf[5]; //1 // And calculate Z-axis angle + _trY = alignxf[1]; //2 + rPosTheta[2] = atan2( _trY, _trX ); + } + + rPosTheta[0] = rPosTheta[0]; + rPosTheta[1] = rPosTheta[1]; + rPosTheta[2] = rPosTheta[2]; + + if (rPos != 0) { + rPos[0] = alignxf[12]; + rPos[1] = alignxf[13]; + rPos[2] = alignxf[14]; + } +} + + +/** + * Sets a 3x3 matrix to the identity matrix + * + * @param M input 3x3 matrix + */ +template +static inline void M3identity( T *M ) +{ + M[0] = M[4] = M[8] = 1.0; + M[1] = M[2] = M[3] = M[5] = M[6] = M[7] = 0.0; +} + +/** + * Gets the current time (in ms) + * + * @return current time (in ms) + */ +static inline unsigned long GetCurrentTimeInMilliSec() +{ + static unsigned long milliseconds; +#ifdef _MSC_VER + SYSTEMTIME stime; + GetSystemTime(&stime); + milliseconds = ((stime.wHour * 60 + stime.wMinute) * 60 + stime.wSecond) * 1000 + stime.wMilliseconds; +#else + static struct timeval tv; + gettimeofday(&tv, NULL); + milliseconds = tv.tv_sec * 1000 + tv.tv_usec / 1000; +#endif + return milliseconds; +} + +/** + * generates random numbers in [0..rnd] + * + * @param rnd maximum number + * @return random number between 0 and rnd + */ +inline int rand(int rnd) +{ + return (int) ((double)rnd * (double)std::rand() / (RAND_MAX + 1.0)); +} + +/** + * generates unsigned character random numbers in [0..rnd] + * + * @param rnd maximum number + * @return random number between 0 and rnd + */ +inline unsigned char randUC(unsigned char rnd) +{ + return (unsigned char) ((float)rnd * std::rand() / (RAND_MAX + 1.0)); +} + +/** + * Computes the angle between 2 points in polar coordinates + */ +inline double polardist(double* p, double *p2) { + double stheta = sin(p[0]) * sin(p2[0]); + double myd2 = acos( stheta * cos(p[1]) * cos(p2[1]) + + stheta * sin(p[1]) * sin(p2[1]) + + cos(p[0]) * cos(p2[0])); + return myd2; +} + + + +inline void toKartesian(double *polar, double *kart) { + kart[0] = polar[2] * cos( polar[1] ) * sin( polar[0] ); + kart[1] = polar[2] * sin( polar[1] ) * sin( polar[0] ); + kart[2] = polar[2] * cos( polar[0] ); + +} + +/** + * Transforms a point in cartesian coordinates into polar + * coordinates + */ + +inline void toPolar(double *n, double *polar) { + double phi, theta, rho; + rho = Len(n); + Normalize3(n); + + // if(fabs(1 - fabs(n[1])) < 0.001) { + // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl; + + phi = acos(n[2]); + //if ( fabs(phi) < 0.0001) phi = 0.0001; + //if ( fabs(M_PI - phi) < 0.0001) phi = 0.0001; + + double theta0; + + if(fabs(phi) < 0.0001) { + theta = 0.0; + } else if(fabs(M_PI - phi) < 0.0001) { + theta = 0.0; + } else { + if(fabs(n[0]/sin(phi)) > 1.0) { + if(n[0]/sin(phi) < 0) { + theta0 = M_PI; + } else { + theta0 = 0.0; + } + } else { + theta0 = acos(n[0]/sin(phi)); + + } + + + double sintheta = n[1]/sin(phi); + + double EPS = 0.0001; + + if(fabs(sin(theta0) - sintheta) < EPS) { + theta = theta0; + } else if(fabs( sin( 2*M_PI - theta0 ) - sintheta ) < EPS) { + theta = 2*M_PI - theta0; + } else { + theta = 0; + cout << "Fehler" << endl; + } + + } + /* } else { + theta = 0.0; + phi = 0.0; + }*/ + polar[0] = phi; + polar[1] = theta; + polar[2] = rho; + +} + +/* + * Computes the submatrix without + * row i and column j + * + * @param Min input 4x4 matrix + * @param Mout output 3x3 matrix + * @param i row index i + * @param j column index j + */ +template +static inline void M4_submat(const T *Min, T *Mout, int i, int j ) { + int di, dj, si, sj; + // loop through 3x3 submatrix + for( di = 0; di < 3; di ++ ) { + for( dj = 0; dj < 3; dj ++ ) { + // map 3x3 element (destination) to 4x4 element (source) + si = di + ( ( di >= i ) ? 1 : 0 ); + sj = dj + ( ( dj >= j ) ? 1 : 0 ); + // copy element + Mout[di * 3 + dj] = Min[si * 4 + sj]; + } + } +} + +/* + * Computes the determinant of a 4x4 matrix + * + * @param 4x4 matrix + * @return determinant + */ +template +static inline double M4det(const T *M ) +{ + T det, result = 0, i = 1.0; + T Msub3[9]; + int n; + for ( n = 0; n < 4; n++, i *= -1.0 ) { + M4_submat( M, Msub3, 0, n ); + det = M3det( Msub3 ); + result += M[n] * det * i; + } + return( result ); +} + + +/* + * invert a 4x4 Matrix + * + * @param Min input 4x4 matrix + * @param Mout output matrix + * @return 1 if successful + */ +template +static inline int M4inv(const T *Min, T *Mout ) +{ + T mdet = M4det( Min ); + if ( fabs( mdet ) < 0.0005 ) { + cout << "Error matrix inverting!" << endl; + M4identity( Mout ); + return( 0 ); + } + T mtemp[9]; + int i, j, sign; + for ( i = 0; i < 4; i++ ) { + for ( j = 0; j < 4; j++ ) { + sign = 1 - ( (i +j) % 2 ) * 2; + M4_submat( Min, mtemp, i, j ); + Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet; + } + } + return( 1 ); +} + +/* + * transposes a 4x4 matrix + * + * @param Min input 4x4 matrix + * @param Mout output 4x4 matrix + */ +template +static inline int M4transpose(const T *Min, T *Mout ) +{ + Mout[0] = Min[0]; + Mout[4] = Min[1]; + Mout[8] = Min[2]; + Mout[12] = Min[3]; + Mout[1] = Min[4]; + Mout[5] = Min[5]; + Mout[9] = Min[6]; + Mout[13] = Min[7]; + Mout[2] = Min[8]; + Mout[6] = Min[9]; + Mout[10] = Min[10]; + Mout[14] = Min[11]; + Mout[3] = Min[12]; + Mout[7] = Min[13]; + Mout[11] = Min[14]; + Mout[15] = Min[15]; + return( 1 ); +} + +/* +++++++++-------------++++++++++++ + * NAME + * choldc + * DESCRIPTION + * Cholesky Decomposition of a symmetric + * positive definite matrix + * Overwrites lower triangle of matrix + * Numerical Recipes, but has a bit of + * the fancy C++ template thing happening + * +++++++++-------------++++++++++++ */ +static inline bool choldc(double A[3][3], double diag[3]) +{ + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = i; j < 3; j++) { + double sum = A[i][j]; + for (int k=i-1; k >= 0; k--) + sum -= A[i][k] * A[j][k]; + if (i == j) { + if (sum < 1.0e-7) + return false; + diag[i] = sqrt(sum); + } else { + A[j][i] = sum / diag[i]; + } + } + } + return true; +} + + +/* +++++++++-------------++++++++++++ + * NAME + * choldc + * DESCRIPTION + * Cholesky Decomposition of a symmetric + * positive definite matrix + * Overwrites lower triangle of matrix + * Numerical Recipes, but has a bit of + * the fancy C++ template thing happening + * +++++++++-------------++++++++++++ */ +static inline bool choldc(unsigned int n, double **A, double *diag) +{ + for (unsigned int i = 0; i < n; i++) { + for (unsigned int j = i; j < n; j++) { + double sum = A[i][j]; + for (int k=i-1; k >= 0; k--) + sum -= A[i][k] * A[j][k]; + if (i == j) { + if (sum < 1.0e-7) + return false; + diag[i] = sqrt(sum); + } else { + A[j][i] = sum / diag[i]; + } + } + } + return true; +} + + +/* +++++++++-------------++++++++++++ + * NAME + * cholsl + * DESCRIPTION + * Solve Ax=B after choldc + * +++++++++-------------++++++++++++ */ +static inline void cholsl(double A[3][3], + double diag[3], + double B[3], + double x[3]) +{ + for (int i=0; i < 3; i++) { + double sum = B[i]; + for (int k=i-1; k >= 0; k--) + sum -= A[i][k] * x[k]; + x[i] = sum / diag[i]; + } + for (int i=2; i >= 0; i--) { + double sum = x[i]; + for (int k=i+1; k < 3; k++) + sum -= A[k][i] * x[k]; + x[i] = sum / diag[i]; + } +} + + +/* +++++++++-------------++++++++++++ + * NAME + * cholsl + * DESCRIPTION + * Solve Ax=B after choldc + * +++++++++-------------++++++++++++ */ +static inline void cholsl(unsigned int n, + double **A, + double *diag, + double *B, + double *x) +{ + for (unsigned int i=0; i < n; i++) { + double sum = B[i]; + for (int k=(int)i-1; k >= 0; k--) + sum -= A[i][k] * x[k]; + x[i] = sum / diag[i]; + } + for (int i=(int)n-1; i >= 0; i--) { + double sum = x[i]; + for (unsigned int k=i+1; k < n; k++) + sum -= A[k][i] * x[k]; + x[i] = sum / diag[i]; + } +} + + +/** + * Transforms a a quaternion and a translation vector into a 4x4 + * Matrix + * + * @param quat input quaternion + * @param t input translation + * @param mat output matrix + */ +static inline void QuatToMatrix4(const double *quat, const double *t, double *mat) +{ + // double q00 = quat[0]*quat[0]; + double q11 = quat[1]*quat[1]; + double q22 = quat[2]*quat[2]; + double q33 = quat[3]*quat[3]; + double q03 = quat[0]*quat[3]; + double q13 = quat[1]*quat[3]; + double q23 = quat[2]*quat[3]; + double q02 = quat[0]*quat[2]; + double q12 = quat[1]*quat[2]; + double q01 = quat[0]*quat[1]; + mat[0] = 1 - 2 * (q22 + q33); + mat[5] = 1 - 2 * (q11 + q33); + mat[10] = 1 - 2 * (q11 + q22); + mat[4] = 2.0*(q12-q03); + mat[1] = 2.0*(q12+q03); + mat[8] = 2.0*(q13+q02); + mat[2] = 2.0*(q13-q02); + mat[9] = 2.0*(q23-q01); + mat[6] = 2.0*(q23+q01); + + mat[3] = mat[7] = mat[11] = 0.0; + + if (t == 0) { + mat[12] = mat[13] = mat[14] = 0.0; + } else { + mat[12] = t[0]; + mat[13] = t[1]; + mat[14] = t[2]; + } + mat[15] = 1.0; +} + + +/** + * Transforms a 4x4 Transformation Matrix into a quaternion + * + * @param mat matrix to be converted + * @param quat resulting quaternion + * @param t resulting translation + */ +static inline void Matrix4ToQuat(const double *mat, double *quat, double *t = 0) +{ + + double T, S, X, Y, Z, W; + T = 1 + mat[0] + mat[5] + mat[10]; + if ( T > 0.00000001 ) { // to avoid large distortions! + + S = sqrt(T) * 2; + X = ( mat[9] - mat[6] ) / S; + Y = ( mat[2] - mat[8] ) / S; + Z = ( mat[4] - mat[1] ) / S; + W = 0.25 * S; + } else if ( mat[0] > mat[5] && mat[0] > mat[10] ) { // Column 0: + S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2; + X = 0.25 * S; + Y = (mat[4] + mat[1] ) / S; + Z = (mat[2] + mat[8] ) / S; + W = (mat[9] - mat[6] ) / S; + } else if ( mat[5] > mat[10] ) { // Column 1: + S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2; + X = (mat[4] + mat[1] ) / S; + Y = 0.25 * S; + Z = (mat[9] + mat[6] ) / S; + W = (mat[2] - mat[8] ) / S; + } else { // Column 2: + S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2; + X = (mat[2] + mat[8] ) / S; + Y = (mat[9] + mat[6] ) / S; + Z = 0.25 * S; + W = (mat[4] - mat[1] ) / S; + } + quat[0] = W; + quat[1] = -X; + quat[2] = -Y; + quat[3] = -Z; + + Normalize4(quat); + if (t != 0) { + t[0] = mat[12]; + t[1] = mat[13]; + t[2] = mat[14]; + } +} + +/** + * Transforms a Quaternion to the corresponding Axis-Angle representation + * + * @param quat 4-vector of quaternion + * gets overridden by the axis/angle representation + */ +static inline void QuatToAA(double *quat){ + //double x, y, z, w; + double sum = 0.0; + + double cos_a, angle, x, y, z, sin_a; + + for(int i = 0; i < 4; i++){ + sum += quat[i]*quat[i]; + } + sum = sqrt(sum); + + //quaternion_normalise( |W,X,Y,Z| ); + cos_a = quat[0]/sum; + angle = acos( cos_a ) * 2; + sin_a = sqrt( 1.0 - cos_a * cos_a ); + if ( fabs( sin_a ) < 0.0005 ) sin_a = 1; + x = quat[1] / sin_a; + y = quat[2] / sin_a; + z = quat[3] / sin_a; + + quat[0] = angle; + quat[1] = x; + quat[2] = y; + quat[3] = z; +} + +/** + * Quaternion Multiplication q1 * q2 = q3 + */ +static inline void QMult(const double *q1, const double *q2, double *q3) { + q3[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + q3[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + q3[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + q3[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; +} + +/** + * Quaternion SLERP + * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ + */ +static inline void slerp(const double *qa, const double *qb, const double t, double *qm) { + // Calculate angle between them. + double cosHalfTheta = qa[0] * qb[0] + qa[1] * qb[1] + qa[2] * qb[2] + qa[3] * qb[3]; + // if qa=qb or qa=-qb then theta = 0 and we can return qa + if (fabs(cosHalfTheta) >= 1.0) { + qm[0] = qa[0]; + qm[1] = qa[1]; + qm[2] = qa[2]; + qm[3] = qa[3]; + return; + } + // Calculate temporary values. + double halfTheta = acos(cosHalfTheta); + double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + if (fabs(sinHalfTheta) < 0.001){ + qm[0] = (qa[0] * 0.5 + qb[0] * 0.5); + qm[1] = (qa[1] * 0.5 + qb[1] * 0.5); + qm[2] = (qa[2] * 0.5 + qb[2] * 0.5); + qm[3] = (qa[3] * 0.5 + qb[3] * 0.5); + Normalize4(qm); + return; + } + double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; + double ratioB = sin(t * halfTheta) / sinHalfTheta; + //calculate Quaternion. + qm[0] = (qa[0] * ratioA + qb[0] * ratioB); + qm[1] = (qa[1] * ratioA + qb[1] * ratioB); + qm[2] = (qa[2] * ratioA + qb[2] * ratioB); + qm[3] = (qa[3] * ratioA + qb[3] * ratioB); + Normalize4(qm); +} + +/* taken from ROOT (CERN) + * as well in: + * Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning + * James J. Kuffner + * + * Distance between two rotations in Quaternion form + * Note: The rotation group is isomorphic to a 3-sphere + * with diametrically opposite points identified. + * The (rotation group-invariant) is the smaller + * of the two possible angles between the images of + * the two rotations on that sphere. Thus the distance + * is never greater than pi/2. + */ +inline double quat_dist(double quat1[4], double quat2[4]) { + double chordLength = std::fabs(quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3]); + if (chordLength > 1) chordLength = 1; // in case roundoff fouls us up + return acos(chordLength) / M_PI * 180.0; +} + + + +/** + * Converts a Rotation given by Axis-Angle and a Translation into a + * 4x4 Transformation matrix + * + * @param aa axis and angle aa[0] is the angle + * @param trans vector containing the translation + * @param matrix matrix to be computed + */ +inline void AAToMatrix(double *aa, double *trans, double *matrix ){ + double rcos = cos(aa[0]); + double rsin = sin(aa[0]); + double u = aa[1]; + double v = aa[2]; + double w = aa[3]; + + matrix[0] = rcos + u*u*(1-rcos); + matrix[1] = w * rsin + v*u*(1-rcos); + matrix[2] = -v * rsin + w*u*(1-rcos); + matrix[3] = 0.0; + matrix[4] = -w * rsin + u*v*(1-rcos); + matrix[5] = rcos + v*v*(1-rcos); + matrix[6] = u * rsin + w*v*(1-rcos); + matrix[7] = 0.0; + matrix[8] = v * rsin + u*w*(1-rcos); + matrix[9] = -u * rsin + v*w*(1-rcos); + matrix[10] = rcos + w*w*(1-rcos); + matrix[11] = 0.0; + matrix[12] = trans[0]; + matrix[13] = trans[1]; + matrix[14] = trans[2]; + matrix[15] = 1.0; + +} + + +/** + * Factors matrix A into lower and upper triangular matrices + * (L and U respectively) in solving the linear equation Ax=b. + * + * @param A (input/output) Matrix(1:n, 1:n) In input, matrix to be + * factored. On output, overwritten with lower and + * upper triangular factors. + * + * @param indx (output) Vector(1:n) Pivot vector. Describes how + * the rows of A were reordered to increase + * numerical stability. + * + * @return return int(0 if successful, 1 otherwise) + */ +inline int LU_factor( double A[4][4], int indx[4]) +{ + int M = 4; + int N = 4; + + int i=0,j=0,k=0; + int jp=0; + + double t; + + int minMN = 4; + + for (j = 0; j < minMN; j++) + { + + // find pivot in column j and test for singularity. + + jp = j; + t = fabs(A[j][j]); + for (i = j+1; i < M; i++) + if ( fabs(A[i][j]) > t) + { + jp = i; + t = fabs(A[i][j]); + } + + indx[j] = jp; + + // jp now has the index of maximum element + // of column j, below the diagonal + + if ( A[jp][j] == 0 ) + return 1; // factorization failed because of zero pivot + + + if (jp != j) // swap rows j and jp + for (k = 0; k < N; k++) + { + t = A[j][k]; + A[j][k] = A[jp][k]; + A[jp][k] =t; + } + + if (j < M) // compute elements j+1:M of jth column + { + // note A(j,j), was A(jp,p) previously which was + // guarranteed not to be zero (Label #1) + // + double recp = 1.0 / A[j][j]; + + for (k = j+1; k < M; k++) + A[k][j] *= recp; + } + + if (j < minMN) + { + // rank-1 update to trailing submatrix: E = E - x*y; + // + // E is the region A(j+1:M, j+1:N) + // x is the column vector A(j+1:M,j) + // y is row vector A(j,j+1:N) + + int ii,jj; + + for (ii = j+1; ii < M; ii++) + for (jj = j+1; jj < N; jj++) + A[ii][jj] -= A[ii][j]*A[j][jj]; + } + } + + return 0; +} + +/** + * Solves a linear system via LU after LU factor + * + * @param A 4x4 matrix + * @param indx indices + * @param b 4 vectort + * + * @return 0 + * + */ +inline int LU_solve(const double A[4][4], const int indx[4], double b[4]) +{ + int i,ii=0,ip,j; + int n = 4; + double sum = 0.0; + + for (i = 0; i < n; i++) + { + ip=indx[i]; + sum=b[ip]; + b[ip]=b[i]; + if (ii) + for (j = ii;j <= i-1; j++) + sum -= A[i][j]*b[j]; + else if (sum) ii=i; + b[i]=sum; + } + for (i = n-1; i >= 0; i--) + { + sum=b[i]; + for (j = i+1; j < n; j++) + sum -= A[i][j]*b[j]; + b[i]=sum/A[i][i]; + } + + return 0; +} + +/** + * Calculates the cross product of two 4-vectors + * + * @param x input 1 + * @param y input 2 + * @param T output + * + */ +template +static inline void Cross(const T *x, const T *y, T *result) +{ + result[0] = x[1] * y[2] - x[2] * y[1]; + result[1] = x[2] * y[0] - x[0] * y[2]; + result[2] = x[0] * y[1] - x[1] * y[0]; + return; +} + +/** + * converts a quaternion to Euler angels in the roll pitch yaw system + */ +static inline void QuatRPYEuler(const double *quat, double *euler) +{ + double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]); + double s = n > 0?2./(n*n):0.; + + double m00, m10, m20, m21, m22; + + + double xs = quat[1]*s; + double ys = quat[2]*s; + double zs = quat[3]*s; + + double wx = quat[0]*xs; + double wy = quat[0]*ys; + double wz = quat[0]*zs; + + double xx = quat[1]*xs; + double xy = quat[1]*ys; + double xz = quat[1]*zs; + + double yy = quat[2]*ys; + double yz = quat[2]*zs; + + double zz = quat[3]*zs; + + m00 = 1.0 - (yy + zz); + m22 = 1.0 - (xx + yy); + + + m10 = xy + wz; + + m20 = xz - wy; + m21 = yz + wx; + + euler[0] = atan2(m21,m22); + euler[1] = atan2(-m20,sqrt(m21*m21 + m22*m22)); + euler[2] = atan2(m10,m00); +} + +/** + * converts from Euler angels in the roll pitch yaw system to a quaternion + */ +static inline void RPYEulerQuat(const double *euler, double *quat) +{ + double sphi = sin(euler[0]); + double stheta = sin(euler[1]); + double spsi = sin(euler[2]); + double cphi = cos(euler[0]); + double ctheta = cos(euler[1]); + double cpsi = cos(euler[2]); + + double _r[3][3] = { //create rotational Matrix + {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi}, + {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi}, + { -stheta, ctheta*sphi, ctheta*cphi} + }; + +#define MY_MAX(a,b) (((a)>(b))?(a):(b)) + double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0; + double _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0; + double _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0; + double _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0; + quat[0] = _w; + quat[1] = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x); + quat[2] = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y); + quat[3] = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z); +} + + +inline void transform3(const double *alignxf, double *point) +{ + double x_neu, y_neu, z_neu; + x_neu = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8]; + y_neu = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9]; + z_neu = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10]; + point[0] = x_neu + alignxf[12]; + point[1] = y_neu + alignxf[13]; + point[2] = z_neu + alignxf[14]; +} + +inline void transform3(const double *alignxf, const double *point, double *tpoint) +{ + tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12]; + tpoint[1] = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9] + alignxf[13]; + tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14]; +} + +inline std::string trim(const std::string& source) +{ + unsigned int start = 0, end = source.size() - 1; + while(source[start] == ' ') start++; + while(source[end] == ' ') end--; + return source.substr(start, end - start + 1); +} + +#endif diff --git a/.svn/pristine/a7/a7bec2140b91dce3babcd38795dea82c82ec1485.svn-base b/.svn/pristine/a7/a7bec2140b91dce3babcd38795dea82c82ec1485.svn-base new file mode 100644 index 0000000..98595db --- /dev/null +++ b/.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 +#include + + +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 &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 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> m_data; + + std::vector 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 diff --git a/.svn/pristine/ae/aec7ea9b471ff8f28d6520bc427532b9b0643e02.svn-base b/.svn/pristine/ae/aec7ea9b471ff8f28d6520bc427532b9b0643e02.svn-base new file mode 100644 index 0000000..ef7086f --- /dev/null +++ b/.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 +#include +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#include +using std::ofstream; +using std::flush; +using std::cout; +using std::string; +using std::cerr; +using std::endl; +#include + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#include +#include +#else +#include +#include +#include +#include +#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=)" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << 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 &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 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 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: cannot be smaller than .\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::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 points; + CollisionPlane * plane; + plane = new LightBulbPlane(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 *)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 +*/ +} + diff --git a/.svn/pristine/ae/aeea7442cea67a8152af68ec023a043e48af2f16.svn-base b/.svn/pristine/ae/aeea7442cea67a8152af68ec023a043e48af2f16.svn-base new file mode 100644 index 0000000..95a8b00 --- /dev/null +++ b/.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 +#include +#include + +#ifdef __APPLE__ +#include +#include +#else +#include +#include +#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 */ +} diff --git a/.svn/pristine/b0/b056f289f2ded3cf1ca7db3f73dd6418bffe3a19.svn-base b/.svn/pristine/b0/b056f289f2ded3cf1ca7db3f73dd6418bffe3a19.svn-base new file mode 100644 index 0000000..54a7a53 --- /dev/null +++ b/.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 +#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 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 diff --git a/.svn/pristine/c4/c460c133ba1883f7187e9dde77510e05e4b94939.svn-base b/.svn/pristine/c4/c460c133ba1883f7187e9dde77510e05e4b94939.svn-base new file mode 100644 index 0000000..2c9c5f4 --- /dev/null +++ b/.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&_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 + + diff --git a/.svn/pristine/c9/c969cb15b1ff8c8771f040288107b7b1b0d8eabf.svn-base b/.svn/pristine/c9/c969cb15b1ff8c8771f040288107b7b1b0d8eabf.svn-base new file mode 100644 index 0000000..12e2406 --- /dev/null +++ b/.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 +#include + +#include "slam6d/basicScan.h" +#include "veloslam/gridcell.h" +#include "veloslam/gridcluster.h" + +bool FilterNOMovingObjcets(clusterFeature &glu, cluster &gluData); + +class Trajectory +{ + public: + Trajectory(); + public: + vector 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 diff --git a/.svn/pristine/ce/ceebc6cd1fa5bafc9044862d682daa76900564d0.svn-base b/.svn/pristine/ce/ceebc6cd1fa5bafc9044862d682daa76900564d0.svn-base new file mode 100644 index 0000000..3545c97 --- /dev/null +++ b/.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 +#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 { +public: + KDtree(double **pts, int n); + + virtual ~KDtree(); + + virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const; +}; + +#endif diff --git a/.svn/pristine/d3/d363c05176b5c741c8e0b7bced15231a1aae80af.svn-base b/.svn/pristine/d3/d363c05176b5c741c8e0b7bced15231a1aae80af.svn-base new file mode 100644 index 0000000..5150144 --- /dev/null +++ b/.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 +#include +#include +using std::ifstream; +using std::ostrstream; +using std::cerr; +using std::endl; +using std::ends; + +#include +using std::swap; + +#include +#include +#include +#include +#include + +using namespace std; + +#ifdef _MSC_VER +#include +#endif +#include +#include +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"<(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."<* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* amplitude, + std::vector* type, + std::vector* 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 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 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 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* xyz, + std::vector* rgb, + std::vector* reflectance, + std::vector* temperature, + std::vector* amplitude, + std::vector* type, + std::vector* 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<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< +using std::string; + +#include +using std::cout; +using std::endl; + +#include + +#include +namespace po = boost::program_options; + +#include "slam6d/fbr/panorama.h" + +#include +#include + +#include +#include + +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& 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& 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& 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() == 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() == 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(&start)->default_value(0), + "start at scan (i.e., neglects the first scans) " + "[ATTENTION: counting naturally starts with 0]") + ("end,e", po::value(&end)->default_value(-1), + "end after scan ") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library 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(&maxDist)->default_value(-1), + "neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&minDist)->default_value(-1), + "neglegt all data points with a distance smaller than 'units") + ("scanserver,S", po::value(&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(&width)->default_value(1280), + "width of panorama") + ("height,h", po::value(&height)->default_value(960), + "height of panorama") + ("panorama,p", po::value(&ptype)-> + default_value(fbr::EQUIRECTANGULAR), "panorama type (EQUIRECTANGULAR, " + "CYLINDRICAL, MERCATOR, RECTILINEAR, PANNINI, STEREOGRAPHIC, ZAXIS, " + "CONIC)") + ("num-images,N", po::value(&nImages)->default_value(1), + "number of images used for some projections") + ("proj-param,P", po::value(&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(&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(&stype)-> + default_value(PYR_MEAN_SHIFT), "segmentation method (THRESHOLD, " + "ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, PYR_SEGMENTATION, WATERSHED)") + ("marker,K", po::value(&marker), + "marker mask for watershed segmentation") + ("thresh,T", po::value(&thresh), + "threshold for threshold segmentation") + ("maxlevel,X", po::value(&maxlevel), + "maximum level for meanshift segmentation") + ("radius,R", po::value(&radius), + "radius for meanshift segmentation") + ("links,l", po::value(&pyrlinks), + "error threshold for establishing the links for pyramid segmentation") + ("clustering,c", po::value(&pyrcluster), + "error threshold for the segments clustering for pyramid " + "segmentation") + ("levels,E", po::value(&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(&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_ it; + it = scan.begin(); + 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_ it = source.begin(); + it != source.end(); ++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 values are larger than it + vector sorted(source.cols*source.rows); + int i = 0; + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++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_ src = source.begin(); + cv::MatIterator_ dst = result.begin(); + cv::MatIterator_ end = source.end(); + 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> &segmented_points, + cv::Mat &img, vector > > 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(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> &segmented_points, + cv::Mat &img, vector > > 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> histogram(256); + + for (i = 0; i < res.rows; i++) { + for (j = 0; j < res.cols; j++) { + idx = res.at(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> &segmented_points, + cv::Mat &img, vector > > 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<height &= -(1< 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(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> &segmented_points, + cv::Mat &img, vector > > 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(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> &segmented_points, + string &marker, cv::Mat &img, vector > > extendedMap) +{ + int i, j, idx; + cv::Mat markerMask = cv::imread(marker, 0); + vector > contours; + vector 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(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> &segmented_points, string &segdir) +{ + unsigned int i; + + vector 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::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> 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()); + } +} diff --git a/.svn/pristine/f1/f127b01bfac80b6078ad9ad6d09ff516c2479da6.svn-base b/.svn/pristine/f1/f127b01bfac80b6078ad9ad6d09ff516c2479da6.svn-base new file mode 100644 index 0000000..7d489c5 --- /dev/null +++ b/.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 +#include + +#include +#include +#include +#include + +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 edges; + std::vector& points; + int V; + int E; + + int nr_neighbors; + float radius; + + + struct he{ int x; float w; }; + std::vector< std::list > adjency_list; +}; + +#endif diff --git a/.svn/pristine/f3/f3e9f1b6c337fbc37530de57024eb510574a0af0.svn-base b/.svn/pristine/f3/f3e9f1b6c337fbc37530de57024eb510574a0af0.svn-base new file mode 100644 index 0000000..991aed4 --- /dev/null +++ b/.svn/pristine/f3/f3e9f1b6c337fbc37530de57024eb510574a0af0.svn-base @@ -0,0 +1,1250 @@ +/* + * veloslam implementation + * + * Copyright (C) Andreas Nuechter, Li Wei, Li Ming + * + * Released under the GPL version 3. + * + */ + +/** + * @file + * @brief Main programm for dynamic Velodyne SLAM + * + * @author Andreas Nuechter. Jacobs University Bremen, Germany + * @author Li Wei, Wuhan University, China + * @author Li Ming, Wuhan University, China + */ + +#ifdef _MSC_VER +#ifdef OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#ifdef _MSC_VER +#include +#else +#include +#endif + +#include "slam6d/icp6Dapx.h" +#include "slam6d/icp6Dsvd.h" +#include "slam6d/icp6Dquat.h" +#include "slam6d/icp6Dortho.h" +#include "slam6d/icp6Dhelix.h" +#include "slam6d/icp6Ddual.h" +#include "slam6d/icp6Dlumeuler.h" +#include "slam6d/icp6Dlumquat.h" +#include "slam6d/icp6Dquatscale.h" +#include "slam6d/icp6D.h" +#ifdef WITH_CUDA +#include "slam6d/cuda/icp6Dcuda.h" +#endif +#include "slam6d/lum6Deuler.h" +#include "slam6d/lum6Dquat.h" +#include "slam6d/ghelix6DQ2.h" +#include "slam6d/graphToro.h" +#include "slam6d/graphHOG-Man.h" +#include "slam6d/elch6Deuler.h" +#include "slam6d/elch6Dquat.h" +#include "slam6d/elch6DunitQuat.h" +#include "slam6d/elch6Dslerp.h" +#include "slam6d/loopToro.h" +#include "slam6d/loopHOG-Man.h" +#include "slam6d/graphSlam6D.h" +#include "slam6d/gapx6D.h" +#include "slam6d/graph.h" +#include "slam6d/globals.icc" + +#ifndef _MSC_VER +#include +#else +#include "XGetopt.h" +#endif + +#include +#include "veloslam/veloscan.h" +#include "veloslam/debugview.h" +#include "veloslam/tracker.h" +#include "veloslam/trackermanager.h" +#include "veloslam/intersection_detection.h" + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif //WITH_METRICS + +#include /* OpenGL header file */ +#include /* OpenGL utilities header file */ + +#ifdef _MSC_VER +#include +#else +#include +#endif + +#ifndef _MSC_VER +#include +void Sleep(unsigned int mseconds) +{ + clock_t goal = mseconds + clock(); + while (goal > clock()); +} + +#endif + +#define WANT_STREAM ///< define the WANT stream :) + +#include +using std::string; +#include +using std::cout; +using std::cerr; +using std::endl; +#include +using std::ifstream; + +#include +#include +#include + +using namespace boost; +extern boost::mutex keymutex; +extern boost::condition keycond; + + +extern void StartShow(); +extern TrackerManager trackMgr; +extern int sliding_window_size; +extern int current_sliding_window_pos; + +extern Trajectory VelodyneTrajectory; +extern VeloScan* g_pfirstScan; +extern bool g_pause; +extern float constant_static_or_moving; +extern bool DebugDrawFinished ; +extern bool ICPFinished; +extern bool save_animation; + +extern int anim_frame_rate; +extern int scanCount; + +// Handling Segmentation faults and CTRL-C +void sigSEGVhandler (int v) +{ + static bool segfault = false; + if(!segfault) { + segfault = true; + cout << endl + << "# **************************** #" << endl + << " Segmentation fault or Ctrl-C" << endl + << "# **************************** #" << endl + << endl; + + // save frames and close scans + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + (*it)->saveFrames(); + } + cout << "Frames saved." << endl; + Scan::closeDirectory(); + } + exit(-1); +} + + +/** + * Explains the usage of this program's command line parameters + */ +void usage(char* prog) +{ +#ifndef _MSC_VER + const string bold("\033[1m"); + const string normal("\033[m"); +#else + const string bold(""); + const string normal(""); +#endif + cout << endl + << bold << "USAGE " << normal << endl + << " " << prog << " [options] directory" << endl << endl; + cout << bold << "OPTIONS" << normal << endl + + << bold << " -a" << normal << " NR, " << bold << "--algo=" << normal << "NR [default: 1]" << endl + << " selects the minimizazion method for the ICP matching algorithm" << endl + << " 1 = unit quaternion based method by Horn" << endl + << " 2 = singular value decomposition by Arun et al. " << endl + << " 3 = orthonormal matrices by Horn et al." << endl + << " 4 = dual quaternion method by Walker et al." << endl + << " 5 = helix approximation by Hofer & Potmann" << endl + << " 6 = small angle approximation" << endl + << " 7 = Lu & Milios style, i.e., uncertainty based, with Euler angles" << endl + << " 8 = Lu & Milios style, i.e., uncertainty based, with Quaternion" << endl + << " 9 = unit quaternion with scale method by Horn" << endl + << endl + << bold << " -A" << normal << " NR, " << bold << "--anim=" << normal << "NR [default: first and last frame only]" << endl + << " if specified, use only every NR-th frame for animation" << endl + << endl + << bold << " -c" << normal << " NR, " << bold << "--cldist=" << normal << "NR [default: 500]" << endl + << " specifies the maximal distance for closed loops" << endl + << endl + << bold << " -C" << normal << " NR, " << bold << "--clpairs=" << normal << "NR [default: 6]" << endl + << " specifies the minimal number of points for an overlap. If not specified" << endl + << " cldist is used instead" << endl + << endl + << bold << " --cache" << normal << endl + << " turns on cached k-d tree search" << endl + << endl + << bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR [default: 25]" << endl + << " sets the maximal point-to-point distance for matching with ICP to 'units'" << endl + << " (unit of scan data, e.g. cm)" << endl + << endl + << bold << " -D" << normal << " NR, " << bold << "--distSLAM=" + << normal << "NR [default: same value as -d option]" << endl + << " sets the maximal point-to-point distance for matching with SLAM to 'units'" << endl + << " (unit of scan data, e.g. cm)" << endl + << endl + << bold << " --DlastSLAM" << normal << " NR [default not set]" << endl + << " sets the maximal point-to-point distance for the final SLAM correction," << endl + << " if final SLAM is not required don't set it." << endl + << endl + << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl + << " end after scan NR" << endl + << endl + << bold << " --exportAllPoints" << normal << endl + << " writes all registered reduced points to the file points.pts before" << endl + << " slam6D terminated" << endl + << endl + << bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl + << " stop ICP iteration if difference is smaller than NR" << endl + << endl + << bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl + << " stop SLAM iteration if average difference is smaller than 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, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl + << " selects the minimizazion method for the SLAM matching algorithm" << endl + << " 0 = no global relaxation technique" << endl + << " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl + << " 2 = Lu & Milios extension using using unit quaternions" << endl + << " 3 = HELIX approximation by Hofer and Pottmann" << endl + << " 4 = small angle approximation" << endl + << " 5 = TORO" << endl + << " 6 = HOG-Man" << endl + << endl + << bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl + << " sets the maximal number of ICP iterations to " << endl + << endl + << bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl + << " sets the maximal number of iterations for SLAM to " << endl + << " (if not set, graphSLAM is not executed)" << endl + << endl + << bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl + << " sets the size of a loop, i.e., a loop must exceed of scans" << endl + << endl + << bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl + << " selects the method for closing the loop explicitly" << endl + << " 0 = no loop closing technique" << endl + << " 1 = euler angles" << endl + << " 2 = quaternions " << endl + << " 3 = unit quaternions" << endl + << " 4 = SLERP (recommended)" << endl + << " 5 = TORO" << endl + << " 6 = HOG-Man" << endl + << endl + << bold << " --metascan" << normal << endl + << " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << 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 << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl + << " specifies the file that includes the net structure for SLAM" << endl + << endl + << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl + << " use randomized octree based point reduction (pts per voxel=)" << endl + << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl + << endl + << bold << " -p, --trustpose" << normal << endl + << " Trust the pose file, do not extrapolate the last transformation." << endl + << " (just for testing purposes, or gps input.)" << endl + << endl + << bold << " -q, --quiet" << normal << endl + << " Quiet mode. Suppress (most) messages" << endl + << endl + << bold << " -Q, --veryquiet" << normal << endl + << " Very quiet mode. Suppress all messages, except in case of error." << endl + << endl + << bold << " -S, --scanserver" << normal << endl + << " Use the scanserver as an input method and handling of scan data" << endl + << endl + << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl + << " turns on octree based point reduction (voxel size=)" << endl + << endl + << bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl + << " turns on randomized reduction, using about every -th point only" << 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 << "--nns_method=" << normal << "NR [default: 1]" << endl + << " selects the Nearest Neighbor Search Algorithm" << endl + << " 0 = simple k-d tree " << endl + << " 1 = cached k-d tree " << endl + << " 2 = ANNTree " << endl + << " 3 = BOCTree " << endl + << endl + << bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl + << " this option activates icp running on GPU instead of CPU"< +class WriteOnce { +public: + WriteOnce(T& value) : value(value), written(false) {} + WriteOnce& operator=(const T& other) { if(!written) { value = other; written = true; } return *this; } + operator T() const { return value; } +private: + T& value; + bool written; +}; + +/** + * Parsing of a formats file in the scan directory for default type and scan + * index ranges without overwriting user set parameters. Does nothing if + * file doesn't exist. + * + * @param dir directory the scans and format file are contained in + * @param type which ScanIO to use for the scans in that directory + * @param start index for the first valid scan + * @param end index for the last valid scan + */ +void parseFormatFile(string& dir, WriteOnce& type, WriteOnce& start, WriteOnce& end) +{ + ifstream file((dir+"format").c_str()); + if(!file.good()) return; + + string line, key, value, format; + while(getline(file, line)) { + size_t pos = line.find('='); + key = trim(line.substr(0, pos - 0)); + value = trim(line.substr(pos+1)); + if(key == "format") { + try { + format = value; + type = formatname_to_io_type(format.c_str()); + } catch (...) { // runtime_error + cerr << "Error while parsing format file: Format '" << format << "' unknown." << endl; + break; + } + } else if(key == "start") { + stringstream str(value.c_str()); + int s; + str >> s; + start = s; + } else if(key == "end") { + stringstream str(value.c_str()); + int e; + str >> e; + end = e; + } else { + cerr << "Error while parsing format file: Unknown key '" << key << "'" << endl; + break; + } + } +} + +/** A function that parses the command-line arguments and sets the respective flags. + * @param argc the number of arguments + * @param argv the arguments + * @param dir the directory + * @param red using point reduction? + * @param rand use randomized point reduction? + * @param mdm maximal distance match + * @param mdml maximal distance match for SLAM + * @param mni maximal number of iterations + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param maxDist - maximal distance of points being loaded + * @param minDist - minimal distance of points being loaded + * @param quiet switches on/off the quiet mode + * @param veryQuiet switches on/off the 'very quiet' mode + * @param extrapolate_pose - i.e., extrapolating the odometry by the last transformation + * (vs. taking the pose file as exact) + * @param meta match against all scans (= meta scan), or against the last scan only??? + * @param anim selects the rotation representation for the matching algorithm + * @param mni_lum sets the maximal number of iterations for SLAM + * @param net specifies the file that includes the net structure for SLAM + * @param cldist specifies the maximal distance for closed loops + * @param epsilonICP stop ICP iteration if difference is smaller than this value + * @param epsilonSLAM stop SLAM iteration if average difference is smaller than this value + * @param algo specfies the used algorithm for rotation computation + * @param lum6DAlgo specifies the used algorithm for global SLAM correction + * @param loopsize defines the minimal loop size + * @param tracking select sematic algorithm of none/classification/tracking on/off the point classification mode + * @return 0, if the parsing was successful. 1 otherwise + */ +int parseArgs(int argc, char **argv, string &dir, double &red, int &rand, + double &mdm, double &mdml, double &mdmll, + int &mni, int &start, int &end, int &maxDist, int &minDist, bool &quiet, bool &veryQuiet, + bool &extrapolate_pose, bool &meta, int &algo,int &tracking, int &loopSlam6DAlgo, int &lum6DAlgo, int &anim, + int &mni_lum, string &net, double &cldist, int &clpairs, int &loopsize,int &trackingAlgo, + double &epsilonICP, double &epsilonSLAM, int &nns_method, bool &exportPts, double &distLoop, + int &iterLoop, double &graphDist, int &octree, bool &cuda_enabled, IOType &type, + bool& scanserver) +{ + int c; + // from unistd.h: + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + /* options descriptor */ + // 0: no arguments, 1: required argument, 2: optional argument + static struct option longopts[] = { + { "format", required_argument, 0, 'f' }, + { "algo", required_argument, 0, 'a' }, + { "tracking", required_argument, 0, 'b' }, + { "track_value", required_argument, 0, 'T' }, + { "nns_method", required_argument, 0, 't' }, + { "loop6DAlgo", required_argument, 0, 'L' }, + { "graphSlam6DAlgo", required_argument, 0, 'G' }, + { "net", required_argument, 0, 'n' }, + { "iter", required_argument, 0, 'i' }, + { "iterSLAM", required_argument, 0, 'I' }, + { "max", required_argument, 0, 'm' }, + { "loopsize", required_argument, 0, 'l' }, + { "cldist", required_argument, 0, 'c' }, + { "clpairs", required_argument, 0, 'C' }, + { "min", required_argument, 0, 'M' }, + { "dist", required_argument, 0, 'd' }, + { "distSLAM", required_argument, 0, 'D' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "octree", optional_argument, 0, 'O' }, + { "random", required_argument, 0, 'R' }, + { "quiet", no_argument, 0, 'q' }, + { "veryquiet", no_argument, 0, 'Q' }, + { "trustpose", no_argument, 0, 'p' }, + { "anim", required_argument, 0, 'A' }, + { "metascan", no_argument, 0, '2' }, // use the long format only + { "DlastSLAM", required_argument, 0, '4' }, // use the long format only + { "epsICP", required_argument, 0, '5' }, // use the long format only + { "epsSLAM", required_argument, 0, '6' }, // use the long format only + { "exportAllPoints", no_argument, 0, '8' }, + { "distLoop", required_argument, 0, '9' }, // use the long format only + { "iterLoop", required_argument, 0, '1' }, // use the long format only + { "graphDist", required_argument, 0, '3' }, // use the long format only + { "cuda", no_argument, 0, 'u' }, // cuda will be enabled + { "trackingAlgo", required_argument, 0, 'y'},//tracking algorithm + { "scanserver", no_argument, 0, 'S' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + cout << endl; + while ((c = getopt_long(argc, argv, "T:O:v:f:A:G:L:a:b:t:r:R:d:D:i:l:I:c:C:n:S:s:e:m:M:y:u:q:Q:p", longopts, NULL)) != -1) + switch (c) + { + case 'a': + algo = atoi(optarg); + if ((algo < 0) || (algo > 9)) { + cerr << "Error: ICP Algorithm not available." << endl; + exit(1); + } + break; + case 'b': + tracking = atoi(optarg); + break; + case 'v': + save_animation = true; + anim_frame_rate = atoi(optarg); + break; + case 'T': + constant_static_or_moving = atof(optarg); + if ((constant_static_or_moving < 0) || (constant_static_or_moving > 300)) { + cerr << "Error: constant_static_or_moving not available." << endl; + exit(1); + } + break; + case 't': + nns_method = atoi(optarg); + if ((nns_method < 0) || (nns_method > 3)) { + cerr << "Error: NNS Method not available." << endl; + exit(1); + } + break; + case 'L': + loopSlam6DAlgo = atoi(optarg); + if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) { + cerr << "Error: global loop closing algorithm not available." << endl; + exit(1); + } + break; + case 'G': + lum6DAlgo = atoi(optarg); + if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) { + cerr << "Error: global relaxation algorithm not available." << endl; + exit(1); + } + break; + case 'c': + cldist = atof(optarg); + break; + case 'C': + clpairs = atoi(optarg); + break; + case 'l': + loopsize = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'R': + rand = atoi(optarg); + break; + case 'd': + mdm = atof(optarg); + break; + case 'D': + mdml = atof(optarg); + break; + case 'i': + mni = atoi(optarg); + break; + case 'I': + mni_lum = atoi(optarg); + break; + case 'n': + net = optarg; + break; + case 's': + w_start = atoi(optarg); + if (w_start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (w_end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (w_end < w_start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'q': + quiet = true; + break; + case 'Q': + quiet = veryQuiet = true; + break; + case 'p': + extrapolate_pose = false; + break; + case 'y': + trackingAlgo=atoi(optarg);//choose tracking algorithm + break; + case 'A': + anim = atoi(optarg); + break; + case '2': // = --metascan + meta = true; + break; + case '4': // = --DlastSLAM + mdmll = atof(optarg); + break; + case '5': // = --epsICP + epsilonICP = atof(optarg); + break; + case '6': // = --epsSLAM + epsilonSLAM = atof(optarg); + break; + case '8': // not used + exportPts = true; + break; + case '9': // = --distLoop + distLoop = atof(optarg); + break; + case '1': // = --iterLoop + iterLoop = atoi(optarg); + break; + case '3': // = --graphDist + graphDist = atof(optarg); + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case 'u': + cuda_enabled = true; + break; + case 'S': + scanserver = true; // maybe some errors. + break; + case '?': + usage(argv[0]); + return 1; + default: + abort (); + } + + scanserver = true; + if (optind != argc-1) + { + cerr << "\n*** Directory missing ***" << endl; + usage(argv[0]); + } + dir = argv[optind]; + +#ifndef _MSC_VER + if (dir[dir.length()-1] != '/') dir = dir + "/"; +#else + if (dir[dir.length()-1] != '\\') dir = dir + "\\"; +#endif + + parseFormatFile(dir, w_type, w_start, w_end); + + return 0; +} + +/** + * This function is does all the matching stuff + * it iterates over all scans using the algorithm objects to calculate new poses + * objects could be NULL if algorithm should not be used + * + * @param cldist maximal distance for closing loops + * @param loopsize minimal loop size + * @param allScans Contains all laser scans + * @param my_icp6D the ICP implementation + * @param meta_icp math ICP against a metascan + * @param nns_method Indicates the nearest neigbor search method to be used + * @param my_loopSlam6D used loopoptimizer + * @param my_graphSlam6D used global optimization + * @param nrIt The number of iterations the global SLAM-algorithm will run + * @param epsilonSLAM epsilon for global SLAM iteration + * @param mdml maximal distance match for global SLAM + * @param mdmll maximal distance match for global SLAM after all scans ar matched + */ +void matchGraph6Dautomatic(double cldist, int loopsize, vector allScans, icp6D *my_icp6D, + bool meta_icp, int nns_method, bool cuda_enabled, + loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt, + double epsilonSLAM, double mdml, double mdmll, double graphDist, + bool &eP, IOType type) +{ + double cldist2 = sqr(cldist); + + // list of scan for metascan + vector < Scan* > metas; + + // graph for loop optimization + graph_t g; + + int n = allScans.size(); + + int loop_detection = 0; + double dist, min_dist = -1; + int first = 0, last = 0; + + for(int i = 1; i < n; i++) { + cout << i << "/" << n << endl; + + add_edge(i-1, i, g); + + if(eP) { + allScans[i]->mergeCoordinatesWithRoboterPosition(allScans[i-1]); + } + + //Hack to get all icp transformations into the .frames Files + if(i == n-1 && my_icp6D != NULL && my_icp6D->get_anim() == -2) { + my_icp6D->set_anim(-1); + } + + /*if(i == 85 || i == 321 || i == 533) { + my_icp6D->set_anim(1); + }*/ + + if(my_icp6D != NULL){ + cout << "ICP" << endl; + // Matching strongly linked scans with ICPs + if(meta_icp) + { +// metas.push_back(allScans[i - 1]); +// MetaScan* meta_scan = new MetaScan(metas); +// my_icp6D->match(meta_scan, allScans[i]); + //delete meta_scan; + } else { + switch(type) { + case UOS_MAP: + case UOS_MAP_FRAMES: + my_icp6D->match(allScans[0], allScans[i]); + break; + case RTS_MAP: + //untested (and could not work) + //if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan); + my_icp6D->match(allScans[0], allScans[i]); + break; + default: + my_icp6D->match(allScans[i - 1], allScans[i]); + break; + } + } + } else { + double id[16]; + M4identity(id); + allScans[i]->transform(id, Scan::ICP, 0); + } + + /*if(i == 85 || i == 321 || i == 533) { + my_icp6D->set_anim(-2); + }*/ + + if(loop_detection == 1) { + loop_detection = 2; + } + + for(int j = 0; j < i - loopsize; j++) { + dist = Dist2(allScans[j]->get_rPos(), allScans[i]->get_rPos()); + if(dist < cldist2) { + loop_detection = 1; + if(min_dist < 0 || dist < min_dist) { + min_dist = dist; + first = j; + last = i; + } + } + } + + if(loop_detection == 2) { + loop_detection = 0; + min_dist = -1; + + if(my_loopSlam6D != NULL) { + cout << "Loop close: " << first << " " << last << endl; + my_loopSlam6D->close_loop(allScans, first, last, g); + add_edge(first, last, g); + } + + if(my_graphSlam6D != NULL && mdml > 0) { + int j = 0; + double ret; + do { + // recalculate graph + Graph *gr = new Graph(i + 1, cldist2, loopsize); + cout << "Global: " << j << endl; + ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1); + delete gr; + j++; + } while (j < nrIt && ret > epsilonSLAM); + } + } + } + + if(loop_detection == 1 && my_loopSlam6D != NULL) { + cout << "Loop close: " << first << " " << last << endl; + my_loopSlam6D->close_loop(allScans, first, last, g); + add_edge(first, last, g); + } + + if(my_graphSlam6D != NULL && mdml > 0.0) { + int j = 0; + double ret; + do { + // recalculate graph + Graph *gr = new Graph(n, cldist2, loopsize); + cout << "Global: " << j << endl; + ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1); + delete gr; + j++; + } while (j < nrIt && ret > epsilonSLAM); + } + + if(my_graphSlam6D != NULL && mdmll > 0.0) { + my_graphSlam6D->set_mdmll(mdmll); + int j = 0; + double ret; + do { + // recalculate graph + Graph *gr = new Graph(n, sqr(graphDist), loopsize); + cout << "Global: " << j << endl; + ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1); + delete gr; + j++; + } while (j < nrIt && ret > epsilonSLAM); + } +} + +icp6Dminimizer * CreateICPalgo( int algo , bool quiet ) +{ + icp6Dminimizer *my_icp6Dminimizer = 0; + switch (algo) { + case 1 : + my_icp6Dminimizer = new icp6D_QUAT(quiet); + break; + case 2 : + my_icp6Dminimizer = new icp6D_SVD(quiet); + break; + case 3 : + my_icp6Dminimizer = new icp6D_ORTHO(quiet); + break; + case 4 : + my_icp6Dminimizer = new icp6D_DUAL(quiet); + break; + case 5 : + my_icp6Dminimizer = new icp6D_HELIX(quiet); + break; + case 6 : + my_icp6Dminimizer = new icp6D_APX(quiet); + break; + case 7 : + my_icp6Dminimizer = new icp6D_LUMEULER(quiet); + break; + case 8 : + my_icp6Dminimizer = new icp6D_LUMQUAT(quiet); + break; + case 9 : + my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet); + break; + } + return my_icp6Dminimizer; + } + +int FinalSLAM( double &red, int &rand, + double &mdm, double &mdml, double &mdmll, + int &mni, int &start, int &end, int &maxDist, int &minDist, bool &quiet, bool &veryQuiet, + bool &eP, bool &meta, int &algo, int &loopSlam6DAlgo, int &lum6DAlgo, int &anim, + int &mni_lum, string &net, double &cldist, int &clpairs, int &loopsize, + double &epsilonICP, double &epsilonSLAM, int &nns_method, bool &exportPts, double &distLoop, + int &iterLoop, double &graphDist, int &octree, bool &cuda_enabled, IOType &type + ) +{ + + icp6Dminimizer *my_icp6Dminimizer = 0; + my_icp6Dminimizer= CreateICPalgo( algo, quiet); + + if (mni_lum == -1 && loopSlam6DAlgo == 0) + { + icp6D *my_icp = 0; + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); + } + + if (my_icp) my_icp->doICP(Scan::allScans); + delete my_icp; + } else if (clpairs > -1) { + //!!!!!!!!!!!!!!!!!!!!!!!! + icp6D *my_icp = 0; + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); + } + my_icp->doICP(Scan::allScans); + graphSlam6D *my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, + rand, eP, anim, epsilonICP, nns_method, epsilonSLAM); + my_graphSlam6D->matchGraph6Dautomatic(Scan::allScans, mni_lum, clpairs, loopsize); + //!!!!!!!!!!!!!!!!!!!!!!!! + } else { + graphSlam6D *my_graphSlam6D = 0; + switch (lum6DAlgo) { + case 1 : + my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 2 : + my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 3 : + my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 4 : + my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, epsilonSLAM); + break; + case 5 : + my_graphSlam6D = new graphToro(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + -2, epsilonICP, nns_method, epsilonSLAM); + break; + case 6 : + my_graphSlam6D = new graphHOGMan(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, + -2, epsilonICP, nns_method, epsilonSLAM); + break; + } + // Construct Network + if (net != "none") { + icp6D *my_icp = 0; + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); + } + my_icp->doICP(Scan::allScans); + + Graph* structure; + structure = new Graph(net); + my_graphSlam6D->doGraphSlam6D(*structure, Scan::allScans, mni_lum); + if(mdmll > 0.0) { + my_graphSlam6D->set_mdmll(mdmll); + my_graphSlam6D->doGraphSlam6D(*structure, Scan::allScans, mni_lum); + } + + } else { + icp6D *my_icp = 0; + if(algo > 0) { + if (cuda_enabled) { +#ifdef WITH_CUDA + my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); +#else + cout << "slam6d was not compiled for excuting CUDA code" << endl; +#endif + } else { + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method); + } + + loopSlam6D *my_loopSlam6D = 0; + switch(loopSlam6DAlgo) { + case 1: + my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 2: + my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 3: + my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 4: + my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 5: + my_loopSlam6D = new loopToro(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + case 6: + my_loopSlam6D = new loopHOGMan(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, + rand, eP, 10, epsilonICP, nns_method); + break; + } + + matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta, + nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D, + mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type); + delete my_icp; + if(loopSlam6DAlgo > 0) { + delete my_loopSlam6D; + } + } + if(my_graphSlam6D > 0) { + delete my_graphSlam6D; + } + } + } + delete my_icp6Dminimizer; + return 0; + } + +void MatchTwoScan(icp6D *my_icp, VeloScan* currentScan, int scanCount, bool eP ) +{ + Scan *PreviousScan = 0; + //////////////////////ICP////////////////////// + if (scanCount > 0) + { + PreviousScan = Scan::allScans[scanCount-1]; + // extrapolate odometry // 以前一帧的坐标为基准 + if (eP) + currentScan->mergeCoordinatesWithRoboterPosition(PreviousScan); + + my_icp->match(PreviousScan, currentScan); + } +} +/** + * Main program for 6D SLAM. + * Usage: bin/slam6D 'dir', + * with 'dir' the directory of a set of scans + * ... + */ +int main(int argc, char **argv) +{ + +#ifndef _MSC_VER + glutInit(&argc, argv); +#else + glutInit(&argc, argv); +#endif + + signal (SIGSEGV, sigSEGVhandler); + signal (SIGINT, sigSEGVhandler); + + cout << "slam6D - A highly efficient SLAM implementation based on scan matching" << endl + << " with 6 degrees of freedom" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, 2006 - 2009" << 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, mdmll = -1.0, mdml = 25.0, mdm = 25.0; + int rand = -1, mni = 50; + int start = 0, end = -1; + bool quiet = false; + bool veryQuiet = false; + int maxDist = -1; + int minDist = -1; + bool eP = true; // should we extrapolate the pose?? + bool meta = false; // match against meta scan, or against LAST scan only? + int algo = 1; + int mni_lum = -1; + double cldist = 500; + int clpairs = -1; + int loopsize = 20; + string net = "none"; + int anim = -1; + double epsilonICP = 0.00001; + double epsilonSLAM = 0.5; + int nns_method = simpleKD; + bool exportPts = false; + int loopSlam6DAlgo = 0; + int lum6DAlgo = 0; + int tracking = 1; + double distLoop = 700.0; + int iterLoop = 100; + double graphDist = cldist; + int octree = 0; // employ randomized octree reduction? + bool cuda_enabled = false; + IOType type = UOS; + int trackingAlgo=0; + bool scanserver = false; + + parseArgs(argc, argv, dir, red, rand, mdm, mdml, mdmll, mni, start, end, + maxDist, minDist, quiet, veryQuiet, eP, meta, algo, tracking, + loopSlam6DAlgo, lum6DAlgo, anim, + mni_lum, net, cldist, clpairs, loopsize, trackingAlgo,epsilonICP, epsilonSLAM, + nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type, + scanserver); + + + cout << "VeloSLAM will proceed with the following parameters:" << endl; + + //@@@ to do :-) + icp6Dminimizer *my_icp6Dminimizer = 0; + my_icp6Dminimizer= CreateICPalgo( algo, quiet); + icp6D *my_icp = 0; + my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, + anim, epsilonICP, nns_method, cuda_enabled); + + if (my_icp==0) + { + cerr<< "can not create ICP " << endl; + exit(0); + } + + Scan::openDirectory(scanserver, dir, type, start, end); + + if(VeloScan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + double eu[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + vector ptss; + veryQuiet =true; + + if(!veryQuiet) + StartShow(); + ICPFinished =true; + + //Main Loop for ICP with Moving Object Detection and Tracking + for(ScanVector::iterator it = Scan::allScans.begin(); + it != Scan::allScans.end(); + ++it) + { + while(DebugDrawFinished ==false && !veryQuiet) + { + Sleep(1); + } + VeloScan* currentScan =(VeloScan* ) *it; + currentScan->setRangeFilter(maxDist, minDist); + currentScan->setReductionParameter(red, octree); + currentScan->setSearchTreeParameter(nns_method, cuda_enabled); + currentScan->isTrackerHandled=false; + currentScan->scanid = scanCount; /// + + ICPFinished =false; + if(tracking ==1 ) + { + currentScan->FindingAllofObject(maxDist, minDist); + currentScan->ClassifiAllofObject(); + } + if(tracking ==2 ) + { + int windowsize =3; + currentScan->FindingAllofObject(maxDist, minDist); + currentScan->TrackingAllofObject(trackingAlgo); + currentScan->ClassifibyTrackingAllObject(scanCount, windowsize); + // trackMgr.ListTrackers(); + } + if( tracking ==0 || tracking ==1 ||tracking ==2 ) + { + currentScan->ExchangePointCloud(); + currentScan->calcReducedPoints_byClassifi(red, octree, PointType()); + } + + currentScan->createSearchTree(); +#ifdef NO_SLIDING_WINDOW + MatchTwoScan(my_icp, currentScan, scanCount, eP); +#else + if(current_sliding_window_pos > sliding_window_size ) + MatchTwoScan(my_icp, currentScan, sliding_window_size, eP); + else + MatchTwoScan(my_icp, currentScan, scanCount, eP); +#endif + + // update the cluster position in trakers. + /////////////////////////////////////////////////////////////////// + const double* p; + p = currentScan->get_rPos(); + Point x(p[0], p[1], p[2]); + VelodyneTrajectory.path.push_back(x); + ////////////////////////////////////////// + + scanCount++; + current_sliding_window_pos++; + +#ifdef NO_SLIDING_WINDOW + +#else + if(current_sliding_window_pos > sliding_window_size ) + { + vector ::iterator Iter = Scan::allScans.begin(); + VeloScan *deleteScan = (VeloScan*)(*Iter); + // cout << "delete scan " << deleteScan->scanid << endl; + delete deleteScan; + } +#endif + + ICPFinished =true; + if(!veryQuiet) + { + glutPostRedisplay(); + } + + while(DebugDrawFinished ==false && !veryQuiet) + { + Sleep(1); + } + //////////////////////////////////////// + } + + // long starttime = GetCurrentTimeInMilliSec(); + + // //Finall graph Matching + // FinalSLAM( red, rand, + // mdm, mdml, mdmll, + // mni, start, end, maxDist, minDist, quiet, veryQuiet, + // eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim, + // mni_lum, net, cldist, clpairs, loopsize, + // epsilonICP, epsilonSLAM, nns_method, exportPts, distLoop, + // iterLoop, graphDist, octree, cuda_enabled, type + //); + + if (exportPts) + { + cout << "Export all 3D Points to file \"points.pts\"" << endl; + ofstream redptsout("points.pts"); + for(unsigned int i = 0; i < Scan::allScans.size(); i++) + { + DataXYZ xyz_r(Scan::allScans[i]->get("xyz reduced")); + for(unsigned int i = 0; i < xyz_r.size(); ++i) + redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] << '\n'; + redptsout << std::flush; + } + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) + { + Scan* scan = *it; + scan->saveFrames(); + } + + Scan::closeDirectory(); + + delete my_icp6Dminimizer; + delete my_icp; + + cout << endl << endl; + cout << "Normal program end." << endl; + +} diff --git a/.svn/pristine/f5/f5984da835350005c283abf5b1b259e056026afd.svn-base b/.svn/pristine/f5/f5984da835350005c283abf5b1b259e056026afd.svn-base new file mode 100644 index 0000000..18330dc --- /dev/null +++ b/.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) \ No newline at end of file diff --git a/.svn/pristine/f6/f6689d2e558bed60b90f9bf5a6a989aac6928a7e.svn-base b/.svn/pristine/f6/f6689d2e558bed60b90f9bf5a6a989aac6928a7e.svn-base new file mode 100644 index 0000000..1cb9ffd --- /dev/null +++ b/.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 /configure --prefix= + 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) diff --git a/.svn/pristine/f8/f88eab7718ced1859f76d8ca97e665745cb78707.svn-base b/.svn/pristine/f8/f88eab7718ced1859f76d8ca97e665745cb78707.svn-base new file mode 100644 index 0000000..820ddb7 --- /dev/null +++ b/.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) diff --git a/.svn/pristine/fb/fb21832fa26a23fca92bb898aaaec41ef38a05d1.svn-base b/.svn/pristine/fb/fb21832fa26a23fca92bb898aaaec41ef38a05d1.svn-base new file mode 100644 index 0000000..8247522 --- /dev/null +++ b/.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 +using std::cout; +using std::cerr; +using std::endl; +#include +using std::swap; +#include +#include + +// KDtree class static variables +template +KDParams KDTreeImpl::params[MAX_OPENMP_NUM_THREADS]; + +KDtreeMetaManaged::KDtreeMetaManaged(const vector& 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& scans) +{ + unsigned int n = getPointsSize(scans); + + m_temp_indices = new Index[n]; + unsigned int s = 0, j = 0; + unsigned int scansize = scans[s]->size("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("xyz reduced"); + } + } + return m_temp_indices; +} + +unsigned int KDtreeMetaManaged::getPointsSize(const vector& scans) +{ + unsigned int n = 0; + for(vector::const_iterator it = scans.begin(); it != scans.end(); ++it) { + n += (*it)->size("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 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 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]; + } + } +} diff --git a/.svn/pristine/fe/fec41303bc005ae264f9cb242d23dfb77bcf2163.svn-base b/.svn/pristine/fe/fec41303bc005ae264f9cb242d23dfb77bcf2163.svn-base new file mode 100644 index 0000000..4adadec --- /dev/null +++ b/.svn/pristine/fe/fec41303bc005ae264f9cb242d23dfb77bcf2163.svn-base @@ -0,0 +1,170 @@ +#include +#include +#include + + +#ifdef _MSC_VER +#include +#include +#include +#elif __APPLE__ +#include +#include +#include +#else +#include +#include +#include +#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 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 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 +template +float RayDist(T *point) +{ + return point[0] * dir[0] + point[1] * dir[1] + point[2] * dir[2] - dist; +} + +template +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