diff --git a/.svn/pristine/33/3311774e885a91641251368710b1a05a30391331.svn-base b/.svn/pristine/33/3311774e885a91641251368710b1a05a30391331.svn-base new file mode 100644 index 0000000..f160e05 --- /dev/null +++ b/.svn/pristine/33/3311774e885a91641251368710b1a05a30391331.svn-base @@ -0,0 +1,1166 @@ +/* + * show_common implementation + * + * Copyright (C) Kai Lingemann, Andreas Nuechter, Jan Elseberg, Dorit Borrmann + * + * Released under the GPL version 3. + * + */ + +#ifdef WITH_GLEE +#include +#endif + +#include "show/show.h" +#include "show/show_Boctree.h" +#include "show/compacttree.h" +#include "show/NurbsPath.h" +#include "show/vertexarray.h" +#ifndef DYNAMIC_OBJECT_REMOVAL +#include "slam6d/scan.h" +#include "slam6d/managedScan.h" +#else +#include "veloslam/veloscan.h" +#endif +#include "glui/glui.h" /* Header File For The glui functions */ +#include +using std::ifstream; +#include +using std::exception; +#include + +#ifdef WITH_METRICS +#include "slam6d/metrics.h" +#endif + +#ifdef _MSC_VER +#include "XGetopt.h" +#else +#include +#endif + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +#include "slam6d/point_type.h" +#include "slam6d/io_utils.h" +#include "show/display.h" + +/** + * This vector contains the pointer to a vertex array for + * all colors (inner vector) and all scans (outer vector) + */ +vector< vector > vvertexArrayList; + +vector< ::SDisplay*> displays; +/** + * the octrees that store the points for each scan + */ +//Show_BOctTree **octpts; +vector octpts; +/** + * Storing the base directory + */ +string scan_dir; + +/** + * Storing the ID of the main windows + */ +int window_id; + +/** + * Size of points + */ +GLfloat pointsize = 1.0; +int anim_delay = 5; + +/** + * Select Color Buffer + */ +GLenum buffermode = GL_BACK; + +/** + * Indicator whether and how the drawing window + * has to be updated. + * + * haveToUpdate == 1 redisplay + * haveToUpdate == 2 reshape + * haveToUpdate == 3 animation scan matching + * haveToUpdate == 4 stop animation scan matching + * haveToUpdate == 6 path animation + * haveToUpdate == 7 force redisplay with all points + */ +int haveToUpdate = 0; + +/** + * Flag for invert the scene + */ +bool invert = true; + +/** + * Flag for indicating brid eyes view + */ +bool showTopView = false; + +/** + * Flag for idicating camera add mode + */ +bool addCameraView = false; //Is the view in add box mode? + +/** + * Storing the apex angle of the camera + */ +GLfloat cangle = 60.0; // Current camera opening mode +GLfloat cangle_old = cangle; + +/** + * Current rotation axis of the scene as quaternion + */ +GLdouble quat[4] ={0.0, 0.0, 0.0, 1.0}; +GLdouble Rquat[4] ={0.0, 0.0, 0.0, 1.0}; + +/** + * Current translation of the scene + */ +GLdouble X = 0.0, Y = 0.0, Z = 0.0; +GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0; + +/** + * Center of Mass coordinates + */ +GLdouble CoM[3] = { 0.0, 0.0, 0.0 }; + +/** + * parallel zoom (similar to apex angle) for parallel projection + */ +GLfloat pzoom = 2000.0; +GLfloat pzoom_old = pzoom; + + +/** + * Mode of the fog (exp, exp2, linear) + */ +GLint fogMode = GL_EXP; + +/** + * Indicates if fog should be shown + */ +int show_fog = 1; + +/** + * Indicates if the points should be shown + */ +int show_points = 1; // Show data points in the viewer? + +/** + * Indicates if camera boxes should be shown + */ +int show_cameras = 1; // Show the camera boxes in the viewer? + +/** + * Indicates if camera path or robot path should be shown + */ +int show_path = 1; // Show the camera movement path ? + +/** + * Camera navigation by mouse or by panel + */ +int cameraNavMouseMode = 1; + +int mouseNavX, mouseNavY; +int mouseNavButton = -1; + +double mouseRotX = 0.0; +double mouseRotY = 0.0; +double mouseRotZ = 0.0; + +bool keymap[256]; + +//@@@ +//int animate_both = 0; // Animate both scan matchin and path? + +int frameNr = 0; + +/** + * Storing of all transformation (frames for animation) of all scans + */ +vector < vector > MetaMatrix; + +/** + * Storing of AlgoType for all frames + */ +vector < vector > MetaAlgoType; + +/** + * Window position + */ +int START_X = 0; +int START_Y = 0; +int START_WIDTH = 960; +int START_HEIGHT = 540; +GLdouble aspect = (double)START_WIDTH/(double)START_HEIGHT; // Current aspect ratio +bool advanced_controls = false; + +bool fullscreen = false; +int current_width = START_WIDTH; +int current_height = START_HEIGHT; + + +// the following values are scale dependant, i.e. all values are in m +float neardistance = 0.10; +double oldneardistance = 0.10; +float maxfardistance = 400.0; +double fardistance = 400.0; +double oldfardistance = 40000.0; +double movementSpeed = 0.1; +double defaultZoom = 20.0; +GLfloat fogDensity = 0.1; +double voxelSize = 0.20; + + +float adaption_rate = 1.0; +float LevelOfDetail = 0.0001; + + +// Defines for Point Semantic +#define TYPE_UNKNOWN 0x0000 +#define TYPE_OBJECT 0x0001 +#define TYPE_GROUND 0x0002 +#define TYPE_CEILING 0x0003 + +unsigned int cam_choice = 0; + +static unsigned int path_iterator = 0; +static int oldcamNavMode = 0; + +/** + * Animation sould be saved to file + */ +int save_animation = 0; +/** + * If true, interpolation for camera path is based on distance, else always + * the same number of images between two cameras. + */ +int inter_by_dist = 1; + +/**some variables for the camera path**/ +vector path_vectorX, path_vectorZ, lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ; +vector cams; +vector lookats; +vector ups; + +NurbsPath cam_nurbs_path; +char *path_file_name; +char *pose_file_name; + +/** Factor for saved image size */ +int factor = 1; + +/** + * program tries to have this framerate + */ +float idealfps = 20.0; +/** + * value of the listBox fo Color Value and Colormap + */ +int listboxColorVal = 0; +int listboxColorMapVal = 0; +int colorScanVal = 0; +ScanColorManager *cm; +float mincolor_value = 0.0; +float maxcolor_value = 0.0; +//unsigned int types = Point::USE_HEIGHT; +PointType pointtype; + +/** + * Contains the selected points for each scan + */ +set *selected_points; +/** + * Select single points? + */ +int select_voxels = 0; +/** + * Select or unselect points ? + */ +int selectOrunselect = 1; +/** octree depth for selecting groups of points */ +int selection_depth = 1; +int brush_size = 0; +char *selection_file_name; + +int current_frame = 0; +#include "show_menu.cc" +#include "show_animate.cc" +#include "show_gl.cc" + +/** + * Explains the usage of this program's command line parameters + * @param prog name of the program + */ +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 << " -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, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl + << endl + << bold << " -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl + << " will attempt to display points with a framerate of NR" << endl + << endl + << bold << " -l" << normal << " FILE, " << bold << "--loadObj=" << normal << + "FILE" << endl + << " load objects specified in " << endl + << 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 << " -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 << " -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl + << " sets the starting and reset position to: " << endl + << " 0 = the origin of the coordinate system (default)" << endl + << " 1 = the position of the first scan (default if --origin is in argument list)" << endl + << " 2 = the center of the first scan" << 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 << " -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 << " -C" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl + << " scale factor to use (default: 0.01), modifies movement speed etc. " << endl + << " use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl + << " " << endl + << endl + + << bold << " -R, --reflectance, --reflectivity" << normal << endl + << " use reflectivity values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -D, --temperature, --degree" << normal << endl + << " use temperature values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -a, --amplitude" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -d, --deviation" << endl << normal + << " use amplitude values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -h, --height" << endl << normal + << " use y-values for coloring point clouds" << endl + << " only works when using octree display" << endl + << endl + << bold << " -T, --type" << endl << normal + << " use type values for coloring point clouds" << endl + << " only works when using octree display" << endl + << bold << " -c, --color" << endl << normal + << " use color RGB values for coloring point clouds" << endl + << bold << " -b" << normal << " NR, " << bold << "--sphere=" << normal << "NR" << endl + << " map all measurements on a sphere (of radius NRcm)" << endl + << bold << " --saveOct" << endl << normal + << " stores all used scans as octrees in the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are stored as well." << endl + << " only works when using octree display" << endl + << bold << " --loadOct" << endl << normal + << " only reads octrees from the given directory" << endl + << " All reflectivity/amplitude/deviation/type settings are read from file." << endl + << " --reflectance/--amplitude and similar parameters are therefore ignored." << endl + << " only works when using octree display" << endl + << endl << endl; + + exit(1); +} + +/** + * A function that parses the command-line arguments and sets the respective flags. + * + * @param argc the number of arguments + * @param argv the arguments + * @param dir parsing result - the directory + * @param start parsing result - starting at scan number 'start' + * @param end parsing result - stopping at scan number 'end' + * @param maxDist parsing result - maximal distance + * @param minDist parsing result - minimal distance + * @param readInitial parsing result - read a file containing a initial transformation matrix + * @param type parsing result - file format to be read + * @return 0, if the parsing was successful, 1 otherwise + */ +int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, + double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, + bool &loadOct, bool &saveOct, int &origin, double &scale, IOType &type, bool& scanserver, + double& sphereMode) +{ + unsigned int types = PointType::USE_NONE; + start = 0; + end = -1; // -1 indicates no limitation + maxDist = -1; // -1 indicates no limitation + int c; + // from unistd.h + extern char *optarg; + extern int optind; + + WriteOnce w_type(type); + WriteOnce w_start(start), w_end(end); + + cout << endl; + static struct option longopts[] = { + { "origin", optional_argument, 0, 'o' }, + { "format", required_argument, 0, 'f' }, + { "fps", required_argument, 0, 'F' }, + { "scale", required_argument, 0, 'C' }, + { "start", required_argument, 0, 's' }, + { "end", required_argument, 0, 'e' }, + { "reduce", required_argument, 0, 'r' }, + { "max", required_argument, 0, 'm' }, + { "min", required_argument, 0, 'M' }, + { "octree", optional_argument, 0, 'O' }, + { "reflectance", no_argument, 0, 'R' }, + { "reflectivity", no_argument, 0, 'R' }, + { "temperature", no_argument, 0, 'D' }, + { "degree", no_argument, 0, 'D' }, + { "amplitude", no_argument, 0, 'a' }, + { "deviation", no_argument, 0, 'd' }, + { "height", no_argument, 0, 'h' }, + { "type", no_argument, 0, 'T' }, + { "color", no_argument, 0, 'c' }, + { "loadObj", required_argument, 0, 'l' }, + { "saveOct", no_argument, 0, '0' }, + { "loadOct", no_argument, 0, '1' }, + { "advanced", no_argument, 0, '2' }, + { "scanserver", no_argument, 0, 'S' }, + { "sphere", required_argument, 0, 'b' }, + { 0, 0, 0, 0} // needed, cf. getopt.h + }; + + while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:C:SwtRDadhTcb", longopts, NULL)) != -1) { + switch (c) { + case 's': + w_start = atoi(optarg); + if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } + break; + case 'e': + w_end = atoi(optarg); + if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } + if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } + break; + case 'm': + maxDist = atoi(optarg); + break; + case 'M': + minDist = atoi(optarg); + break; + case 'r': + red = atof(optarg); + break; + case 't': + readInitial = true; + break; + case 'O': + if (optarg) { + octree = atoi(optarg); + } else { + octree = 1; + } + break; + case 'f': + try { + w_type = formatname_to_io_type(optarg); + } catch (...) { // runtime_error + cerr << "Format " << optarg << " unknown." << endl; + abort(); + } + break; + case '?': + usage(argv[0]); + return 1; + case 'R': + types |= PointType::USE_REFLECTANCE; + break; + case 'D': + types |= PointType::USE_TEMPERATURE; + break; + case 'a': + types |= PointType::USE_AMPLITUDE; + break; + case 'd': + types |= PointType::USE_DEVIATION; + break; + case 'h': + types |= PointType::USE_HEIGHT; + break; + case 'T': + types |= PointType::USE_TYPE; + break; + case 'c': + types |= PointType::USE_COLOR; + break; + case 'F': + fps = atof(optarg); + break; + case 'C': + scale = atof(optarg); + break; + case 'S': + scanserver = true; + break; + case 'o': + if (optarg) { + origin = atoi(optarg); + } else { + origin = 1; + } + break; + case '0': + saveOct = true; + break; + case '1': + loadOct = true; + break; + case 'l': + loadObj = optarg; + break; + case '2': + advanced_controls = true; + break; + case 'b': + sphereMode = atof(optarg); + break; + 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 + + parseFormatFile(dir, w_type, w_start, w_end); + + ptype = PointType(types); + return 0; +} + +void setResetView(int origin) { + if (origin == 1) { + // set origin to the pose of the first scan + double *transmat = MetaMatrix[0].back(); + cout << transmat << endl; + + RVX = -transmat[12]; + RVY = -transmat[13]; + RVZ = -transmat[14]; + Matrix4ToQuat(transmat, Rquat); + X = RVX; + Y = RVY; + Z = RVZ; + quat[0] = Rquat[0]; + quat[1] = Rquat[1]; + quat[2] = Rquat[2]; + quat[3] = Rquat[3]; + } else if (origin == 2) { + // set origin to the center of the first octree + double center[3], center_transformed[3]; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[0])->getCenter(center); +#else + ((Show_BOctTree*)octpts[0])->getCenter(center); +#endif + VTrans(MetaMatrix[0].back(), center, center_transformed); + RVX = -center_transformed[0]; + RVY = -center_transformed[1]; + RVZ = -center_transformed[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } else if (origin == 3) { + // set origin to the center of mass of all scans + for (size_t i = 0; i < octpts.size(); ++i) { + vector points; +#ifdef USE_COMPACT_TREE + ((compactTree*)octpts[i])->AllPoints( points ); +#else + BOctTree* cur_tree = ((Show_BOctTree*)octpts[i])->getTree(); + cur_tree->AllPoints( points ); +#endif + + cout << "Scan " << i << " size: " << points.size() << endl; + double centroid[3] = {0., 0., 0.}; + double centroid_transformed[3];; + for (size_t j = 0; j < points.size(); ++j) { + for (unsigned int k = 0; k < 3; ++k) + centroid[k] += points[j][k]; + } + for (unsigned int k = 0; k < 3; ++k) { + centroid[k] /= (double)points.size(); + } + VTrans(MetaMatrix[i].back(), centroid, centroid_transformed); + for (unsigned int k = 0; k < 3; ++k) { + CoM[k] += centroid_transformed[k]; + } + } + for (unsigned int k = 0; k < 3; ++k) + CoM[k] /= octpts.size() * 1.; + + cout << "Center of Mass at: " << CoM[0] << ", " << CoM[1] << ", " << CoM[2] << endl; + + RVX = -CoM[0]; + RVY = -CoM[1]; + RVZ = -CoM[2]; + X = RVX; + Y = RVY; + Z = RVZ; + } +} + +/* + * A function that read the .frame files created by slam6D + * + * @param dir the directory + * @param start starting at scan number 'start' + * @param end stopping at scan number 'end' + * @param read a file containing a initial transformation matrix and apply it + */ +int readFrames(string dir, int start, int end, bool readInitial, IOType &type) +{ + + // convert to OpenGL coordinate system + double mirror[16]; + M4identity(mirror); + mirror[10] = -1.0; + + double initialTransform[16]; + if (readInitial) { + cout << "Initial Transform:" << endl; + string initialTransformFileName = dir + "initital.frame"; + ifstream initial_in(initialTransformFileName.c_str()); + if (!initial_in.good()) { + cout << "Error opening " << initialTransformFileName << endl; + exit(-1); + } + initial_in >> initialTransform; + cout << initialTransform << endl; + + // update the mirror to apply the initial frame for all frames + double tempxf[16]; + MMult(mirror, initialTransform, tempxf); + memcpy(mirror, tempxf, sizeof(tempxf)); + } + + for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + const double* transformation; + Scan::AlgoType algoType; + vector Matrices; + vector algoTypes; + + // iterate over frames (stop if none were created) and pull/convert the frames into local containers + unsigned int frame_count; + try { + frame_count = (*it)->readFrames(); + } catch(std::ios_base::failure& e) { + break; + } + for(unsigned int i = 0; i < frame_count; ++i) { + (*it)->getFrame(i, transformation, algoType); + double* transMatOpenGL = new double[16]; + + // apply mirror to convert (and initial frame if requested) the frame and save in opengl + MMult(mirror, transformation, transMatOpenGL); + + Matrices.push_back(transMatOpenGL); + algoTypes.push_back(algoType); + } + + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + + if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && it == Scan::allScans.begin()) { + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + } + current_frame = MetaMatrix.back().size() - 1; + } + + if (MetaMatrix.size() == 0) { + cerr << "*****************************************" << endl; + cerr << "** ERROR: No .frames could be found! **" << endl; + cerr << "*****************************************" << endl; + cerr << " ERROR: Missing or empty directory: " << dir << endl << endl; + return -1; + } + return 0; +} + +void generateFrames(int start, int end, bool identity) { + if (identity) { + cout << "using Identity for frames " << endl; + } else { + cout << "using pose information for frames " << endl; + } + int fileCounter = start; + int index = 0; + for (;;) { + if (fileCounter > end) break; // 'nuf read + fileCounter++; + + vector Matrices; + vector algoTypes; + + for (int i = 0; i < 3; i++) { + double *transMat = new double[16]; + + if (identity) { + M4identity(transMat); + transMat[10] = -1.0; + } else { + EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); + } + + Matrices.push_back(transMat); + algoTypes.push_back(Scan::ICP); + + } + index++; + MetaAlgoType.push_back(algoTypes); + MetaMatrix.push_back(Matrices); + current_frame = MetaMatrix.back().size() - 1; + } +} + + + +/* + * create display lists + * @to do general framework for color & type definitions + */ +void createDisplayLists(bool reduced) +{ + for(unsigned int s = 0; s < Scan::allScans.size() ; s++) { + Scan* scan = Scan::allScans[s]; + + vertexArray* myvertexArray1; + vertexArray* myvertexArray2; + + // count points + unsigned int color1 = 0, color2 = 0; + if(!reduced) { + scan->get(DATA_XYZ | DATA_TYPE); + DataType type(scan->get("type")); + + if(type.valid()) { + for(unsigned int i = 0; i < type.size(); ++i) { + if(type[i] & TYPE_GROUND) { + color1 += 3; + } else { + color2 += 3; + } + } + } else { + color2 = 3 * scan->size("xyz"); + } + + myvertexArray1 = new vertexArray(color1); + myvertexArray2 = new vertexArray(color2); + + color1 = 0; color2 = 0; + + DataXYZ xyz(scan->get("xyz")); + for(unsigned int i = 0; i < xyz.size(); ++i) { + if(type[i] & TYPE_GROUND) { + for(unsigned int j = 0; j < 3; ++j) { + myvertexArray1->array[color1++] = xyz[i][j]; + } + } else { + for(unsigned int j = 0; j < 3; ++j) { + myvertexArray2->array[color2++] = xyz[i][j]; + } + } + } + } else { + color2 = 3 * scan->size("xyz reduced"); + + myvertexArray1 = new vertexArray(0); + myvertexArray2 = new vertexArray(color2); + + color2 = 0; + + DataXYZ xyz_r(scan->get("xyz reduced")); + for(unsigned int i = 0; i < xyz_r.size(); ++i) { + for(unsigned int j = 0; j < 3; ++j) { + myvertexArray2->array[color2++] = xyz_r[i][j]; + } + } + } + + + + glNewList(myvertexArray1->name, GL_COMPILE); + //@ + //glColor4d(0.44, 0.44, 0.44, 1.0); + //glColor4d(0.66, 0.66, 0.66, 1.0); + glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender); + glDisableClientState(GL_VERTEX_ARRAY); + glEndList(); + + glNewList(myvertexArray2->name, GL_COMPILE); + //glColor4d(1.0, 1.0, 1.0, 1.0); + //glColor4d(0.0, 0.0, 0.0, 1.0); + glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array); + glEnableClientState(GL_VERTEX_ARRAY); + glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender); + glDisableClientState(GL_VERTEX_ARRAY); + glEndList(); + + // append to vector + vector vvertexArray; + vvertexArray.push_back(myvertexArray1); + vvertexArray.push_back(myvertexArray2); + vvertexArrayList.push_back(vvertexArray); + } +} + +void cycleLOD() { + LevelOfDetail = 0.00001; + for (unsigned int i = 0; i < octpts.size(); i++) + octpts[i]->cycleLOD(); +} + + +void initShow(int argc, char **argv){ + + /***************/ + /* init OpenGL */ + /***************/ + glutInit(&argc,argv); + + cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl + << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl + << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; + + if(argc <= 1){ + usage(argv[0]); + } + + double red = -1.0; + int start = 0, end = -1, maxDist = -1, minDist = -1; + string dir; + bool readInitial = false; + IOType type = UOS; + int octree = 0; + bool loadOct = false; + bool saveOct = false; + string loadObj; + int origin = 0; + double scale = 0.01; // in m + bool scanserver = false; + double sphereMode = 0.0; + + pose_file_name = new char[1024]; + path_file_name = new char[1024]; + selection_file_name = new char[1024]; + + strncpy(pose_file_name, "pose.dat", 1024); + strncpy(path_file_name, "path.dat", 1024); + strncpy(selection_file_name, "selected.3d", 1024); + + parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial, + octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, + type, scanserver, sphereMode); + + // modify all scale dependant variables + scale = 1.0 / scale; + movementSpeed *= scale; + neardistance *= scale; + oldneardistance *= scale; + maxfardistance *= scale; + fardistance *= scale; + fogDensity /= scale; + defaultZoom *= scale; + voxelSize *= scale; +// oldfardistance *= scale; + + //////////////////////// + SDisplay::readDisplays(loadObj, displays); + //////////////////// + + if (type == OCT) { + loadOct = true; + } + + // if we want to load display file get pointtypes from the files first + if(loadOct) { + string scanFileName = dir + "scan" + to_string(start,3) + ".oct"; + cout << "Getting point information from " << scanFileName << endl; + cout << "Attention! All subsequent oct-files must be of the same type!" << endl; + + pointtype = BOctTree::readType(scanFileName); + } + scan_dir = dir; + + // init and create display + M4identity(view_rotate_button); + obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0; + + // Loading scans, reducing, loading frames and generation if neccessary + + // load all available scans + Scan::openDirectory(scanserver, dir, type, start, end); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + scan->setRangeFilter(maxDist, minDist); + if (sphereMode > 0.0) scan->setRangeMutation(sphereMode); + if (red > 0) { + // scanserver differentiates between reduced for slam and reduced for show, can handle both at the same time + if(scanserver) { + dynamic_cast(scan)->setShowReductionParameter(red, octree); + } else { + scan->setReductionParameter(red, octree); + } + } + } + cm = new ScanColorManager(4096, pointtype); + +#ifdef USE_GL_POINTS // use octtrees +#ifdef USE_COMPACT_TREE + cout << "Creating compact display octrees.." << endl; +#else + cout << "Creating display octrees.." << endl; +#endif + + if(loadOct) cout << "Loading octtrees from file where possible instead of creating them from scans." << endl; + + // for managed scans the input phase needs to know how much it can handle + std::size_t free_mem = 0; + if(scanserver) + free_mem = ManagedScan::getMemorySize(); + + for(unsigned int i = 0; i < Scan::allScans.size(); ++i) { + Scan* scan = Scan::allScans[i]; + + // create data structures +#ifdef USE_COMPACT_TREE // FIXME: change compact tree, then this case can be removed + compactTree* tree; + try { + if (red > 0) { // with reduction, only xyz points + DataXYZ xyz_r(scan->get("xyz reduced show")); + tree = new compactTree(PointerArray(xyz_r).get(), xyz_r.size(), voxelSize, pointtype, cm); + } else { // without reduction, xyz + attribute points + sfloat** pts = pointtype.createPointArray(scan); + unsigned int nrpts = scan->size("xyz"); + tree = new compactTree(pts, nrpts, voxelSize, pointtype, cm); + for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts; + } + } catch(...) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } +#else // FIXME: remove the case above + scan->setOcttreeParameter(red, voxelSize, pointtype, loadOct, saveOct); + + DataOcttree* data_oct; + try { + data_oct = new DataOcttree(scan->get("octtree")); + } catch(runtime_error& e) { + cout << "Scan " << i << " could not be loaded into memory, stopping here." << endl; + break; + } + BOctTree* btree = &(data_oct->get()); + unsigned int tree_size = btree->getMemorySize(); + + if(scanserver) { + // check if the octtree would actually fit with all the others + if(tree_size > free_mem) { + delete data_oct; + cout << "Stopping at scan " << i << ", no more octtrees could fit in memory." << endl; + break; + } else { + // subtract available memory + free_mem -= tree_size; + } + } +#endif //FIXME: COMPACT_TREE + +#if !defined USE_COMPACT_TREE + // show structures + // associate show octtree with the scan and hand over octtree pointer ownership + + Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); + + // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points + tree->unlockCachedTree(); +#endif + + // octtrees have been created successfully + octpts.push_back(tree); + + // print something +#ifdef USE_COMPACT_TREE // TODO: change compact tree for memory footprint output, remove this case + cout << "Scan " << i << " octree finished." << endl; +#else + cout << "Scan " << i << " octree finished ("; + bool space = false; + if(tree_size/1024/1024 > 0) { + cout << tree_size/1024/1024 << "M"; + space = true; + } + if((tree_size/1024)%1024 > 0) { + if(space) cout << " "; + cout << (tree_size/1024)%1024 << "K"; + space = true; + } + if(tree_size%1024 > 0) { + if(space) cout << " "; + cout << tree_size%1024 << "B"; + } + cout << ")." << endl; +#endif + } + +/* +TODO: to maximize space for octtrees, implement a heuristic to remove all +CacheObjects sequentially from the start and pack octtrees one after another +until it reaches the maximum size allowed, resetting the index, but causing the +first to be locked again and stopping by catching the exception +set heuristic, do locking, catch exception, reset heuristic to default or old +*/ +#if !defined USE_COMPACT_TREE + if(scanserver) { + // activate all octtrees until they don't fit anymore + cout << "Locking managed octtrees in memory " << flush; + bool stop = false; + unsigned int loaded = 0; + unsigned int dots = (octpts.size() / 20); + if(dots == 0) dots = 1; + vector::iterator it_remove_first = octpts.end(); + for(vector::iterator it = octpts.begin(); it != octpts.end(); ++it) { + if(!stop) { + // try to lock the octtree in cache + try { + Show_BOctTree* stree = dynamic_cast*>(*it); + stree->lockCachedTree(); + loaded++; + if(loaded % dots == 0) cout << '.' << flush; + } catch(runtime_error& e) { + stop = true; + it_remove_first = it; + } + } + if(stop) { + // delete the octtree, resize after iteration + delete *it; + } + } + // now remove iterators for deleted octtrees + if(stop) octpts.erase(it_remove_first, octpts.end()); + cout << ' ' << loaded << " octtrees loaded." << endl; + } + +#endif // !COMPACT_TREE + +#else // not using octtrees + createDisplayLists(red > 0); +#endif // USE_GL_POINTS + + // load frames now that we know how many scans we actually loaded + unsigned int real_end = min((unsigned int)(end), + (unsigned int)(start + octpts.size() - 1)); + if(readFrames(dir, start, real_end, readInitial, type)) + generateFrames(start, real_end, true); + + cm->setCurrentType(PointType::USE_HEIGHT); + //ColorMap cmap; + //cm->setColorMap(cmap); + resetMinMax(0); + + selected_points = new set[octpts.size()]; + + // sets (and computes if necessary) the pose that is used for the reset button + setResetView(origin); + + for (unsigned int i = 0; i < 256; i++) { + keymap[i] = false; + } +} + +void deinitShow() +{ + static volatile bool done = false; + if(done) return; + done = true; + + cout << "Cleaning up octtrees and scans." << endl; + if(octpts.size()) { + // delete octtrees to release the cache locks within + for(vector::iterator it = octpts.begin(); it!= octpts.end(); ++it) { + delete *it; + } + } + + Scan::closeDirectory(); +} + +/** + * Global program scope destructor workaround to clean up data regardless of + * the way of program exit. + */ +struct Deinit { ~Deinit() { deinitShow(); } } deinit; diff --git a/.svn/pristine/40/40906d7a83a6c7878ccf6990ea0bd862525ab2f7.svn-base b/.svn/pristine/40/40906d7a83a6c7878ccf6990ea0bd862525ab2f7.svn-base new file mode 100644 index 0000000..0d1f3c0 --- /dev/null +++ b/.svn/pristine/40/40906d7a83a6c7878ccf6990ea0bd862525ab2f7.svn-base @@ -0,0 +1,1570 @@ +/** + * @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. + */ + +#ifndef BOCTREE_H +#define BOCTREE_H + +#include "searchTree.h" +#include "point_type.h" +#include "data_types.h" +#include "allocator.h" +#include "limits.h" +#include "nnparams.h" +#include "globals.icc" + + +#include + +#include +using std::vector; +#include +using std::deque; +#include +using std::set; +#include +using std::list; +#include +#include +#include + +#if __GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) + #define POPCOUNT(mask) __builtin_popcount(mask) +#else + #define POPCOUNT(mask) _my_popcount_3(mask) +#endif + +#include // to avoid ifdeffing for offset_ptr.get(), use &(*ptr) +namespace { namespace ip = boost::interprocess; } + + +// forward declaration +template union bitunion; + +/** + * This is our preferred representation for the leaf nodes (as it is the most compact). + * BOctTree makes an array of this, the first containing the number of points (not the + * number of coordinates) stored. + */ +template union dunion { + T v; + unsigned int length; + dunion() : length(0) {}; + +}; +// typedefs in combination with templates are weird +//typedef dunion pointrep; +#define pointrep union dunion + + + + +/** + * This struct represents the nodes of the octree + * + * child_pointer is a relative pointer to the first child of this node, as it is only + * 48 bit this will cause issues on systems with more than 268 TB of memory. All children + * of this node must be stored sequentially. If one of the children is a leaf, that + * child will be a pointer to however a set of points is represented (pointrep *). + * + * valid is a bitmask describing whether the corresponding buckets are filled. + * + * leaf is a bitmask describing whether the correpsonding bucket is a leaf node. + * + * The representation of the bitmask is somewhat inefficient. We use 16 bits for only + * 3^8 possible states, so in essence we could save 3 bits by compression. + * + */ +class bitoct{ + public: + +#ifdef _MSC_VER + __int64 child_pointer : 48; + unsigned valid : 8; + unsigned leaf : 8; +#else + signed long child_pointer : 48; + unsigned valid : 8; + unsigned leaf : 8; +#endif + /** + * sets the child pointer of parent so it points to child + */ + template + static inline void link(bitoct &parent, bitunion *child) { + parent.child_pointer = (long)((char*)child - (char*)&parent); + } + + /** + * Returns the children of this node (given as parent). + */ + template + static inline void getChildren(const bitoct &parent, bitunion* &children) { + children = (bitunion*)((char*)&parent + parent.child_pointer); + } + + template + inline bitunion* getChild(unsigned char index) { + bitunion *children = (bitunion*)((char*)this + this->child_pointer); + for (unsigned char i = 0; i < index; i++) { + if ( ( 1 << i ) & valid ) { // if ith node exists + children++; + } + } + return children; + } +}; + + +/** + * This union combines an octree node with a pointer to a set of points. This allows + * us to use both nodes and leaves interchangeably. + * + * points is a pointer to the point representation in use + * + * node is simply the octree node + * + */ +template union bitunion { + pointrep *points; + //union dunion *points; + bitoct node; + + bitunion(pointrep *p) : points(p) {}; + bitunion(bitoct b) : node(b) {}; + bitunion() : points(0) { + node.child_pointer = 0; + node.valid = 0; + node.leaf = 0; + }; // needed for new [] + + //! Leaf node: links a pointrep array [length+values] to this union, saved as an offset pointer + static inline void link(bitunion* leaf, pointrep* points) { + // use node child_pointer as offset_ptr, not pointrep + leaf->node.child_pointer = (long)((char*)points - (char*)leaf); + } + + //! Leaf node: points in the array + inline T* getPoints() const { + // absolute pointer + //return &(this->points[1].v); + // offset pointer + return reinterpret_cast( + reinterpret_cast((char*)this + node.child_pointer) + 1 + ); + } + + //! Leaf node: length in the array + inline unsigned int getLength() const { + // absolute pointer + //return this->points[0].length; + // offset pointer + return (reinterpret_cast((char*)this + node.child_pointer))[0].length; + } + + //! Leaf node: all points + inline pointrep* getPointreps() const { + return reinterpret_cast((char*)this + node.child_pointer); + } + + inline bitunion* getChild(unsigned char index) const { + bitunion *children = (bitunion*)((char*)this + this->node.child_pointer); + for (unsigned char i = 0; i < index; i++) { + if ( ( 1 << i ) & node.valid ) { // if ith node exists + children++; + } + } + return children; + } + + inline bool isValid(unsigned char index) { + return ( ( 1 << index ) & node.valid ); + } + /* + inline pointrep* getChild(unsigned char index) { + bitunion *children = (bitunion*)((char*)this + this->node.child_pointer); + return children[index].points; + }*/ + + inline bool childIsLeaf(unsigned char index) { + return ( ( 1 << index ) & node.leaf ); // if ith node is leaf get center + } +}; + + +// initialized in Boctree.cc, sequence intialized on startup +extern char amap[8][8]; +extern char imap[8][8]; +extern char sequence2ci[8][256][8]; // maps preference to index in children array for every valid_mask and every case + + + +/** + * @brief Octree + * + * A cubic bounding box is calculated + * from the given 3D points. Then it + * is recusivly subdivided into smaller + * subboxes + */ +template +class BOctTree : public SearchTree { +public: + BOctTree() { + } + + template + BOctTree(P * const* pts, int n, T voxelSize, PointType _pointtype = PointType(), bool _earlystop = false ) : pointtype(_pointtype), earlystop(_earlystop) + { + alloc = new PackedChunkAllocator; + + this->voxelSize = voxelSize; + + this->POINTDIM = pointtype.getPointDim(); + + //@@@ + cout << "POINTDIM" << this->POINTDIM << endl; + + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + + // initialising + for (unsigned int i = 0; i < POINTDIM; i++) { + mins[i] = pts[0][i]; + maxs[i] = pts[0][i]; + } + + for (unsigned int i = 0; i < POINTDIM; i++) { + for (int j = 1; j < n; j++) { + mins[i] = min(mins[i], (T)pts[j][i]); + maxs[i] = max(maxs[i], (T)pts[j][i]); + } + } + + center[0] = 0.5 * (mins[0] + maxs[0]); + center[1] = 0.5 * (mins[1] + maxs[1]); + center[2] = 0.5 * (mins[2] + maxs[2]); + size = max(max(0.5 * (maxs[0] - mins[0]), 0.5 * (maxs[1] - mins[1])), 0.5 * (maxs[2] - mins[2])); + size += 1.0; // for numerical reasons we increase size + + // calculate new buckets + T newcenter[8][3]; + T sizeNew = size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(center, newcenter[i], size, i); + } + // set up values + uroot = alloc->allocate >(); + root = &uroot->node; + + countPointsAndQueueFast(pts, n, newcenter, sizeNew, *root, center); + init(); + } + + BOctTree(std::string filename) { + alloc = new PackedChunkAllocator; + deserialize(filename); + init(); + } + + template + BOctTree(vector

&pts, T voxelSize, PointType _pointtype = PointType(), bool _earlystop = false) : earlystop(_earlystop) + { + alloc = new PackedChunkAllocator; + + this->voxelSize = voxelSize; + + this->POINTDIM = pointtype.getPointDim(); + + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + + // initialising + for (unsigned int i = 0; i < POINTDIM; i++) { + mins[i] = pts[0][i]; + maxs[i] = pts[0][i]; + } + + for (unsigned int i = 0; i < POINTDIM; i++) { + for (unsigned int j = 1; j < pts.size(); j++) { + mins[i] = min(mins[i], pts[j][i]); + maxs[i] = max(maxs[i], pts[j][i]); + } + } + + center[0] = 0.5 * (mins[0] + maxs[0]); + center[1] = 0.5 * (mins[1] + maxs[1]); + center[2] = 0.5 * (mins[2] + maxs[2]); + size = max(max(0.5 * (maxs[0] - mins[0]), 0.5 * (maxs[1] - mins[1])), 0.5 * (maxs[2] - mins[2])); + + size += 1.0; // for numerical reasons we increase size + + // calculate new buckets + T newcenter[8][3]; + T sizeNew = size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(center, newcenter[i], size, i); + } + // set up values + uroot = alloc->allocate >(); + root = &uroot->node; + + countPointsAndQueue(pts, newcenter, sizeNew, *root, center); + } + + virtual ~BOctTree() + { + if(alloc) { + delete alloc; + } + } + + void init() { + // compute maximal depth as well as the size of the smalles leaf + real_voxelSize = size; + max_depth = 1; + while (real_voxelSize > voxelSize) { + real_voxelSize = real_voxelSize/2.0; + max_depth++; + } + + child_bit_depth = alloc->allocate(max_depth); + child_bit_depth_inv = alloc->allocate(max_depth); + + for(int d=0; d < max_depth; d++) { + child_bit_depth[d] = 1 << (max_depth - d - 1); + child_bit_depth_inv[d] = ~child_bit_depth[d]; + } + + mult = 1.0/real_voxelSize; + add[0] = -center[0] + size; + add[1] = -center[1] + size; + add[2] = -center[2] + size; + + largest_index = child_bit_depth[0] * 2 -1; + } + +protected: + + /** + * Serialization critical variables + */ + //! the root of the octree + ip::offset_ptr root; + ip::offset_ptr > uroot; + + //! storing the center + T center[3]; + + //! storing the dimension + T size; + + //! storing the voxel size + T voxelSize; + + //! The real voxelsize of the leaves + T real_voxelSize; + + //! Offset and real voxelsize inverse factor for manipulation points + T add[3]; + T mult; + + //! Dimension of each point: 3 (xyz) + N (attributes) + unsigned int POINTDIM; + + //! storing minimal and maximal values for all dimensions + ip::offset_ptr mins; + ip::offset_ptr maxs; + + //! Details of point attributes + PointType pointtype; + + //! ? + unsigned char max_depth; + ip::offset_ptr child_bit_depth; + ip::offset_ptr child_bit_depth_inv; + int largest_index; + + /** + * Serialization uncritical, runtime relevant variables + */ + + //! Threadlocal storage of parameters used in SearchTree operations + static NNParams params[100]; + + /** + * Serialization uncritical, runtime irrelevant variables (constructor-stuff) + */ + + //! Whether to stop subdividing at N<10 nodes or not + bool earlystop; + + //! Allocator used for creating nodes in the constructor + Allocator* alloc; + +public: + + inline const T* getMins() const { return &(*mins); } + inline const T* getMaxs() const { return &(*maxs); } + inline const T* getCenter() const { return center; } + inline T getSize() const { return size; } + inline unsigned int getPointdim() const { return POINTDIM; } + inline const bitoct& getRoot() const { return *root; } + inline unsigned int getMaxDepth() const { return max_depth; } + + inline void getCenter(double _center[3]) const { + _center[0] = center[0]; + _center[1] = center[1]; + _center[2] = center[2]; + } + + void GetOctTreeCenter(vector&c) { GetOctTreeCenter(c, *root, center, size); } + void GetOctTreeRandom(vector&c) { GetOctTreeRandom(c, *root); } + void GetOctTreeRandom(vector&c, unsigned int ptspervoxel) { GetOctTreeRandom(c, ptspervoxel, *root); } + void AllPoints(vector &vp) { AllPoints(*BOctTree::root, vp); } + + long countNodes() { return 1 + countNodes(*root); } // computes number of inner nodes + long countLeaves() { return countLeaves(*root); } // computes number of leaves + points + long countOctLeaves() { return countOctLeaves(*root); } // computes number of leaves + + void deserialize(std::string filename ) { + char buffer[sizeof(T) * 20]; + T *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(T)); + 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; + + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + + file.read(reinterpret_cast(&(*mins)), POINTDIM * sizeof(T)); + file.read(reinterpret_cast(&(*maxs)), POINTDIM * sizeof(T)); + + // read root node + uroot = alloc->allocate >(); + root = &uroot->node; + + deserialize(file, *root); + file.close(); + } + + static void deserialize(std::string filename, vector &points ) { + char buffer[sizeof(T) * 20]; + + 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 = PointType::deserialize(file); + + file.read(buffer, 5 * sizeof(T)); // read over voxelsize, center and size + file.read(buffer, sizeof(int)); + + int *ip = reinterpret_cast(buffer); + unsigned int POINTDIM = *ip; + + file.read(buffer, POINTDIM * sizeof(T)); + file.read(buffer, POINTDIM * sizeof(T)); + + // read root node + deserialize(file, points, pointtype); + file.close(); + } + + void serialize(std::string filename) { + char buffer[sizeof(T) * 20]; + T *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(T)])); + *ip = POINTDIM; + + file.write(buffer, 5 * sizeof(T) + 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(T)); + + // write root node + serialize(file, *root); + + file.close(); + } + + static PointType readType(std::string filename ) { + char buffer[sizeof(T) * 20]; + + 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 PointType(); + } + + // read header + PointType pointtype = PointType::deserialize(file); + + file.close(); + + return pointtype; + } + + + /** + * Picks the first point in depth first order starting from the given node + * + */ + T* pickPoint(bitoct &node) { + 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 + // absolute pointer + //return &(children->points[1].v); + // offset pointer + return children->getPoints(); + } else { // recurse + return pickPoint(children->node); + } + ++children; // next child + } + } + return 0; + } + + static void childcenter(const T *pcenter, T *ccenter, T size, unsigned char i) { + switch (i) { + case 0: // 000 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 1: // 001 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 2: // 010 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 3: // 011 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] - size / 2.0; + break; + case 4: // 100 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 5: // 101 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] - size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 6: // 110 + ccenter[0] = pcenter[0] - size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + case 7: // 111 + ccenter[0] = pcenter[0] + size / 2.0; + ccenter[1] = pcenter[1] + size / 2.0; + ccenter[2] = pcenter[2] + size / 2.0; + break; + default: + break; + } + } + + static void childcenter(int x, int y, int z, int &cx, int &cy, int &cz, char i, int size) { + switch (i) { + case 0: // 000 + cx = x - size ; + cy = y - size ; + cz = z - size ; + break; + case 1: // 001 + cx = x + size ; + cy = y - size ; + cz = z - size ; + break; + case 2: // 010 + cx = x - size ; + cy = y + size ; + cz = z - size ; + break; + case 3: // 011 + cx = x + size ; + cy = y + size ; + cz = z - size ; + break; + case 4: // 100 + cx = x - size ; + cy = y - size ; + cz = z + size ; + break; + case 5: // 101 + cx = x + size ; + cy = y - size ; + cz = z + size ; + break; + case 6: // 110 + cx = x - size ; + cy = y + size ; + cz = z + size ; + break; + case 7: // 111 + cx = x + size ; + cy = y + size ; + cz = z + size ; + break; + default: + break; + } + } + +protected: + + void AllPoints( bitoct &node, vector &vp) { + 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 + // absolute pointer + //pointrep *points = children->points; + // offset pointer + 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 *p = new T[BOctTree::POINTDIM]; +// T *p = new T[3]; +// p[0] = point[0]; p[1] = point[1]; p[2] = point[2]; + T *p = new T[BOctTree::POINTDIM]; + for (unsigned int k = 0; k < BOctTree::POINTDIM; k++) + p[k] = point[k]; + + vp.push_back(p); + + //glVertex3f( point[0], point[1], point[2]); + point+=BOctTree::POINTDIM; + } + } else { // recurse + AllPoints( children->node, vp); + } + ++children; // next child + } + } + } + + static void deserialize(std::ifstream &f, vector &vpoints, PointType &pointtype) { + char buffer[2]; + pointrep *point = new pointrep[pointtype.getPointDim()]; + f.read(buffer, 2); + bitoct node; + node.valid = buffer[0]; + node.leaf = buffer[1]; + + 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 + pointrep first; + f.read(reinterpret_cast(&first), sizeof(pointrep)); + unsigned int length = first.length; // read first element, which is the length + for (unsigned int k = 0; k < length; k++) { + f.read(reinterpret_cast(point), sizeof(pointrep) * pointtype.getPointDim()); // read the points + vpoints.push_back( pointtype.createPoint( &(point->v ) ) ); + } + } else { // write child + deserialize(f, vpoints, pointtype); + } + } + } + delete [] point; + } + + void deserialize(std::ifstream &f, bitoct &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 + bitunion *children = alloc->allocate >(n_children); + bitoct::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 + pointrep first; + f.read(reinterpret_cast(&first), sizeof(pointrep)); + unsigned int length = first.length; // read first element, which is the length + pointrep *points = alloc->allocate (POINTDIM*length + 1); + // absolute pointer + //children->points = points; + // offset pointer + bitunion::link(children, points); + points[0] = first; + points++; + f.read(reinterpret_cast(points), sizeof(pointrep) * length * POINTDIM); // read the points + } else { // write child + deserialize(f, children->node); + } + ++children; // next child + } + } + } + + void serialize(std::ofstream &of, bitoct &node) { + char buffer[2]; + buffer[0] = node.valid; + buffer[1] = node.leaf; + of.write(buffer, 2); + + + // write children + 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 write points + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + of.write(reinterpret_cast(points), sizeof(pointrep) * (length * POINTDIM +1)); + } else { // write child + serialize(of, children->node); + } + ++children; // next child + } + } + } + + void GetOctTreeCenter(vector&c, bitoct &node, T *center, T size) { + 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 + T * cp = new T[3]; + for (unsigned int iterator = 0; iterator < 3; iterator++) { + cp[iterator] = ccenter[iterator]; + } + c.push_back(cp); + } else { // recurse + GetOctTreeCenter(c, children->node, ccenter, size/2.0); + } + ++children; // next child + } + } + } + + void GetOctTreeRandom(vector&c, bitoct &node) { + 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 + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + // new version to ignore leaves with less than 3 points + /* + if(points[0].length > 2) { + for(int tmp = 0; tmp < points[0].length; tmp++) { + T *point = &(points[POINTDIM*tmp+1].v); + c.push_back(point); + } + } + */ + //old version + + int tmp = rand(points[0].length); + T *point = &(points[POINTDIM*tmp+1].v); + c.push_back(point); + + + } else { // recurse + GetOctTreeRandom(c, children->node); + } + ++children; // next child + } + } + } + + void GetOctTreeRandom(vector&c, unsigned int ptspervoxel, bitoct &node) { + 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 + // absolute pointer + //pointrep *points = children->points; + // offset pointer + pointrep* points = children->getPointreps(); + unsigned int length = points[0].length; + if (ptspervoxel >= length) { + for (unsigned int j = 0; j < length; j++) + c.push_back(&(points[POINTDIM*j+1].v)); + + ++children; // next child + continue; + } + set indices; + while(indices.size() < ptspervoxel) { + int tmp = rand(length-1); + indices.insert(tmp); + } + for(set::iterator it = indices.begin(); it != indices.end(); it++) + c.push_back(&(points[POINTDIM*(*it)+1].v)); + + } else { // recurse + GetOctTreeRandom(c, ptspervoxel, children->node); + } + ++children; // next child + } + } + } + + long countNodes(bitoct &node) { + long result = 0; + 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 + //++result; + } else { // recurse + result += countNodes(children->node) + 1; + } + ++children; // next child + } + } + return result; + } + + long countLeaves(bitoct &node) { + long result = 0; + 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 + long nrpts = children->getLength(); + result += POINTDIM*nrpts; + } else { // recurse + result += countLeaves(children->node); + } + ++children; // next child + } + } + return result; + } + + long countOctLeaves(bitoct &node) { + long result = 0; + 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 + result ++; + } else { // recurse + result += countTrueLeaves(children->node); + } + ++children; // next child + } + } + return result; + } + + // TODO: is this still needed? nodes and pointreps are all in the Allocator + void deletetNodes(bitoct &node) { + bitunion *children; + bitoct::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 + // absolute pointer + //delete [] children->points; + // offset pointer + delete [] children->getPointreps(); + } else { // recurse + deletetNodes(children->node); + } + ++children; // next child + haschildren = true; + } + } + // delete children + if (haschildren) { + bitoct::getChildren(node, children); + } + } + + template + void* branch( bitoct &node, P * const * splitPoints, int n, T _center[3], T _size) { + // if bucket is too small stop building tree + // ----------------------------------------- + if ((_size <= voxelSize) || (earlystop && n <= 10) ) { + + // copy points + pointrep *points = alloc->allocate (POINTDIM*n + 1); + + points[0].length = n; + int i = 1; + for (int j = 0; j < n; j++) { + for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) { + points[i++].v = splitPoints[j][iterator]; + } + } + return points; + } + + // calculate new buckets + T newcenter[8][3]; + T sizeNew; + + sizeNew = _size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(_center, newcenter[i], _size, i); + } + + countPointsAndQueueFast(splitPoints, n, newcenter, sizeNew, node, _center); + return 0; + } + + template + void* branch( bitoct &node, vector &splitPoints, T _center[3], T _size) { + // if bucket is too small stop building tree + // ----------------------------------------- + if ((_size <= voxelSize) || (earlystop && splitPoints.size() <= 10) ) { + // copy points + pointrep *points = alloc->allocate (POINTDIM*splitPoints.size() + 1); + points[0].length = splitPoints.size(); + int i = 1; + for (typename vector

::iterator itr = splitPoints.begin(); + itr != splitPoints.end(); itr++) { + for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) { + points[i++].v = (*itr)[iterator]; + } + } + return points; + } + + // calculate new buckets + T newcenter[8][3]; + T sizeNew; + + sizeNew = _size / 2.0; + + for (unsigned char i = 0; i < 8; i++) { + childcenter(_center, newcenter[i], _size, i); + } + + countPointsAndQueue(splitPoints, newcenter, sizeNew, node, _center); + return 0; + } + + template + void countPointsAndQueue(vector &i_points, T center[8][3], T size, bitoct &parent, T *pcenter) { + vector points[8]; + int n_children = 0; + for (typename vector

::iterator itr = i_points.begin(); itr != i_points.end(); itr++) { + points[childIndex

(pcenter, *itr)].push_back( *itr ); + } + + i_points.clear(); + vector().swap(i_points); + for (int j = 0; j < 8; j++) { + if (!points[j].empty()) { + parent.valid = ( 1 << j ) | parent.valid; + ++n_children; + } + } + // create children + bitunion *children = alloc->allocate >(n_children); + bitoct::link(parent, children); + + int count = 0; + for (int j = 0; j < 8; j++) { + if (!points[j].empty()) { + pointrep *c = (pointrep*)branch(children[count].node, points[j], center[j], size); // leaf node + if (c) { + // absolute pointer + //children[count].points = c; // set this child to deque of points + // offset pointer + bitunion::link(&children[count], c); + parent.leaf = ( 1 << j ) | parent.leaf; // remember this is a leaf + } + points[j].clear(); + vector().swap(points[j]); + ++count; + } + } + } + + template + void countPointsAndQueueFast(P * const* points, int n, T center[8][3], T size, bitoct &parent, T pcenter[3]) { + P * const *blocks[9]; + blocks[0] = points; + blocks[8] = points + n; + fullsort(points, n, pcenter, blocks+1); + + int n_children = 0; + + for (int j = 0; j < 8; j++) { + // if non-empty set valid flag for this child + if (blocks[j+1] - blocks[j] > 0) { + parent.valid = ( 1 << j ) | parent.valid; + ++n_children; + } + } + + // create children + bitunion *children = alloc->allocate >(n_children); + bitoct::link(parent, children); + int count = 0; + for (int j = 0; j < 8; j++) { + if (blocks[j+1] - blocks[j] > 0) { + pointrep *c = (pointrep*)branch(children[count].node, blocks[j], blocks[j+1] - blocks[j], center[j], size); // leaf node + if (c) { + // absolute pointer + //children[count].points = c; // set this child to vector of points + // offset pointer + bitunion::link(&children[count], c); // set this child to vector of points + parent.leaf = ( 1 << j ) | parent.leaf; // remember this is a leaf + } + ++count; + } + } + } + + + void getByIndex(T *point, T *&points, unsigned int &length) { + unsigned int x,y,z; + x = (point[0] + add[0]) * mult; + y = (point[1] + add[1]) * mult; + z = (point[2] + add[2]) * mult; + + bitunion *node = uroot; + unsigned char child_index; + unsigned int child_bit; + unsigned int depth = 0; + + while (true) { + child_bit = child_bit_depth[depth]; + child_index = ((x & child_bit )!=0) | (((y & child_bit )!=0 )<< 1) | (((z & child_bit )!=0) << 2); + if (node->childIsLeaf(child_index) ) { + node = node->getChild(child_index); + points = node->getPoints(); + length = node->getLength(); + return; + } else { + node = node->getChild(child_index); + } + depth++; + } + } + + template + inline unsigned char childIndex(const T *center, const P *point) { + return (point[0] > center[0] ) | ((point[1] > center[1] ) << 1) | ((point[2] > center[2] ) << 2) ; + } + + /** + * Given a leaf node, this function looks for the closest point to params[threadNum].closest + * in the list of points. + */ + inline void findClosestInLeaf(bitunion *node, int threadNum) const { + if (params[threadNum].count >= params[threadNum].max_count) return; + params[threadNum].count++; + T* points = node->getPoints(); + unsigned int length = node->getLength(); + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + double myd2 = Dist2(params[threadNum].p, points); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = points; + if (myd2 <= 0.0001) { + params[threadNum].closest_v = 0; // the search radius in units of voxelSize + } else { + params[threadNum].closest_v = sqrt(myd2) * mult + 1; // the search radius in units of voxelSize + } + } + points+=BOctTree::POINTDIM; + } + } + + + +/** + * This function finds the closest point in the octree given a specified + * radius. This implementation is quit complex, although it is already + * simplified. The simplification incurs a significant loss in speed, as + * several calculations have to be performed repeatedly and a high number of + * unnecessary jumps are executed. + */ + double *FindClosest(double *point, double maxdist2, int threadNum) const + { + params[threadNum].closest = 0; // no point found currently + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = point; + params[threadNum].x = (point[0] + add[0]) * mult; + params[threadNum].y = (point[1] + add[1]) * mult; + params[threadNum].z = (point[2] + add[2]) * mult; + params[threadNum].closest_v = sqrt(maxdist2) * mult + 1; // the search radius in units of voxelSize + params[threadNum].count = 0; + params[threadNum].max_count = 10000; // stop looking after this many buckets + + + // box within bounds in voxel coordinates + int xmin, ymin, zmin, xmax, ymax, zmax; + xmin = max(params[threadNum].x-params[threadNum].closest_v, 0); + ymin = max(params[threadNum].y-params[threadNum].closest_v, 0); + zmin = max(params[threadNum].z-params[threadNum].closest_v, 0); + +// int largest_index = child_bit_depth[0] * 2 -1; + + xmax = min(params[threadNum].x+params[threadNum].closest_v, largest_index); + ymax = min(params[threadNum].y+params[threadNum].closest_v, largest_index); + zmax = min(params[threadNum].z+params[threadNum].closest_v, largest_index); + + unsigned char depth = 0; + unsigned int child_bit; + unsigned int child_index_min; + unsigned int child_index_max; + + bitunion *node = &(*uroot); + + int cx, cy, cz; + + child_bit = child_bit_depth[depth]; + cx = child_bit_depth[depth]; + cy = child_bit_depth[depth]; + cz = child_bit_depth[depth]; + + while (true) { // find the first node where branching is required + child_index_min = ((xmin & child_bit )!=0) | (((ymin & child_bit )!=0 )<< 1) | (((zmin & child_bit )!=0) << 2); + child_index_max = ((xmax & child_bit )!=0) | (((ymax & child_bit )!=0 )<< 1) | (((zmax & child_bit )!=0) << 2); + + // if these are the same, go there + // TODO: optimization: also traverse if only single child... + if (child_index_min == child_index_max) { + if (node->childIsLeaf(child_index_min) ) { // luckily, no branching is required + findClosestInLeaf(node->getChild(child_index_min), threadNum); + return static_cast(params[threadNum].closest); + } else { + if (node->isValid(child_index_min) ) { // only descend when there is a child + childcenter(cx,cy,cz, cx,cy,cz, child_index_min, child_bit/2 ); + node = node->getChild(child_index_min); + child_bit /= 2; + } else { // there is no child containing the bounding box => no point is close enough + return 0; + } + } + } else { + // if min and max are not in the same child we must branch + break; + } + } + + // node contains all box-within-bounds cells, now begin best bin first search + _FindClosest(threadNum, node->node, child_bit/2, cx, cy, cz); + return static_cast(params[threadNum].closest); + } + + /** + * This is the heavy duty search function doing most of the (theoretically unneccesary) work. The tree is recursively searched. + * Depending on which of the 8 child-voxels is closer to the query point, the children are examined in a special order. + * This order is defined in map, imap is its inverse and sequence2ci is a speedup structure for faster access to the child indices. + */ + void _FindClosest(int threadNum, bitoct &node, int size, int x, int y, int z) const + { + // Recursive case + + // compute which child is closest to the query point + unsigned char child_index = ((params[threadNum].x - x) >= 0) | + (((params[threadNum].y - y) >= 0) << 1) | + (((params[threadNum].z - z) >= 0) << 2); + + char *seq2ci = sequence2ci[child_index][node.valid]; // maps preference to index in children array + char *mmap = amap[child_index]; // maps preference to area index + + bitunion *children; + bitoct::getChildren(node, children); + int cx, cy, cz; + cx = cy = cz = 0; // just to shut up the compiler warnings + for (unsigned char i = 0; i < 8; i++) { // in order of preference + child_index = mmap[i]; // the area index of the node + if ( ( 1 << child_index ) & node.valid ) { // if ith node exists + childcenter(x,y,z, cx,cy,cz, child_index, size); + if ( params[threadNum].closest_v == 0 || max(max(abs( cx - params[threadNum].x ), + abs( cy - params[threadNum].y )), + abs( cz - params[threadNum].z )) - size + > params[threadNum].closest_v ) { + continue; + } + // find the closest point in leaf seq2ci[i] + if ( ( 1 << child_index ) & node.leaf ) { // if ith node is leaf + findClosestInLeaf( &children[seq2ci[i]], threadNum); + } else { // recurse + _FindClosest(threadNum, children[seq2ci[i]].node, size/2, cx, cy, cz); + } + } + } + } + + + /** + * This function shows the possible speedup that can be gained by using the + * octree for nearest neighbour search, if a more sophisticated + * implementation were given. Here, only the bucket in which the query point + * falls is looked up. If doing the same thing in the kd-tree search, this + * function is about 3-5 times as fast + */ + double *FindClosestInBucket(double *point, double maxdist2, int threadNum) { + params[threadNum].closest = 0; + params[threadNum].closest_d2 = maxdist2; + params[threadNum].p = point; + unsigned int x,y,z; + x = (point[0] + add[0]) * mult; + y = (point[1] + add[1]) * mult; + z = (point[2] + add[2]) * mult; + T * points; + unsigned int length; + + bitunion *node = uroot; + unsigned char child_index; + + unsigned int child_bit = child_bit_depth[0]; + + while (true) { + child_index = ((x & child_bit )!=0) | (((y & child_bit )!=0 )<< 1) | (((z & child_bit )!=0) << 2); + if (node->childIsLeaf(child_index) ) { + node = node->getChild(child_index); + points = node->getPoints(); + length = node->getLength(); + + for(unsigned int iterator = 0; iterator < length; iterator++ ) { + double myd2 = Dist2(params[threadNum].p, points); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = points; + } + points+=BOctTree::POINTDIM; + } + return static_cast(params[threadNum].closest); + } else { + if (node->isValid(child_index) ) { + node = node->getChild(child_index); + } else { + return 0; + } + } + child_bit >>= 1; + } + return static_cast(params[threadNum].closest); + } + + +template + void fullsort(P * const * points, int n, T splitval[3], P * const * blocks[9]) { + P* const * L0; + P* const * L1; + P* const * L2; + unsigned int n0L, n0R, n1L, n1R ; + + // sort along Z + L0 = sort(points, n, splitval[2], 2); + + n0L = L0 - points; + // sort along Y (left of Z) points -- L0 + L1 = sort(points, n0L, splitval[1], 1); + + n1L = L1 - points; + // sort along X (left of Y) points -- L1 + L2 = sort(points, n1L, splitval[0], 0); + + blocks[0] = L2; + + n1R = n0L - n1L; + // sort along X (right of Y) // L1 -- L0 + L2 = sort(L1, n1R, splitval[0], 0); + + blocks[1] = L1; + blocks[2] = L2; + + n0R = n - n0L; + // sort along Y (right of Z) L0 -- end + L1 = sort(L0, n0R, splitval[1], 1); + + n1L = L1 - L0; + // sort along X (left of Y) points -- L1 + L2 = sort(L0, n1L, splitval[0], 0); + + blocks[3] = L0; + blocks[4] = L2; + + n1R = n0R - n1L; + // sort along X (right of Y) // L1 -- L0 + L2 = sort(L1, n1R, splitval[0], 0); + + blocks[5] = L1; + blocks[6] = L2; + } + + + template + P* const * sort(P* const * points, unsigned int n, T splitval, unsigned char index) { + if (n==0) return points; + + if (n==1) { + if (points[0][index] < splitval) + return points+1; + else + return points; + } + + P **left = const_cast(points); + P **right = const_cast(points + n - 1); + + + while (1) { + while ((*left)[index] < splitval) + { + left++; + if (right < left) + break; + } + while ((*right)[index] >= splitval) + { + right--; + if (right < left) + break; + } + if (right < left) + break; + + std::swap(*left, *right); + } + return left; + } + +public: + /** + * Copies another (via new constructed) octtree into cache allocated memory and makes it position independant + */ + BOctTree(const BOctTree& other, unsigned char* mem_ptr, unsigned int mem_max) + { + alloc = new SequentialAllocator(mem_ptr, mem_max); + + // "allocate" space for *this + alloc->allocate >(); + + // take members + unsigned int i; + for(i = 0; i < 3; ++i) + center[i] = other.center[i]; + size = other.size; + voxelSize = other.voxelSize; + real_voxelSize = other.real_voxelSize; + for(i = 0; i < 3; ++i) + add[i] = other.add[i]; + mult = other.mult; + POINTDIM = other.POINTDIM; + mins = alloc->allocate(POINTDIM); + maxs = alloc->allocate(POINTDIM); + for(i = 0; i < POINTDIM; ++i) { + mins[i] = other.mins[i]; + maxs[i] = other.maxs[i]; + } + pointtype = other.pointtype; + max_depth = other.max_depth; + child_bit_depth = alloc->allocate(max_depth); + child_bit_depth_inv = alloc->allocate(max_depth); + for(i = 0; i < max_depth; ++i) { + child_bit_depth[i] = other.child_bit_depth[i]; + child_bit_depth_inv[i] = other.child_bit_depth_inv[i]; + } + largest_index = other.largest_index; + + // take node structure + uroot = alloc->allocate >(); + root = &uroot->node; + copy_children(*other.root, *root); + + // test if allocator has used up his space + //alloc->printSize(); + + // discard allocator, space is managed by the cache manager + delete alloc; alloc = 0; + } + +private: + void copy_children(const bitoct& other, bitoct& my) { + // copy node attributes + my.valid = other.valid; + my.leaf = other.leaf; + + // other children + bitunion* other_children; + bitoct::getChildren(other, other_children); + + // create own children + unsigned int n_children = POPCOUNT(other.valid); + bitunion* my_children = alloc->allocate >(n_children); + bitoct::link(my, my_children); + + // iterate over all (valid) children and copy them + for(unsigned int i = 0; i < 8; ++i) { + if((1<getLength(); + pointrep* other_pointreps = other_children->getPointreps(); + pointrep* my_pointreps = alloc->allocate(POINTDIM * length + 1); + for(unsigned int j = 0; j < POINTDIM * length + 1; ++j) + my_pointreps[j] = other_pointreps[j]; + // assign + bitunion::link(my_children, my_pointreps); + } else { + // child is already created, copy and create children for it + copy_children(other_children->node, my_children->node); + } + ++other_children; + ++my_children; + } + } + } + +public: + //! Size of the whole tree structure, including the main class, its serialize critical allocated variables and nodes, not the allocator + unsigned int getMemorySize() + { + return sizeof(*this) // all member variables + + 2*POINTDIM*sizeof(T) // mins, maxs + + 2*max_depth*sizeof(unsigned int) // child_bit_depth(_inv) + + sizeof(bitunion) // uroot + + sizeChildren(*root); // all nodes + } + +private: + //! Recursive size of a node's children + unsigned int sizeChildren(const bitoct& node) { + unsigned int s = 0; + bitunion* children; + bitoct::getChildren(node, children); + + // size of children allocation + unsigned int n_children = POPCOUNT(node.valid); + s += sizeof(bitunion)*n_children; + + // iterate over all (valid) children and sum them up + for(unsigned int i = 0; i < 8; ++i) { + if((1<getLength()*POINTDIM+1); + } else { + // childe node is already accounted for, add its children + s += sizeChildren(children->node); + } + ++children; // next (valid) child + } + } + return s; + } +}; + +typedef SingleObject > DataOcttree; + +template +NNParams BOctTree::params[100]; + +#endif diff --git a/.svn/pristine/58/581dfdd05052a00820b477e1102e80e2e4f6a91e.svn-base b/.svn/pristine/58/581dfdd05052a00820b477e1102e80e2e4f6a91e.svn-base new file mode 100644 index 0000000..be5ba8e --- /dev/null +++ b/.svn/pristine/58/581dfdd05052a00820b477e1102e80e2e4f6a91e.svn-base @@ -0,0 +1,432 @@ +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) + +## Normals + +OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF) + +IF(WITH_NORMALS) + MESSAGE(STATUS "With normals") +ELSE(WITH_NORMALS) + MESSAGE(STATUS "Without normals") +ENDIF(WITH_NORMALS) + +## 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/normals) +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/6b/6bd98969a6c7812a27bc688fa80f43b4cfb3f059.svn-base b/.svn/pristine/6b/6bd98969a6c7812a27bc688fa80f43b4cfb3f059.svn-base new file mode 100644 index 0000000..da9f414 --- /dev/null +++ b/.svn/pristine/6b/6bd98969a6c7812a27bc688fa80f43b4cfb3f059.svn-base @@ -0,0 +1,752 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include +#include "newmat/newmat.h" +#include "newmat/newmatap.h" + +using namespace NEWMAT; + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// 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, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals 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") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + 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 << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &normals,vector &points, int k, const double _rPos[3] ) +{ + cout<<"Total number of points: "< &normals,vector &points, + int kmin, int kmax, const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) + break; + } + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1rd column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } +} + +/////////////////////////////////////////////////////// +/////////////NORMALS USING IMAGE NEIGHBORS //////////// +/////////////////////////////////////////////////////// +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< neighbors; + for (size_t i=0; i< extendedMap.size(); i++) + { + for (size_t j=0; j= 0 && x < (int)extendedMap.size() && + y >= 0 && y < (int)extendedMap[x].size()) { + for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { + neighbors.push_back(extendedMap[x][y][k]); + } + } + } + + nr_neighbors = neighbors.size(); + cv::Vec3f p = extendedMap[i][j][0]; + + //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero + if (nr_neighbors < 3) + { + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(0.0,0.0,0.0)); + continue; + } + + //calculate mean for all the points + Matrix X(nr_neighbors,3); + SymmetricMatrix A(3); + Matrix U(3,3); + DiagonalMatrix D(3); + + //calculate mean for all the points + for(int k = 0; k < nr_neighbors; k++) + { + cv::Vec3f pp = neighbors[k]; + mean.x += pp[0]; + mean.y += pp[1]; + mean.z += pp[2]; + } + + mean.x /= nr_neighbors; + mean.y /= nr_neighbors; + mean.z /= nr_neighbors; + //calculate covariance = A for all the points + for (int i = 0; i < nr_neighbors; ++i) { + cv::Vec3f pp = neighbors[i]; + X(i+1, 1) = pp[0] - mean.x; + X(i+1, 2) = pp[1] - mean.y; + X(i+1, 3) = pp[2] - mean.z; + } + + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + + for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { + cv::Vec3f p = extendedMap[i][j][k]; + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(n(1), n(2), n(3))); + } + } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE ////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////// +/* + void calculateNormalsFAST(vector &normals,vector &points,cv::Mat &img,vector>> extendedMap) + { + cout<<"Total number of points: "<(i-1,j); + dRdPhi += 3 *img.at(i-1,j-1); + dRdPhi += 3 *img.at(i-1,j+1); + dRdPhi -= 10*img.at(i+1,j); + dRdPhi -= 3 *img.at(i+1,j-1); + dRdPhi -= 3 *img.at(i+1,j+1); + + dRdTheta += 10*img.at(i,j-1); + dRdTheta += 3 *img.at(i-1,j-1); + dRdTheta += 3 *img.at(i+1,j-1); + dRdTheta -= 10*img.at(i,j+1); + dRdTheta -= 3 *img.at(i-1,j+1); + dRdTheta -= 3 *img.at(i+1,j+1); + + n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) + + cos(theta) * cos(phi) * dRdPhi / rho; + + n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) + + sin(theta) * cos(phi) * dRdPhi / rho; + + n[2] = cos(phi) - sin(phi) * dRdPhi / rho; + + //n[2] = -n[2]; + + m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]); + n[0] /= m; n[1] /= m; n[2] /= m; + + points.push_back(Point(x, y, z)); + normals.push_back(Point(n[0],n[1],n[2])); + } + } + } + } +*/ + +/* + * 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]; + if(xyz_reflectance.size() != 0) + { + 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; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++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; +} +/// Write a pose file with the specofied name +void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber) +{ + string poseFileName = dir + "/scan" + to_string(scanNumber, 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, vector &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + 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()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsAKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + else + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} + diff --git a/.svn/pristine/91/91b33c2ba6ec5bce5614c2794ae7e5510bcb2635.svn-base b/.svn/pristine/91/91b33c2ba6ec5bce5614c2794ae7e5510bcb2635.svn-base new file mode 100644 index 0000000..dd04f6b --- /dev/null +++ b/.svn/pristine/91/91b33c2ba6ec5bce5614c2794ae7e5510bcb2635.svn-base @@ -0,0 +1,97 @@ +/** + * @file + * @brief Representation of a general search trees + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#ifndef __SEARCHTREE_H__ +#define __SEARCHTREE_H__ + +#include +using std::vector; + +#include "ptpair.h" +#include "data_types.h" + +/** + * @brief The tree structure + * + * A tree holds the pointer to the data + **/ +class Tree { +public: + /** + * Destructor - deletes the tree + * pure virtual, i.e., must be implented by a derived class + */ + virtual inline ~Tree() {}; +}; + + +class Scan; +/** + * @brief The search tree structure + * + * A search tree holds the pointer to the data. + * Furthermore, search functionality must be privided + **/ +class SearchTree : public Tree { + friend class Scan; +public: + /** + * Constructor (default) + */ + inline SearchTree() {}; + + /** + * Constructor - Constructs a tree from the input. + * must be implented by a derived class + * @param pts 3D array of points + * @param n number of points + */ + SearchTree(double **pts, int n); + + /** + * Destructor - deletes the tree + * virtual, i.e., must be implented by a derived class + */ + virtual inline ~SearchTree() {}; + + /** + * Aquire a lock on this tree, signaling that its resources are in use. Neccessary for cached data in the scanserver. + */ + virtual inline void lock() {}; + + /** + * Release the lock on this tree, signaling that its resources are aren't in use anymore. Neccessary for cached data in the scanserver. + */ + virtual inline void unlock() {}; + + /** + * This Search function returns a pointer to the closest point + * of the query point within maxdist2. If there if no such point + * a 0-pointer might be returned. + * + * @param _p Pointer to query point + * @param maxdist2 Maximal distance for closest points + * @param threadNum If parallel threads share the search tree the thread num must be given + * @return Pointer to closest point + */ + virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0 ; + + virtual void getPtPairs(vector *pairs, + double *source_alignxf, + double * const *q_points, unsigned int startindex, unsigned int endindex, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d); + + virtual void getPtPairs(vector *pairs, + double *source_alignxf, + const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex, + int thread_num, + int rnd, double max_dist_match2, double &sum, + double *centroid_m, double *centroid_d); +}; + +#endif diff --git a/.svn/pristine/b7/b73ed4ce091543af79171dc5e4ec687a58452594.svn-base b/.svn/pristine/b7/b73ed4ce091543af79171dc5e4ec687a58452594.svn-base new file mode 100644 index 0000000..50004dc --- /dev/null +++ b/.svn/pristine/b7/b73ed4ce091543af79171dc5e4ec687a58452594.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/cf/cff7dee6d33280a94d73d3570a8c394a2e3a27b7.svn-base b/.svn/pristine/cf/cff7dee6d33280a94d73d3570a8c394a2e3a27b7.svn-base new file mode 100644 index 0000000..2ad9074 --- /dev/null +++ b/.svn/pristine/cf/cff7dee6d33280a94d73d3570a8c394a2e3a27b7.svn-base @@ -0,0 +1,7 @@ +IF(WITH_NORMALS) + add_executable(normals normals.cc) + + FIND_PACKAGE(OpenCV REQUIRED) + + target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS}) +ENDIF(WITH_NORMALS) diff --git a/.svn/wc.db b/.svn/wc.db index d2fcbbb..2df87e6 100644 Binary files a/.svn/wc.db and b/.svn/wc.db differ diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cb9ffd..be5ba8e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -175,7 +175,18 @@ ELSE(WITH_SEGMENTATION) MESSAGE(STATUS "Without segmentation") ENDIF(WITH_SEGMENTATION) +## Normals + +OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF) + +IF(WITH_NORMALS) + MESSAGE(STATUS "With normals") +ELSE(WITH_NORMALS) + MESSAGE(STATUS "Without normals") +ENDIF(WITH_NORMALS) + ## CAD matching + OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF) IF (WITH_CAD) @@ -380,6 +391,7 @@ add_subdirectory(src/slam6d) add_subdirectory(src/scanio) add_subdirectory(src/scanserver) add_subdirectory(src/segmentation) +add_subdirectory(src/normals) add_subdirectory(src/veloslam) add_subdirectory(src/show) add_subdirectory(src/grid) diff --git a/include/show/show_Boctree.h b/include/show/show_Boctree.h index a9e687a..50004dc 100644 --- a/include/show/show_Boctree.h +++ b/include/show/show_Boctree.h @@ -94,7 +94,7 @@ public: //! 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); } diff --git a/include/slam6d/Boctree.h b/include/slam6d/Boctree.h index f2bfc55..0d1f3c0 100644 --- a/include/slam6d/Boctree.h +++ b/include/slam6d/Boctree.h @@ -226,6 +226,9 @@ public: this->POINTDIM = pointtype.getPointDim(); + //@@@ + cout << "POINTDIM" << this->POINTDIM << endl; + mins = alloc->allocate(POINTDIM); maxs = alloc->allocate(POINTDIM); diff --git a/include/slam6d/d2tree.icc b/include/slam6d/d2tree.icc deleted file mode 100644 index 88bc0b0..0000000 --- a/include/slam6d/d2tree.icc +++ /dev/null @@ -1,42 +0,0 @@ - -inline void D2Tree::calcDivide(double x_size, double y_size, double z_size, - double center[3], - double &x_sizeNew, double &y_sizeNew, double &z_sizeNew, - double newcenter[8][3]) -{ - x_sizeNew = x_size * 0.5; - y_sizeNew = y_size * 0.5; - z_sizeNew = z_size * 0.5; - - newcenter[0][0] = center[0] - x_sizeNew; - newcenter[0][1] = center[1] - y_sizeNew; - newcenter[0][2] = center[2] - z_sizeNew; - - newcenter[1][0] = center[0] + x_sizeNew; - newcenter[1][1] = center[1] - y_sizeNew; - newcenter[1][2] = center[2] - z_sizeNew; - - newcenter[2][0] = center[0] - x_sizeNew; - newcenter[2][1] = center[1] + y_sizeNew; - newcenter[2][2] = center[2] - z_sizeNew; - - newcenter[3][0] = center[0] - x_sizeNew; - newcenter[3][1] = center[1] - y_sizeNew; - newcenter[3][2] = center[2] + z_sizeNew; - - newcenter[4][0] = center[0] + x_sizeNew; - newcenter[4][1] = center[1] + y_sizeNew; - newcenter[4][2] = center[2] - z_sizeNew; - - newcenter[5][0] = center[0] + x_sizeNew; - newcenter[5][1] = center[1] - y_sizeNew; - newcenter[5][2] = center[2] + z_sizeNew; - - newcenter[6][0] = center[0] - x_sizeNew; - newcenter[6][1] = center[1] + y_sizeNew; - newcenter[6][2] = center[2] + z_sizeNew; - - newcenter[7][0] = center[0] + x_sizeNew; - newcenter[7][1] = center[1] + y_sizeNew; - newcenter[7][2] = center[2] + z_sizeNew; -} diff --git a/include/slam6d/kdcache.h b/include/slam6d/kdcache.h deleted file mode 100644 index d0039bd..0000000 --- a/include/slam6d/kdcache.h +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @file - * @brief Representation of the cache for cached k-d tree search. - * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. - * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. - */ - -#ifndef __KD_CACHE_H__ -#define __KD_CACHE_H__ - -#include "searchCache.h" -#include "kdparams.h" - -// just a prototype -class KDtree_cache; -class Scan; - -/** - * @brief cache item - * - * The return value of the cached k-d tree - * is a bundle of the closest point and - * a pointer to the leaf. - **/ -class KDCacheItem : public SearchTreeCacheItem { -public: - KDCacheItem() { node = 0; }; - KDParams param; - KDtree_cache *node; -}; - -/** - * @brief cache - * - * The cache consists of an array of KDCacheItems - * and two Scan numbers. - **/ -class KDCache { -public: -// KDCache() { SourceScanNr = TargetScanNr = 0; item = 0; }; - KDCache() { target = 0; item = 0; }; - KDCacheItem *item; // array of items -// unsigned int SourceScanNr; -// unsigned int TargetScanNr; - Scan const * target; -}; - -#endif - - diff --git a/include/slam6d/searchCache.h b/include/slam6d/searchCache.h deleted file mode 100644 index 03f24ac..0000000 --- a/include/slam6d/searchCache.h +++ /dev/null @@ -1,22 +0,0 @@ -/** - * @file - * @brief Representation of a general cache for search trees - * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. - */ - -#ifndef __SEARCHCACHE_H__ -#define __SEARCHCACHE_H__ - -/** - * @brief The general Cache Item - * - * This class contains a general cache item - */ -class SearchTreeCacheItem -{ - public: - virtual ~SearchTreeCacheItem() {}; -}; - - -#endif diff --git a/include/slam6d/searchTree.h b/include/slam6d/searchTree.h index e59f0a6..dd04f6b 100644 --- a/include/slam6d/searchTree.h +++ b/include/slam6d/searchTree.h @@ -10,7 +10,6 @@ #include using std::vector; -#include "searchCache.h" #include "ptpair.h" #include "data_types.h" @@ -95,61 +94,4 @@ public: double *centroid_m, double *centroid_d); }; - -/** - * @brief The search tree structure - * - * A search tree holds the pointer to the data. - * Furthermore, search functionality must be privided - **/ -class CachedSearchTree : public SearchTree { -public: - /** - * Constructor (default) - */ - inline CachedSearchTree() {}; - - /** - * Constructor - Constructs a tree from the input. - * pure virtual, i.e., must be implented by a derived class - * @param pts 3D array of points - * @param n number of points - */ - CachedSearchTree(double **pts, int n, CachedSearchTree *_parent = 0); - - /** - * Destructor - deletes the tree - * pure virtual, i.e., must be implented by a derived class - */ - virtual inline ~CachedSearchTree() {}; - - /** - * This Search function returns a pointer to the closest point - * of the query point within maxdist2. This function searches from the - * root to the leafs. - * - * @param _p Pointer to query point - * @param maxdist2 Maximal distance for closest points - * @param threadNum If parallel threads share the search tree the thread num must be given - * @return A new cach Item - */ - virtual SearchTreeCacheItem* FindClosestCacheInit(double *_p, double maxdist2, int threadNum = 0) = 0; - - /** - * This Search function returns a pointer to the closest point - * of the query point within maxdist2. This function might be started - * from the leafs. - * - * @param _p Pointer to query point - * @param maxdist2 Maximal distance for closest points - * @param threadNum If parallel threads share the search tree the thread num must be given - * @return A new cach Item - */ - virtual SearchTreeCacheItem* FindClosestCache(double *_p, double maxdist2, int threadNum = 0) = 0; - double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const { - return 0; - } -}; - - #endif diff --git a/src/normals/CMakeLists.txt b/src/normals/CMakeLists.txt new file mode 100755 index 0000000..2ad9074 --- /dev/null +++ b/src/normals/CMakeLists.txt @@ -0,0 +1,7 @@ +IF(WITH_NORMALS) + add_executable(normals normals.cc) + + FIND_PACKAGE(OpenCV REQUIRED) + + target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS}) +ENDIF(WITH_NORMALS) diff --git a/src/normals/normals.cc b/src/normals/normals.cc new file mode 100644 index 0000000..da9f414 --- /dev/null +++ b/src/normals/normals.cc @@ -0,0 +1,752 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include +#include "newmat/newmat.h" +#include "newmat/newmatap.h" + +using namespace NEWMAT; + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// 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, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals 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") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + 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 << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/calculateNormals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &normals,vector &points, int k, const double _rPos[3] ) +{ + cout<<"Total number of points: "< &normals,vector &points, + int kmin, int kmax, const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) + break; + } + + //normal = eigenvector corresponding to lowest + //eigen value that is the 1rd column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + normals.push_back(Point(n(1), n(2), n(3))); + } +} + +/////////////////////////////////////////////////////// +/////////////NORMALS USING IMAGE NEIGHBORS //////////// +/////////////////////////////////////////////////////// +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]) +{ + ColumnVector rPos(3); + for (int i = 0; i < 3; ++i) + rPos(i+1) = _rPos[i]; + + cout<<"Total number of points: "< neighbors; + for (size_t i=0; i< extendedMap.size(); i++) + { + for (size_t j=0; j= 0 && x < (int)extendedMap.size() && + y >= 0 && y < (int)extendedMap[x].size()) { + for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { + neighbors.push_back(extendedMap[x][y][k]); + } + } + } + + nr_neighbors = neighbors.size(); + cv::Vec3f p = extendedMap[i][j][0]; + + //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero + if (nr_neighbors < 3) + { + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(0.0,0.0,0.0)); + continue; + } + + //calculate mean for all the points + Matrix X(nr_neighbors,3); + SymmetricMatrix A(3); + Matrix U(3,3); + DiagonalMatrix D(3); + + //calculate mean for all the points + for(int k = 0; k < nr_neighbors; k++) + { + cv::Vec3f pp = neighbors[k]; + mean.x += pp[0]; + mean.y += pp[1]; + mean.z += pp[2]; + } + + mean.x /= nr_neighbors; + mean.y /= nr_neighbors; + mean.z /= nr_neighbors; + //calculate covariance = A for all the points + for (int i = 0; i < nr_neighbors; ++i) { + cv::Vec3f pp = neighbors[i]; + X(i+1, 1) = pp[0] - mean.x; + X(i+1, 2) = pp[1] - mean.y; + X(i+1, 3) = pp[2] - mean.z; + } + + A << 1.0/nr_neighbors * X.t() * X; + + EigenValues(A, D, U); + //normal = eigenvector corresponding to lowest + //eigen value that is the 1st column of matrix U + ColumnVector n(3); + n(1) = U(1,1); + n(2) = U(2,1); + n(3) = U(3,1); + ColumnVector point_vector(3); + point_vector(1) = p[0] - rPos(1); + point_vector(2) = p[1] - rPos(2); + point_vector(3) = p[2] - rPos(3); + point_vector = point_vector / point_vector.NormFrobenius(); + Real angle = (n.t() * point_vector).AsScalar(); + if (angle < 0) { + n *= -1.0; + } + n = n / n.NormFrobenius(); + + for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { + cv::Vec3f p = extendedMap[i][j][k]; + points.push_back(Point(p[0], p[1], p[2])); + normals.push_back(Point(n(1), n(2), n(3))); + } + } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE ////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////// +/* + void calculateNormalsFAST(vector &normals,vector &points,cv::Mat &img,vector>> extendedMap) + { + cout<<"Total number of points: "<(i-1,j); + dRdPhi += 3 *img.at(i-1,j-1); + dRdPhi += 3 *img.at(i-1,j+1); + dRdPhi -= 10*img.at(i+1,j); + dRdPhi -= 3 *img.at(i+1,j-1); + dRdPhi -= 3 *img.at(i+1,j+1); + + dRdTheta += 10*img.at(i,j-1); + dRdTheta += 3 *img.at(i-1,j-1); + dRdTheta += 3 *img.at(i+1,j-1); + dRdTheta -= 10*img.at(i,j+1); + dRdTheta -= 3 *img.at(i-1,j+1); + dRdTheta -= 3 *img.at(i+1,j+1); + + n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) + + cos(theta) * cos(phi) * dRdPhi / rho; + + n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) + + sin(theta) * cos(phi) * dRdPhi / rho; + + n[2] = cos(phi) - sin(phi) * dRdPhi / rho; + + //n[2] = -n[2]; + + m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]); + n[0] /= m; n[1] /= m; n[2] /= m; + + points.push_back(Point(x, y, z)); + normals.push_back(Point(n[0],n[1],n[2])); + } + } + } + } +*/ + +/* + * 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]; + if(xyz_reflectance.size() != 0) + { + 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; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++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; +} +/// Write a pose file with the specofied name +void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber) +{ + string poseFileName = dir + "/scan" + to_string(scanNumber, 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, vector &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + 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()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsAKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + else + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} + diff --git a/src/show/show_common.cc b/src/show/show_common.cc index 3b45148..f160e05 100644 --- a/src/show/show_common.cc +++ b/src/show/show_common.cc @@ -1043,6 +1043,7 @@ void initShow(int argc, char **argv){ #if !defined USE_COMPACT_TREE // show structures // associate show octtree with the scan and hand over octtree pointer ownership + Show_BOctTree* tree = new Show_BOctTree(scan, data_oct, cm); // unlock cached octtree to enable creation of more octtres without blocking the space for full scan points