From b03c2a3e0cc822f21514a9199c021d1b320efe0f Mon Sep 17 00:00:00 2001 From: Razvan Mihalyi Date: Wed, 24 Oct 2012 11:40:31 +0200 Subject: [PATCH] update svn to r733 (3) --- include/segmentation/FHGraph.h | 50 +++ include/segmentation/disjoint-set.h | 48 ++ include/segmentation/segment-graph.h | 49 ++ include/slam6d/kdTreeImpl.h | 238 ++++++++++ src/normals/CMakeLists.txt~ | 7 + src/normals/calculate_normals.cc~ | 643 +++++++++++++++++++++++++++ src/normals/test.cc | 95 ++++ src/segmentation/FHGraph.cc | 269 +++++++++++ src/segmentation/disjoint-set.cc | 47 ++ src/segmentation/fhsegmentation.cc | 297 +++++++++++++ src/segmentation/segment-graph.cc | 50 +++ 11 files changed, 1793 insertions(+) create mode 100644 include/segmentation/FHGraph.h create mode 100644 include/segmentation/disjoint-set.h create mode 100644 include/segmentation/segment-graph.h create mode 100755 include/slam6d/kdTreeImpl.h create mode 100644 src/normals/CMakeLists.txt~ create mode 100644 src/normals/calculate_normals.cc~ create mode 100644 src/normals/test.cc create mode 100644 src/segmentation/FHGraph.cc create mode 100644 src/segmentation/disjoint-set.cc create mode 100644 src/segmentation/fhsegmentation.cc create mode 100644 src/segmentation/segment-graph.cc diff --git a/include/segmentation/FHGraph.h b/include/segmentation/FHGraph.h new file mode 100644 index 0000000..7d489c5 --- /dev/null +++ b/include/segmentation/FHGraph.h @@ -0,0 +1,50 @@ +/** + * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm + * + * Copyright (C) Jacobs University Bremen + * + * Released under the GPL version 3. + * + * @author Mihai-Cotizo Sima + */ + + +#ifndef __FHGRAPH_H_ +#define __FHGRAPH_H_ + +#include +#include + +#include +#include +#include +#include + +class FHGraph { +public: + FHGraph(std::vector< Point >& ps, double weight(Point, Point), double sigma, double eps, int neighbors, float radius); + edge* getGraph(); + Point operator[](int index); + int getNumPoints(); + int getNumEdges(); + + void dispose(); +private: + void compute_neighbors(double weight(Point, Point), double eps); + void do_gauss(double sigma); + void without_gauss(); + + std::vector edges; + std::vector& points; + int V; + int E; + + int nr_neighbors; + float radius; + + + struct he{ int x; float w; }; + std::vector< std::list > adjency_list; +}; + +#endif diff --git a/include/segmentation/disjoint-set.h b/include/segmentation/disjoint-set.h new file mode 100644 index 0000000..3f5859b --- /dev/null +++ b/include/segmentation/disjoint-set.h @@ -0,0 +1,48 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef DISJOINT_SET +#define DISJOINT_SET + +// disjoint-set forests using union-by-rank and path compression (sort of). + +typedef struct { + int rank; + int p; + int size; +} uni_elt; + +class universe { +public: + universe(int elements); + ~universe(); + int find(int x); + void join(int x, int y); + int size(int x) const { + return elts[x].size; + } + int num_sets() const { + return num; + } + +private: + uni_elt *elts; + int num; +}; + +#endif diff --git a/include/segmentation/segment-graph.h b/include/segmentation/segment-graph.h new file mode 100644 index 0000000..d2562b0 --- /dev/null +++ b/include/segmentation/segment-graph.h @@ -0,0 +1,49 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef SEGMENT_GRAPH +#define SEGMENT_GRAPH + +#include +#include +#include "disjoint-set.h" + +// threshold function +#define THRESHOLD(size, c) (c/size) + +typedef struct { + float w; + int a, b; +} edge; + +bool operator<(const edge &a, const edge &b); + +/* + * Segment a graph + * + * Returns a disjoint-set forest representing the segmentation. + * + * num_vertices: number of vertices in graph. + * num_edges: number of edges in graph + * edges: array of edges. + * c: constant for treshold function. + */ +universe *segment_graph(int num_vertices, int num_edges, edge *edges, + float c); + +#endif diff --git a/include/slam6d/kdTreeImpl.h b/include/slam6d/kdTreeImpl.h new file mode 100755 index 0000000..54a7a53 --- /dev/null +++ b/include/slam6d/kdTreeImpl.h @@ -0,0 +1,238 @@ +/** @file + * @brief Representation of the optimized k-d tree. + * @author Remus Dumitru. Jacobs University Bremen, Germany + * @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany + * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. + * @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany. + */ + +#ifndef __KD_TREE_IMPL_H__ +#define __KD_TREE_IMPL_H__ + +#include "slam6d/kdparams.h" +#include "globals.icc" + +#ifdef _MSC_VER +#if !defined _OPENMP && defined OPENMP +#define _OPENMP +#endif +#endif + +#ifdef _OPENMP +#include +#endif + +/** + * @brief The optimized k-d tree. + * + * A kD tree for points, with limited + * capabilities (find nearest point to + * a given point, or to a ray). + **/ +template +class KDTreeImpl { +public: + inline KDTreeImpl() { } + + virtual inline ~KDTreeImpl() { + if (!npts) { +#ifdef WITH_OPENMP_KD + omp_set_num_threads(OPENMP_NUM_THREADS); +#pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < 2; i++) { + if (i == 0 && node.child1) delete node.child1; + if (i == 1 && node.child2) delete node.child2; + } + } else { + if (leaf.p) delete [] leaf.p; + } + } + + virtual void create(PointData pts, AccessorData *indices, size_t n) { + AccessorFunc point; + + // Find bbox + double xmin = point(pts, indices[0])[0], xmax = point(pts, indices[0])[0]; + double ymin = point(pts, indices[0])[1], ymax = point(pts, indices[0])[1]; + double zmin = point(pts, indices[0])[2], zmax = point(pts, indices[0])[2]; + for(unsigned int i = 1; i < n; i++) { + xmin = min(xmin, point(pts, indices[i])[0]); + xmax = max(xmax, point(pts, indices[i])[0]); + ymin = min(ymin, point(pts, indices[i])[1]); + ymax = max(ymax, point(pts, indices[i])[1]); + zmin = min(zmin, point(pts, indices[i])[2]); + zmax = max(zmax, point(pts, indices[i])[2]); + } + + // Leaf nodes + if ((n > 0) && (n <= 10)) { + npts = n; + leaf.p = new AccessorData[n]; + // fill leaf index array with indices + for(unsigned int i = 0; i < n; ++i) { + leaf.p[i] = indices[i]; + } + return; + } + + // Else, interior nodes + npts = 0; + + node.center[0] = 0.5 * (xmin+xmax); + node.center[1] = 0.5 * (ymin+ymax); + node.center[2] = 0.5 * (zmin+zmax); + node.dx = 0.5 * (xmax-xmin); + node.dy = 0.5 * (ymax-ymin); + node.dz = 0.5 * (zmax-zmin); + node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz); + + // Find longest axis + if (node.dx > node.dy) { + if (node.dx > node.dz) { + node.splitaxis = 0; + } else { + node.splitaxis = 2; + } + } else { + if (node.dy > node.dz) { + node.splitaxis = 1; + } else { + node.splitaxis = 2; + } + } + + // Partition + double splitval = node.center[node.splitaxis]; + + if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) { + npts = n; + leaf.p = new AccessorData[n]; + // fill leaf index array with indices + for(unsigned int i = 0; i < n; ++i) { + leaf.p[i] = indices[i]; + } + return; + } + + AccessorData* left = indices, * right = indices + n - 1; + while(true) { + while(point(pts, *left)[node.splitaxis] < splitval) + left++; + while(point(pts, *right)[node.splitaxis] >= splitval) + right--; + if(right < left) + break; + std::swap(*left, *right); + } + + // Build subtrees + int i; +#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas + omp_set_num_threads(OPENMP_NUM_THREADS); +#pragma omp parallel for schedule(dynamic) +#endif + for (i = 0; i < 2; i++) { + if (i == 0) { + node.child1 = new KDTreeImpl(); + node.child1->create(pts, indices, left - indices); + } + if (i == 1) { + node.child2 = new KDTreeImpl(); + node.child2->create(pts, left, n - (left - indices)); + } + } + } + +protected: + /** + * storing the parameters of the k-d tree, i.e., the current closest point, + * the distance to the current closest point and the point itself. + * These global variable are needed in this search. + * + * Padded in the parallel case. + */ +#ifdef _OPENMP +#ifdef __INTEL_COMPILER + __declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS]; +#else + static KDParams params[MAX_OPENMP_NUM_THREADS]; +#endif //__INTEL_COMPILER +#else + static KDParams params[MAX_OPENMP_NUM_THREADS]; +#endif + + /** + * number of points. If this is 0: intermediate node. If nonzero: leaf. + */ + int npts; + + /** + * Cue the standard rant about anon unions but not structs in C++ + */ + union { + /** + * in case of internal node... + */ + struct { + double center[3]; ///< storing the center of the voxel (R^3) + double dx, ///< defining the voxel itself + dy, ///< defining the voxel itself + dz, ///< defining the voxel itself + r2; ///< defining the voxel itself + int splitaxis; ///< defining the kind of splitaxis + KDTreeImpl *child1; ///< pointers to the childs + KDTreeImpl *child2; ///< pointers to the childs + } node; + /** + * in case of leaf node ... + */ + struct { + /** + * store the value itself + * Here we store just a pointer to the data + */ + AccessorData* p; + } leaf; + }; + + void _FindClosest(const PointData& pts, int threadNum) const { + AccessorFunc point; + + // Leaf nodes + if (npts) { + for (int i = 0; i < npts; i++) { + double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); + if (myd2 < params[threadNum].closest_d2) { + params[threadNum].closest_d2 = myd2; + params[threadNum].closest = point(pts, leaf.p[i]); + } + } + return; + } + + // Quick check of whether to abort + double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, + fabs(params[threadNum].p[1]-node.center[1])-node.dy), + fabs(params[threadNum].p[2]-node.center[2])-node.dz); + if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2) + return; + + // Recursive case + double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; + if (myd >= 0.0) { + node.child1->_FindClosest(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child2->_FindClosest(pts, threadNum); + } + } else { + node.child2->_FindClosest(pts, threadNum); + if (sqr(myd) < params[threadNum].closest_d2) { + node.child1->_FindClosest(pts, threadNum); + } + } + } +}; + +#endif diff --git a/src/normals/CMakeLists.txt~ b/src/normals/CMakeLists.txt~ new file mode 100644 index 0000000..791fa8d --- /dev/null +++ b/src/normals/CMakeLists.txt~ @@ -0,0 +1,7 @@ +IF(WITH_NORMALS) + FIND_PACKAGE(OpenCV REQUIRED) + + add_executable(calculateNormals calculate_normals.cc ../slam6d/fbr/fbr_global.cc ../slam6d/fbr/panorama.cc) + + target_link_libraries(calculateNormals scan ANN newmat fbr_panorama fbr_cv_io ${Boost_LIBRARIES} ${OpenCV_LIBS}) +ENDIF(WITH_NORMALS) diff --git a/src/normals/calculate_normals.cc~ b/src/normals/calculate_normals.cc~ new file mode 100644 index 0000000..2a46147 --- /dev/null +++ b/src/normals/calculate_normals.cc~ @@ -0,0 +1,643 @@ +/* + * calculateNormals implementation + * + * Copyright (C) Johannes Schauer, Razvan Mihaly + * + * Released under the GPL version 3 + * + */ + +#include "ANN/ANN.h" + +#include "newmat/newmat.h" +#include "newmat/newmatap.h" +#include "newmat/newmatio.h" +using namespace NEWMAT; + +#include "slam6d/point.h" +#include "normals/pointNeighbor.h" +#include "slam6d/scan.h" +#include "slam6d/globals.icc" +#include "slam6d/fbr/panorama.h" + +#include "normals/point.h" +#include "normals/SRI.h" + +#include +using std::string; + +#include +using std::cout; +using std::endl; +using std::vector; + +#include + +#include +#include +#include +namespace po = boost::program_options; + +enum normal_method {KNN_PCA, AKNN_PCA, PANO_PCA, PANO_SRI}; + +void normal_option_dependency(const po::variables_map & vm, normal_method ntype, const char *option) +{ + if (vm.count("normalMethod") && vm["normalMethod"].as() == ntype) { + if (!vm.count(option)) { + throw std::logic_error (string("this normal method needs ")+option+" to be set"); + } + } +} + +void normal_option_conflict(const po::variables_map & vm, normal_method ntype, const char *option) +{ + if (vm.count("normalMethod") && vm["normalMethod"].as() == ntype) { + if (vm.count(option)) { + throw std::logic_error (string("this normal method is incompatible with ")+option); + } + } +} + +/* + * validates input type specification + */ +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +void 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(), "KNN_PCA") == 0) v = KNN_PCA; + else if(strcasecmp(arg.c_str(), "AKNN_PCA") == 0) v = AKNN_PCA; + else if(strcasecmp(arg.c_str(), "PANO_PCA") == 0) v = PANO_PCA; + else if(strcasecmp(arg.c_str(), "PANO_SRI") == 0) v = PANO_SRI; + else throw std::runtime_error(std::string("normal method ") + arg + std::string(" is unknown")); +} + +/* + * parse commandline options, fill arguments + */ +void parse_options(int argc, char **argv, int &start, int &end, + bool &scanserver, string &dir, IOType &iotype, + int &maxDist, int &minDist, normal_method &normalMethod, int &knn, + int &kmin, int &kmax, double& alpha, int &width, int &height, + bool &flipnormals, double &factor) +{ + po::options_description generic("Generic options"); + generic.add_options() + ("help,h", "output this help message"); + + po::options_description input("Input options"); + input.add_options() + ("start,s", po::value(&start)->default_value(0), + "start at scan (i.e., neglects the first scans) " + "[ATTENTION: counting naturally starts with 0]") + ("end,e", po::value(&end)->default_value(-1), + "end after scan ") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose F from {uos, uos_map, " + "uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, " + "riegl_txt, riegl_rgb, riegl_bin, zahn, ply})") + ("max,M", po::value(&maxDist)->default_value(-1), + "neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&minDist)->default_value(-1), + "neglegt all data points with a distance smaller than 'units") + ("scanserver,S", po::bool_switch(&scanserver), + "Use the scanserver as an input method and handling of scan data") + ; + + po::options_description normal("Normal options"); + normal.add_options() + ("normalMethod,N", po::value(&normalMethod)->default_value(KNN_PCA), + "choose the method for computing normals:\n" + "KNN_PCA -- use kNN and PCA\n" + "AKNN_PCA -- use adaptive kNN and PCA\n" + "PANO_PCA -- use panorama image neighbors and PCA\n" + "PANO_SRI -- use panorama image neighbors and spherical range image differentiation\n") + ("knn,K", po::value(&knn), + "select the k in kNN search") + ("kmin,1", po::value(&kmin), + "select k_min in adaptive kNN search") + ("kmax,2", po::value(&kmax), + "select k_max in adaptive kNN search") + ("alpha,a", po::value(&alpha), + "select the alpha parameter for detecting an ill-conditioned neighborhood") + ("width,w", po::value(&width), + "width of panorama") + ("height,h", po::value(&height), + "height of panorama") + ("flipnormals,F", po::bool_switch(&flipnormals), + "flip orientation of normals towards scan pose") + ("factor,c", po::value(&factor), + "factor for SRI computation") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + // all options + po::options_description all; + all.add(generic).add(input).add(normal).add(hidden); + + // options visible with --help + po::options_description cmdline_options; + cmdline_options.add(generic).add(input).add(normal); + + // positional argument + po::positional_options_description pd; + pd.add("input-dir", 1); + + // process options + po::variables_map vm; + po::store(po::command_line_parser(argc, argv). + options(all).positional(pd).run(), vm); + po::notify(vm); + + // display help + if (vm.count("help")) { + cout << cmdline_options; + exit(0); + } + + normal_option_dependency(vm, KNN_PCA, "knn"); + normal_option_conflict(vm, KNN_PCA, "kmin"); + normal_option_conflict(vm, KNN_PCA, "kmax"); + normal_option_conflict(vm, KNN_PCA, "alpha"); + normal_option_conflict(vm, KNN_PCA, "width"); + normal_option_conflict(vm, KNN_PCA, "height"); + normal_option_conflict(vm, KNN_PCA, "factor"); + + normal_option_conflict(vm, AKNN_PCA, "knn"); + normal_option_dependency(vm, AKNN_PCA, "kmin"); + normal_option_dependency(vm, AKNN_PCA, "kmax"); + normal_option_dependency(vm, AKNN_PCA, "alpha"); + normal_option_conflict(vm, AKNN_PCA, "width"); + normal_option_conflict(vm, AKNN_PCA, "height"); + normal_option_conflict(vm, AKNN_PCA, "factor"); + + //normal_option_conflict(vm, PANO_PCA, "knn"); + normal_option_dependency(vm, KNN_PCA, "knn"); + normal_option_conflict(vm, PANO_PCA, "kmin"); + normal_option_conflict(vm, PANO_PCA, "kmax"); + normal_option_conflict(vm, PANO_PCA, "alpha"); + normal_option_dependency(vm, PANO_PCA, "width"); + normal_option_dependency(vm, PANO_PCA, "height"); + normal_option_conflict(vm, PANO_PCA, "factor"); + + normal_option_conflict(vm, PANO_SRI, "knn"); + normal_option_conflict(vm, PANO_SRI, "kmin"); + normal_option_conflict(vm, PANO_SRI, "kmax"); + normal_option_conflict(vm, PANO_SRI, "alpha"); + normal_option_conflict(vm, PANO_SRI, "width"); + normal_option_conflict(vm, PANO_SRI, "height"); + normal_option_dependency(vm, PANO_SRI, "factor"); + + // add trailing slash to directory if not present yet + if (dir[dir.length()-1] != '/') dir = dir + "/"; +} + +/* + * retrieve a cv::Mat with x,y,z,r from a scan object + * functionality borrowed from scan_cv::convertScanToMat but this function + * does not allow a scanserver to be used, prints to stdout and can only + * handle a single scan + */ +void scan2mat(Scan* scan, cv::Mat& scan_cv) { + DataXYZ xyz = scan->get("xyz"); + unsigned int nPoints = xyz.size(); + scan_cv.create(nPoints,1,CV_32FC(4)); + scan_cv = cv::Scalar::all(0); + cv::MatIterator_ it = scan_cv.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + (*it)[0] = xyz[i][0]; + (*it)[1] = xyz[i][1]; + (*it)[2] = xyz[i][2]; + ++it; + } +} + +/** + * Helper function that maps x, y, z to R, G, B using a linear function + */ +void mapNormalToRGB(const Point& normal, Point& rgb) +{ + rgb.x = 127.5 * normal.x + 127.5; + rgb.y = 127.5 * normal.y + 127.5; + rgb.z = 255.0 * fabs(normal.z); +} + +/** + * Write normals to .3d files using the uos_rgb format + */ +void writeNormals(const Scan* scan, const string& dir, + const vector& points, const vector& normals) +{ + + stringstream ss; + ss << dir << "scan" << string(scan->getIdentifier()) << ".3d"; + ofstream scan_file; + scan_file.open(ss.str().c_str()); + for(size_t i = 0; i < points.size(); ++i) { + Point rgb; + mapNormalToRGB(normals[i], rgb); + scan_file + << points[i].x << " " << points[i].y << " " << points[i].z << " " + << (unsigned int) rgb.x << " " << (unsigned int) rgb.y << " " + << (unsigned int) rgb.z << "\n"; + } + scan_file.close(); + + ss.clear(); ss.str(string()); + ss << dir << "scan" << string(scan->getIdentifier()) << ".pose"; + ofstream pose_file; + pose_file.open(ss.str().c_str()); + pose_file << 0 << " " << 0 << " " << 0 << "\n" << 0 << " " << 0 << " " << 0 << "\n"; + pose_file.close(); +} + +/** + * Compute eigen decomposition of a point and its neighbors using the NEWMAT library + * @param point - input points with corresponding neighbors + * @param e_values - out parameter returns the eigenvalues + * @param e_vectors - out parameter returns the eigenvectors + */ +void computeEigenDecomposition(const PointNeighbor& point, + DiagonalMatrix& e_values, Matrix& e_vectors) +{ + Point centroid(0, 0, 0); + vector neighbors = point.neighbors; + + for (size_t j = 0; j < neighbors.size(); ++j) { + centroid.x += neighbors[j].x; + centroid.y += neighbors[j].y; + centroid.z += neighbors[j].z; + } + centroid.x /= (double) neighbors.size(); + centroid.y /= (double) neighbors.size(); + centroid.z /= (double) neighbors.size(); + + Matrix S(3, 3); + S = 0.0; + for (size_t j = 0; j < neighbors.size(); ++j) { + ColumnVector point_prime(3); + point_prime(1) = neighbors[j].x - centroid.x; + point_prime(2) = neighbors[j].y - centroid.y; + point_prime(3) = neighbors[j].z - centroid.z; + S = S + point_prime * point_prime.t(); + } + // normalize S + for (int j = 0; j < 3; ++j) + for (int k = 0; k < 3; ++k) + S(j+1, k+1) /= (double) neighbors.size(); + + SymmetricMatrix C; + C << S; + // the decomposition + Jacobi(C, e_values, e_vectors); + +#ifdef DEBUG + // Print the result + cout << "The eigenvalues matrix:" << endl; + cout << e_values << endl; +#endif +} + +/** + * Compute neighbors using kNN search + * @param points - input set of points + * @param points_neighbors - output set of points with corresponding neighbors + * @param knn - k constant in kNN search + * @param kmax - to be used in adaptive knn search as the upper bound on adapting the k constant, defaults to -1 for regular kNN search + * @param alpha - to be used in adaptive knn search for detecting ill-conditioned neighborhoods + * @param eps - parameter required by the ANN library in kNN search + */ +void computeKNearestNeighbors(const vector& points, + vector& points_neighbors, int knn, int kmax=-1, + double alpha=1000.0, double eps=1.0) +{ + ANNpointArray point_array = annAllocPts(points.size(), 3); + for (size_t i = 0; i < points.size(); ++i) { + point_array[i] = new ANNcoord[3]; + point_array[i][0] = points[i].x; + point_array[i][1] = points[i].y; + point_array[i][2] = points[i].z; + } + + ANNkd_tree t(point_array, points.size(), 3); + ANNidxArray n; + ANNdistArray d; + + if (kmax < 0) { + /// regular kNN search, allocate memory for knn + n = new ANNidx[knn]; + d = new ANNdist[knn]; + } else { + /// adaptive kNN search, allocate memory for kmax + n = new ANNidx[kmax]; + d = new ANNdist[kmax]; + } + + for (size_t i = 0; i < points.size(); ++i) { + vector neighbors; + ANNpoint p = point_array[i]; + + t.annkSearch(p, knn, n, d, eps); + + neighbors.push_back(points[i]); + for (int j = 0; j < knn; ++j) { + if ( n[j] != (int)i ) + neighbors.push_back(points[n[j]]); + } + + PointNeighbor current_point(points[i], neighbors); + points_neighbors.push_back( current_point ); + Matrix e_vectors(3,3); e_vectors = 0.0; + DiagonalMatrix e_values(3); e_values = 0.0; + computeEigenDecomposition( current_point, e_values, e_vectors ); + + if (kmax > 0) { + /// detecting an ill-conditioned neighborhood + if (e_values(3) / e_values(2) > alpha && e_values(2) > 0.0) { + if (knn < kmax) + cout << "Increasing kmin to " << ++knn << endl; + } + } + } + + delete[] n; + delete[] d; +} + +/** + * Compute neighbors using kNN search + * @param point - input point with neighbors + * @param new_point - output point with new neighbors + * @param knn - k constant in kNN search + * @param eps - parameter required by the ANN library in kNN search + */ +void computeKNearestNeighbors(const PointNeighbor& point, + PointNeighbor& new_point, int knn, + double eps=1.0) +{ + /// allocate memory for all neighbors of point plus the point itself + ANNpointArray point_array = annAllocPts(point.neighbors.size()+1, 3); + for (size_t i = 0; i < point.neighbors.size(); ++i) { + point_array[i] = new ANNcoord[3]; + point_array[i][0] = point.neighbors[i].x; + point_array[i][1] = point.neighbors[i].y; + point_array[i][2] = point.neighbors[i].z; + } + int last = point.neighbors.size(); + point_array[last] = new ANNcoord[3]; + point_array[last][0] = point.point.x; + point_array[last][1] = point.point.y; + point_array[last][2] = point.point.z; + + ANNkd_tree t(point_array, point.neighbors.size()+1, 3); + ANNidxArray n; + ANNdistArray d; + + /// regular kNN search, allocate memory for knn + n = new ANNidx[knn]; + d = new ANNdist[knn]; + + vector new_neighbors; + /// last point in the array is the current point + ANNpoint p = point_array[point.neighbors.size()]; + + t.annkSearch(p, knn, n, d, eps); + new_neighbors.push_back(point.point); + + for (int j = 0; j < knn; ++j) { + if ( n[j] != (int) point.neighbors.size() ) + new_neighbors.push_back(point.neighbors[n[j]]); + } + + new_point.point = point.point; + new_point.neighbors = new_neighbors; + + delete[] n; + delete[] d; +} + +/** + * Compute neighbors using panorama images + * @param fPanorama - input panorama image created from the current scan + * @param points_neighbors - output set of points with corresponding neighbors + */ +void computePanoramaNeighbors(Scan* scan, + vector& points_neighbors, int width, int height, int knn) +{ + cv::Mat scan_cv; + scan2mat(scan, scan_cv); + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan_cv); + cv::Mat img = fPanorama.getRangeImage(); + vector > > extended_map = fPanorama.getExtendedMap(); + for (int row = 0; row < height; ++row) { + for (int col = 0; col < width; ++col) { + vector points_panorama = extended_map[row][col]; + /// if no points found, skip pixel + if (points_panorama.size() < 1) continue; + /// foreach point from panorama consider all points in the bucket as its neighbors + for (size_t point_idx = 0; point_idx < points_panorama.size(); ++point_idx) { + Point point; + point.x = points_panorama[point_idx][0]; + point.y = points_panorama[point_idx][1]; + point.z = points_panorama[point_idx][2]; + vector neighbors; + for (size_t i = 0; i < points_panorama.size(); ++i) { + if (i != point_idx) + neighbors.push_back(Point (points_panorama[i][0], points_panorama[i][1], points_panorama[i][2]) ); + } + /// add neighbors from adjacent pixels and buckets + for (int i = -1; i <= 1; ++i) { + for (int j = -1; j <= 1; ++j) { + if (!(i==0 && j==0) && !(row+i < 0 || col+j < 0) + && !(row+i >= height || col+j >= width) ) { + vector neighbors_panorama = extended_map[row+i][col+j]; + for (size_t k = 0; k < neighbors_panorama.size(); ++k) + neighbors.push_back(Point (neighbors_panorama[k][0], + neighbors_panorama[k][1], + neighbors_panorama[k][2]) ); + } + } + } + /// filter the point by kNN search + PointNeighbor current_point(point, neighbors); + if (knn > 0) { + PointNeighbor filtered_point; + computeKNearestNeighbors(current_point, filtered_point, knn); + points_neighbors.push_back(filtered_point); + } else { + points_neighbors.push_back(current_point); + } + } + } + } +} + +/** + * Compute normals using PCA given a set of points and their neighbors + * @param scan - pointer to current scan, used to compute the position vectors + * @param points - input set of points with corresponding neighbors + * @param normals - output set of normals + */ +void computePCA(const Scan* scan, const vector& points, + vector& normals, bool flipnormals) +{ + ColumnVector origin(3); + const double *scan_pose = scan->get_rPos(); + for (int i = 0; i < 3; ++i) + origin(i+1) = scan_pose[i]; + + for(size_t i = 0; i < points.size(); ++i) { + vector neighbors = points[i].neighbors; + + if (points[i].neighbors.size() < 2) { + normals.push_back( Point(0,0,0) ); + continue; + } + + ColumnVector point_vector(3); + point_vector(1) = points[i].point.x - origin(1); + point_vector(2) = points[i].point.y - origin(2); + point_vector(3) = points[i].point.z - origin(3); + point_vector = point_vector / point_vector.NormFrobenius(); + + Matrix e_vectors(3,3); e_vectors = 0.0; + DiagonalMatrix e_values(3); e_values = 0.0; + computeEigenDecomposition(points[i], e_values, e_vectors); + + ColumnVector v1(3); + v1(1) = e_vectors(1,1); + v1(2) = e_vectors(2,1); + v1(3) = e_vectors(3,1); + // consider first (smallest) eigenvector as the normal + Real angle = (v1.t() * point_vector).AsScalar(); + + // orient towards scan pose + // works better when orientation is not flipped + if (flipnormals && angle < 0) { + v1 *= -1.0; + } + normals.push_back( Point(v1(1), v1(2), v1(3)) ); + } +} + +void computeSRI(int factor, vector& points, vector& normals) +{ + SRI *sri2 = new SRI(0, factor); + + for (size_t i = 0; i < points.size(); i++) { + sri2->addPoint(points[i].x, points[i].y, points[i].z); + } + + points.clear(); + + for (unsigned int i = 0; i < sri2->points.size(); i++) { + double rgbN[3], x, y, z; + PointN* p = sri2->points[i]; + p->getCartesian(x, y, z); + sri2->getNormalSRI(p, rgbN); + normals.push_back(Point(rgbN[0], rgbN[1], rgbN[2])); + points.push_back(Point(x, z, y)); + } +} + +void scan2points(Scan* scan, vector &points) +{ + DataXYZ xyz = scan->get("xyz"); + unsigned int nPoints = xyz.size(); + for(unsigned int i = 0; i < nPoints; ++i) { + points.push_back(Point(xyz[i][0], xyz[i][1], xyz[i][2])); + } +} + +int main(int argc, char **argv) +{ + // commandline arguments + int start, end; + bool scanserver; + int maxDist, minDist; + string dir; + IOType iotype; + normal_method normalMethod; + int knn, kmin, kmax; + double alpha; + int width, height; + bool flipnormals; + double factor; + + parse_options(argc, argv, start, end, scanserver, dir, iotype, maxDist, + minDist, normalMethod, knn, kmin, kmax, alpha, width, height, + flipnormals, factor); + + Scan::openDirectory(scanserver, dir, iotype, start, end); + + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + boost::filesystem::path boost_dir(dir + "normals/"); + boost::filesystem::create_directory(boost_dir); + + for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(maxDist, minDist); + + vector points_neighbors; + vector normals; + vector points; + + scan2points(scan, points); + + switch (normalMethod) { + case KNN_PCA: + computeKNearestNeighbors(points, points_neighbors, knn); + computePCA(scan, points_neighbors, normals, flipnormals); + break; + case AKNN_PCA: + computeKNearestNeighbors(points, points_neighbors, kmin, kmax, alpha); + computePCA(scan, points_neighbors, normals, flipnormals); + break; + case PANO_PCA: + computePanoramaNeighbors(scan, points_neighbors, width, height, knn); + computePCA(scan, points_neighbors, normals, flipnormals); + break; + case PANO_SRI: + computeSRI(factor, points, normals); + break; + default: + cerr << "unknown normal method" << endl; + return 1; + break; + } + + if (points.size() != normals.size()) { + cerr << "got " << points.size() << " points but " << normals.size() << " normals" << endl; + return 1; + } + + writeNormals(scan, dir + "normals/", points, normals); + } + + Scan::closeDirectory(); + + return 0; +} diff --git a/src/normals/test.cc b/src/normals/test.cc new file mode 100644 index 0000000..328f2f2 --- /dev/null +++ b/src/normals/test.cc @@ -0,0 +1,95 @@ +/******************************************************* + A simple program that demonstrates NewMat10 library. + The program defines a random symmetric matrix + and computes its eigendecomposition. + For further details read the NewMat10 Reference Manual +********************************************************/ + +#include +#include +#include + +// the following two are needed for printing +#include +#include +/************************************** +/* The NewMat10 include files */ +#include +#include +#include +#include +/***************************************/ + + +int main(int argc, char **argv) { + int M = 3, N = 5; + Matrix X(M,N); // Define an M x N general matrix + + // Fill X by random numbers between 0 and 9 + // Note that indexing into matrices in NewMat is 1-based! + srand(time(NULL)); + for (int i = 1; i <= M; ++i) { + for (int j = 1; j <= N; ++j) { + X(i,j) = rand() % 10; + } + } + + SymmetricMatrix C; + C << X * X.t(); // fill in C by X * X^t. + // Works because we *know* that the result is symmetric + + cout << "The symmetrix matrix C" << endl; + cout << setw(5) << setprecision(0) << C << endl; + + + // compute eigendecomposition of C + Matrix V(3,3); // for eigenvectors + DiagonalMatrix D(3); // for eigenvalues + + // the decomposition + Jacobi(C, D, V); + + // Print the result + cout << "The eigenvalues matrix:" << endl; + cout << setw(10) << setprecision(5) << D << endl; + cout << "The eigenvectors matrix:" << endl; + cout << setw(10) << setprecision(5) << V << endl; + + // Check that the first eigenvector indeed has the eigenvector property + ColumnVector v1(3); + v1(1) = V(1,1); + v1(2) = V(2,1); + v1(3) = V(3,1); + + ColumnVector Cv1 = C * v1; + ColumnVector lambda1_v1 = D(1) * v1; + + cout << "The max-norm of the difference between C*v1 and lambda1*v1 is " << + NormInfinity(Cv1 - lambda1_v1) << endl << endl; + + // Build the inverse and check the result + Matrix Ci = C.i(); + Matrix I = Ci * C; + + cout << "The inverse of C is" << endl; + cout << setw(10) << setprecision(5) << Ci << endl; + cout << "And the inverse times C is identity" << endl; + cout << setw(10) << setprecision(5) << I << endl; + + // Example for multiple solves (see NewMat documentation) + ColumnVector r1(3), r2(3); + for (i = 1; i <= 3; ++i) { + r1(i) = rand() % 10; + r2(i) = rand() % 10; + } + LinearEquationSolver CLU = C; // decomposes C + ColumnVector s1 = CLU.i() * r1; + ColumnVector s2 = CLU.i() * r2; + + cout << "solution for right hand side r1" << endl; + cout << setw(10) << setprecision(5) << s1 << endl; + cout << "solution for right hand side r2" << endl; + cout << setw(10) << setprecision(5) << s2 << endl; + + return 0; +} diff --git a/src/segmentation/FHGraph.cc b/src/segmentation/FHGraph.cc new file mode 100644 index 0000000..94eaac8 --- /dev/null +++ b/src/segmentation/FHGraph.cc @@ -0,0 +1,269 @@ +/** + * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm + * + * Copyright (C) Jacobs University Bremen + * + * Released under the GPL version 3. + * + * @author Mihai-Cotizo Sima + */ + +#include +#include +#include +#include + +using namespace std; + + + +FHGraph::FHGraph(std::vector< Point >& ps, double weight(Point, Point), double sigma, double eps, int neighbors, float radius) : + points( ps ), V( ps.size() ) +{ + /* + * 1. create adjency list using a map > + * 2. use get_neighbors(e, max_dist) to get all the edges e' that are at a distance smaller than max_dist than e + * 3. using all these edges, compute the gaussian smoothed weight + * 4. insert the edges in a new list + */ + nr_neighbors = neighbors; + this->radius = radius; + + compute_neighbors(weight, eps); + + + if ( sigma > 0.01 ) + { + do_gauss(sigma); + } + else + { + without_gauss(); + } + + adjency_list.clear(); +} + +void FHGraph::compute_neighbors(double weight(Point, Point), double eps) +{ + + adjency_list.reserve(points.size()); + adjency_list.resize(points.size()); + + ANNpointArray pa = annAllocPts(points.size(), 3); + for (size_t i=0; i 0 && nr_neighbors < nret ) + { + random_shuffle(n, n+nret); + nret = nr_neighbors; + } + + for (int j=0; j& v) +{ + double s = 0; + for (size_t i=0; i::iterator k, j; + vector gauss_weight, edge_weight; + edge e; +#pragma omp parallel for private(j, k, e, gauss_weight, edge_weight) schedule(dynamic) + for (int i=0; iw, j->w, sigma)); + edge_weight.push_back(k->w); + } + for (k=adjency_list[j->x].begin(); + k!=adjency_list[j->x].end(); + ++k) + { + gauss_weight.push_back(gauss(k->w, j->w, sigma)); + edge_weight.push_back(k->w); + } + normalize(gauss_weight); + + e.a = i; e.b = j->x; + e.w = 0; + for (size_t k=0; k::iterator j; + edge e; + + for (int i=0; ix; e.w = j->w; + edges.push_back(e); + } + } +} + +edge* FHGraph::getGraph() +{ + edge* ret = new edge[edges.size()]; + for (size_t i=0; i +void vectorFree(T& t) { + T tmp; + t.swap(tmp); +} + +void FHGraph::dispose() { + vectorFree(edges); + vectorFree(points); + vectorFree(adjency_list); +} + diff --git a/src/segmentation/disjoint-set.cc b/src/segmentation/disjoint-set.cc new file mode 100644 index 0000000..3479ed8 --- /dev/null +++ b/src/segmentation/disjoint-set.cc @@ -0,0 +1,47 @@ +/** + * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm + * + * Copyright (C) Jacobs University Bremen + * + * Released under the GPL version 3. + * + * @author Mihai-Cotizo Sima + */ + + +#include + +universe::universe(int elements) { + elts = new uni_elt[elements]; + num = elements; + for (int i = 0; i < elements; i++) { + elts[i].rank = 0; + elts[i].size = 1; + elts[i].p = i; + } +} + +universe::~universe() { + delete [] elts; +} + +int universe::find(int x) { + int y = x; + while (y != elts[y].p) + y = elts[y].p; + elts[x].p = y; + return y; +} + +void universe::join(int x, int y) { + if (elts[x].rank > elts[y].rank) { + elts[y].p = x; + elts[x].size += elts[y].size; + } else { + elts[x].p = y; + elts[y].size += elts[x].size; + if (elts[x].rank == elts[y].rank) + elts[y].rank++; + } + num--; +} diff --git a/src/segmentation/fhsegmentation.cc b/src/segmentation/fhsegmentation.cc new file mode 100644 index 0000000..2825089 --- /dev/null +++ b/src/segmentation/fhsegmentation.cc @@ -0,0 +1,297 @@ +/** + * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm + * + * Copyright (C) Jacobs University Bremen + * + * Released under the GPL version 3. + * + * @author Billy Okal + * @author Mihai-Cotizo Sima + * @file fhsegmentation.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +/// validate IO types +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +/// Parse commandline options +void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, + IOType &iotype, float &sigma, int &k, int &neighbors, float &eps, float &radius, int &min_size) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: fhsegmentation where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose format from [uos|uosr|uos_map|" + "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" + "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") + ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1), "neglegt all data points with a distance smaller than 'units") + ("K,k", po::value(&k)->default_value(1), " value of K value used in the FH segmentation") + ("neighbors,N", po::value(&neighbors)->default_value(1), "use approximate -nearest neighbors search or limit the number of points") + ("sigma,v", po::value(&sigma)->default_value(1.0), "Set the Gaussian variance for smoothing to ") + ("radius,r", po::value(&radius)->default_value(-1.0), "Set the range of radius search to ") + ("eps,E", po::value(&eps)->default_value(1.0), "Set error threshold used by the AKNN algorithm to ") + ("minsize,z", po::value(&min_size)->default_value(0), "Keep segments of size at least ") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + po::positional_options_description pd; + pd.add("input-dir", 1); + + po::options_description all; + all.add(cmd_options).add(hidden); + + po::variables_map vmap; + po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); + po::notify(vmap); + + if (vmap.count("help")) { + cout << cmd_options << endl; + exit(-1); + } + + // read scan path + if (dir[dir.length()-1] != '/') dir = dir + "/"; + +} + +/// distance measures +double weight1(Point a, Point b) +{ + return a.distance(b); +} + +double weight2(Point a, Point b) +{ + return a.distance(b) * .5 + fabs(a.reflectance-b.reflectance) * .5; +} + + +/// Write a pose file with the specofied name +void writePoseFiles(string dir, const double* rPos, const double* rPosTheta, int num, int outnum) +{ + for (int i = outnum; i < num; i++) { + string poseFileName = dir + "segments/scan" + to_string(i, 3) + ".pose"; + ofstream posout(poseFileName.c_str()); + + posout << rPos[0] << " " + << rPos[1] << " " + << rPos[2] << endl + << deg(rPosTheta[0]) << " " + << deg(rPosTheta[1]) << " " + << deg(rPosTheta[2]) << endl; + posout.clear(); + posout.close(); + } +} + +/// write scan files for all segments +void writeScanFiles(string dir, int outnum, const vector* > cloud) +{ + for (int i = outnum, j = 0; i < (int)cloud.size() && j < (int)cloud.size(); i++, j++) { + vector* segment = cloud[j]; + string scanFileName = dir + "segments/scan" + to_string(i,3) + ".3d"; + ofstream scanout(scanFileName.c_str()); + + for (int k = 0; k < (int)segment->size(); k++) { + Point p = segment->at(k); + scanout << p.x << " " << p.y << " " << p.z << endl; + } + scanout.close(); + } +} + + + +/// ============================================= +/// Main +/// ============================================= +int main(int argc, char** argv) +{ + int start, end; + bool scanserver; + int max_dist, min_dist; + string dir; + IOType iotype; + float sigma; + int k, neighbors; + float eps; + float radius; + int min_size; + + parse_options(argc, argv, start, end, scanserver, max_dist, min_dist, + dir, iotype, sigma, k, neighbors, eps, radius, min_size); + + /// ---------------------------------- + /// Prepare and read scans + /// ---------------------------------- + if (scanserver) { + try { + ClientInterface::create(); + } catch(std::runtime_error& e) { + cerr << "ClientInterface could not be created: " << e.what() << endl; + cerr << "Start the scanserver first." << endl; + exit(-1); + } + } + + /// Make directory for saving the scan segments + string segdir = dir + "segments"; + +#ifdef _MSC_VER + int success = mkdir(segdir.c_str()); +#else + int success = mkdir(segdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing segments to " << segdir << endl; + } else if(errno == EEXIST) { + cout << "WARN: Directory " << segdir << " exists already. Contents will be overwriten" << endl; + } else { + cerr << "Creating directory " << segdir << " failed" << endl; + exit(1); + } + + /// Read the scans + Scan::openDirectory(scanserver, dir, iotype, start, end); + if(Scan::allScans.size() == 0) { + cerr << "No scans found. Did you use the correct format?" << endl; + exit(-1); + } + + /// -------------------------------------------- + /// Initialize and perform segmentation + /// -------------------------------------------- + std::vector::iterator it = Scan::allScans.begin(); + int outscan = start; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + /// read scan into points + DataXYZ xyz(scan->get("xyz")); + vector points; + points.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + Point p(xyz[j][0], xyz[j][1], xyz[j][2]); + points.push_back(p); + } + + /// create the graph and get the segments + cout << "creating graph" << endl; + FHGraph sgraph(points, weight2, sigma, eps, neighbors, radius); + + cout << "segmenting graph" << endl; + edge* sedges = sgraph.getGraph(); + universe* segmented = segment_graph(sgraph.getNumPoints(), + sgraph.getNumEdges(), + sedges, k); + + cout << "post processing" << endl; + for (int i = 0; i < sgraph.getNumEdges(); ++i) + { + int a = sedges[i].a; + int b = sedges[i].b; + + int aa = segmented->find(a); + int bb = segmented->find(b); + + if ( (aa!=bb) && + (segmented->size(aa) < min_size || + segmented->size(bb) < min_size) ) + segmented->join(aa, bb); + } + + delete[] sedges; + + int nr = segmented->num_sets(); + cout << "Obtained " << nr << " segment(s)" << endl; + + /// write point clouds with segments + vector< vector* > clouds; + clouds.reserve(nr); + for (int i=0; i ); + + map components2cloud; + int kk = 0; + + for (int i = 0; i < sgraph.getNumPoints(); ++i) + { + int component = segmented->find(i); + if ( components2cloud.find(component)==components2cloud.end() ) + { + components2cloud[component] = kk++; + clouds[components2cloud[component]]->reserve(segmented->size(component)); + } + clouds[components2cloud[component]]->push_back(sgraph[i]); + } + + // pose file (repeated for the number of segments + writePoseFiles(dir, rPos, rPosTheta, clouds.size(), outscan); + // scan files for all segments + writeScanFiles(dir, outscan, clouds); + + outscan += clouds.size(); + + /// clean up + sgraph.dispose(); + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + else + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} + diff --git a/src/segmentation/segment-graph.cc b/src/segmentation/segment-graph.cc new file mode 100644 index 0000000..7f8a298 --- /dev/null +++ b/src/segmentation/segment-graph.cc @@ -0,0 +1,50 @@ +/** + * Point Cloud Segmentation using Felzenszwalb-Huttenlocher Algorithm + * + * Copyright (C) Jacobs University Bremen + * + * Released under the GPL version 3. + * + * @author Mihai-Cotizo Sima + */ + +#include + +bool operator<(const edge &a, const edge &b) { + return a.w < b.w; +} + +universe *segment_graph(int num_vertices, int num_edges, edge *edges, + float c) { + // sort edges by weight + std::sort(edges, edges + num_edges); + + // make a disjoint-set forest + universe *u = new universe(num_vertices); + + // init thresholds + float *threshold = new float[num_vertices]; + for (int i = 0; i < num_vertices; i++) + threshold[i] = THRESHOLD(1,c); + + // for each edge, in non-decreasing weight order... + for (int i = 0; i < num_edges; i++) { + edge *pedge = &edges[i]; + + // components conected by this edge + int a = u->find(pedge->a); + int b = u->find(pedge->b); + if (a != b) { + if ((pedge->w <= threshold[a]) && + (pedge->w <= threshold[b])) { + u->join(a, b); + a = u->find(a); + threshold[a] = pedge->w + THRESHOLD(u->size(a), c); + } + } + } + + // free up + delete threshold; + return u; +}