├── sample_pcl_viewer
    ├── cloud.pcd
    ├── include
    │   └── vis.hpp
    ├── CMakeLists.txt
    └── src
    │   └── main.cpp
├── README.md
├── .gitignore
├── namaris-config.cmake.in
├── include
    └── namaris
    │   ├── 3rd_party
    │       ├── felzenswalb_segmentation
    │       │   ├── README
    │       │   ├── disjoint-set.h
    │       │   ├── segment-graph.h
    │       │   ├── disjoint-set.hpp
    │       │   └── segment-graph.hpp
    │       └── meanshift
    │       │   ├── Readme.txt
    │       │   ├── VectorPointSet.h
    │       │   └── EuclideanGeometry.h
    │   ├── utilities
    │       ├── pcl_typedefs.hpp
    │       ├── pcl_visualization
    │       │   ├── color.h
    │       │   ├── vtk_colormaps.h
    │       │   ├── color.hpp
    │       │   ├── vtk_colormaps.hpp
    │       │   └── vtk_colormaps_lut.hpp
    │       ├── string.hpp
    │       ├── eigen.hpp
    │       ├── opencv.hpp
    │       ├── alphanum
    │       │   └── alphanum.cpp
    │       ├── map.hpp
    │       ├── pointcloud_segmentation_io.hpp
    │       ├── octomap_pcl_conversion.hpp
    │       ├── reconstruction.hpp
    │       └── filesystem.hpp
    │   └── algorithms
    │       ├── felzenswalb_graph_segmentation
    │           ├── felzenswalb_graph_segmentation.h
    │           └── felzenswalb_graph_segmentation.hpp
    │       ├── region_growing_smoothness
    │           ├── region_growing_smoothness.h
    │           └── region_growing_smoothness.hpp
    │       ├── region_growing_normal_variation
    │           ├── region_growing_normal_variation.h
    │           └── region_growing_normal_variation.hpp
    │       ├── meanshift
    │           ├── meanshift.h
    │           └── meanshift.hpp
    │       ├── pointcloud_occlusion_boundaries.hpp
    │       ├── dijkstra_shortest_path.hpp
    │       ├── lccp_segmentation.hpp
    │       ├── pointcloud_alignment.hpp
    │       ├── felzenswalb_segmentation
    │           └── felzenswalb_segmentation.h
    │       ├── mst.hpp
    │       ├── min_cut.hpp
    │       ├── region_growing
    │           └── region_growing.h
    │       ├── min_cut_plane_segmentation.hpp
    │       └── region_growing_plane_segmentation.hpp
├── LICENSE
├── CMakeModules
    ├── CMakeUninstall.cmake.in
    └── InstallPkgConfigFile.cmake
└── CMakeLists.txt


/sample_pcl_viewer/cloud.pcd:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/aecins/namaris/HEAD/sample_pcl_viewer/cloud.pcd


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Namaris
 2 | A header only library with some useful helper functions for C++ development. Some of the functions:
 3 | - Filesystem path manipulation
 4 | - Simple geometry utilities
 5 | - PCL visualization
 6 | - Various pointcloud segmentation algorithms
 7 | - Many more...
 8 | 
 9 | ### PCL visualization
10 | Folder `sample_pcl_viewer` contains a short example of how to use PCL visualization functions. To run it use:
11 | ```
12 | ./sample_pcl_viewer <path to a pcd file>
13 | ```
14 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # In-source CMake Cache & generated CMake files
 2 | CMakeCache.txt
 3 | Makefile
 4 | CMakeFiles
 5 | cmake_install.cmake
 6 | # (for eclipse)
 7 | .project
 8 | .cproject
 9 | 
10 | # Compiled Object files
11 | *.slo
12 | *.lo
13 | *.o
14 | *.obj
15 | 
16 | # Precompiled Headers
17 | *.gch
18 | *.pch
19 | 
20 | # Compiled Dynamic libraries
21 | *.so
22 | *.dylib
23 | *.dll
24 | 
25 | # Fortran module files
26 | *.mod
27 | 
28 | # Compiled Static libraries
29 | *.lai
30 | *.la
31 | *.a
32 | *.lib
33 | 
34 | # Executables
35 | *.exe
36 | *.out
37 | *.app
38 | 
39 | # Binary, library and build folders
40 | bin
41 | build
42 | lib
43 | 
44 | # Kdevelop
45 | *.kdev4
46 | .kdev4
47 | 
48 | # CLion
49 | .idea
50 | 


--------------------------------------------------------------------------------
/namaris-config.cmake.in:
--------------------------------------------------------------------------------
 1 | # ===================================================================================
 2 | #  The Namaris CMake configuration file
 3 | #
 4 | #             ** File generated automatically, do not modify **
 5 | #
 6 | #  Usage from an external project:
 7 | #    In your CMakeLists.txt, add these lines:
 8 | #
 9 | #    FIND_PACKAGE(namaris REQUIRED )
10 | #    INCLUDE_DIRECTORIES(${NAMARIS_INCLUDE_DIRS})
11 | #
12 | #
13 | #    This file will define the following variables:
14 | #      - NAMARIS_INCLUDE_DIRS   : The Namaris include directories.
15 | #
16 | # ===================================================================================
17 | 
18 | @PACKAGE_INIT@
19 | 
20 | set_and_check(NAMARIS_INCLUDE_DIRS "@PACKAGE_NAMARIS_INCLUDE_DIRS@")
21 | 


--------------------------------------------------------------------------------
/include/namaris/3rd_party/felzenswalb_segmentation/README:
--------------------------------------------------------------------------------
 1 | 
 2 | Implementation of the segmentation algorithm described in:
 3 | 
 4 | Efficient Graph-Based Image Segmentation
 5 | Pedro F. Felzenszwalb and Daniel P. Huttenlocher
 6 | International Journal of Computer Vision, 59(2) September 2004.
 7 | 
 8 | The program takes a color image (PPM format) and produces a segmentation
 9 | with a random color assigned to each region.
10 | 
11 | 1) Type "make" to compile "segment".
12 | 
13 | 2) Run "segment sigma k min input output".
14 | 
15 | The parameters are: (see the paper for details)
16 | 
17 | sigma: Used to smooth the input image before segmenting it.
18 | k: Value for the threshold function.
19 | min: Minimum component size enforced by post-processing.
20 | input: Input image.
21 | output: Output image.
22 | 
23 | Typical parameters are sigma = 0.5, k = 500, min = 20.
24 | Larger values for k result in larger components in the result.
25 | 
26 | 


--------------------------------------------------------------------------------
/include/namaris/utilities/pcl_typedefs.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef PCL_TYPEDEFS_HPP
 2 | #define PCL_TYPEDEFS_HPP
 3 | 
 4 | // PCL includes
 5 | #include <pcl/point_types.h>
 6 | 
 7 | // Point type definitions
 8 | // Naming convention:
 9 | //  T:    XYZ
10 | //  N:    normal
11 | //  C:    RGB
12 | //  L:    label
13 | typedef pcl::PointXYZ Point;
14 | typedef pcl::Normal Normal;
15 | typedef pcl::PointNormal PointN;
16 | typedef pcl::PointXYZRGB PointC;
17 | typedef pcl::PointXYZRGBA PointCA;
18 | typedef pcl::PointXYZRGBNormal PointNC;
19 | typedef pcl::PointXYZL PointL;
20 | 
21 | // bool operator== (const PointN &lhs, const PointN &rhs)
22 | // {
23 | //   return (  lhs.x == rhs.x  && 
24 | //             lhs.y == rhs.y  &&
25 | //             lhs.z == rhs.z  &&
26 | //             lhs.normal_x == rhs.normal_x  &&
27 | //             lhs.normal_y == rhs.normal_y  &&
28 | //             lhs.normal_z == rhs.normal_z
29 | //           );
30 | // }
31 | // 
32 | // bool operator!= (const PointN &lhs, const PointN &rhs)
33 | // {
34 | //   return !(lhs == rhs);
35 | // }
36 | 
37 | #endif  // PCL_TYPEDEFS_HPP


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | The MIT License (MIT)
 2 | 
 3 | Copyright (c) 2016 Aleksandrs Ecins
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/CMakeModules/CMakeUninstall.cmake.in:
--------------------------------------------------------------------------------
 1 | # Ignore empty list items.
 2 | cmake_policy(SET CMP0007 OLD)
 3 | 
 4 | if (NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt")
 5 |     message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_BINARY_DIR@/install_manifest.txt\"")
 6 | endif(NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt")
 7 | 
 8 | file(READ "@CMAKE_BINARY_DIR@/install_manifest.txt" files)
 9 | string(REGEX REPLACE "\n" ";" files "${files}")
10 | list(REVERSE files)
11 | foreach (file ${files})
12 |     message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
13 |     if (EXISTS "$ENV{DESTDIR}${file}")
14 |         execute_process(
15 |             COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}"
16 |             OUTPUT_VARIABLE rm_out
17 |             RESULT_VARIABLE rm_retval
18 |         )
19 |         if(NOT ${rm_retval} EQUAL 0)
20 |             message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
21 |         endif (NOT ${rm_retval} EQUAL 0)
22 |     else (EXISTS "$ENV{DESTDIR}${file}")
23 |         message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
24 |     endif (EXISTS "$ENV{DESTDIR}${file}")
25 | endforeach(file)
26 | 


--------------------------------------------------------------------------------
/include/namaris/utilities/pcl_visualization/color.h:
--------------------------------------------------------------------------------
 1 | #ifndef PCL_VISUALIZATION_COLOR_H
 2 | #define PCL_VISUALIZATION_COLOR_H
 3 | 
 4 | #include <vector>
 5 | 
 6 | namespace utl
 7 | {
 8 |   
 9 |   namespace pclvis
10 |   {
11 |     struct Color
12 |     {
13 |       double r;
14 |       double g;
15 |       double b;
16 |       
17 |       Color ()
18 |         : r (-1)
19 |         , g (-1)
20 |         , b (-1)
21 |       {};
22 |       
23 |       Color (const double r, const double g, const double b)
24 |         : r (r)
25 |         , g (g)
26 |         , b (b)
27 |       {};
28 |       
29 |       std::vector<double> toStdVec ()
30 |       {
31 |         std::vector<double> c (3);
32 |         c[0] = r; c[1] = g; c[2] = b;
33 |         return c;
34 |       }
35 |       
36 |       bool operator==(const Color& rhs) const
37 |       {
38 |         return (r == rhs.r && b == rhs.b && g == rhs.b);
39 |       }
40 |       
41 |       bool operator!=(const Color& rhs) const
42 |       {
43 |         return (r != rhs.r && b != rhs.b && g != rhs.b);
44 |       }
45 |     };
46 |         
47 |     typedef std::vector<Color> Colors;
48 |   }
49 | }
50 | 
51 | #endif  // PCL_VISUALIZATION_COLOR_H


--------------------------------------------------------------------------------
/include/namaris/3rd_party/felzenswalb_segmentation/disjoint-set.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (C) 2006 Pedro Felzenszwalb
 3 | 
 4 | This program is free software; you can redistribute it and/or modify
 5 | it under the terms of the GNU General Public License as published by
 6 | the Free Software Foundation; either version 2 of the License, or
 7 | (at your option) any later version.
 8 | 
 9 | This program is distributed in the hope that it will be useful,
10 | but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | GNU General Public License for more details.
13 | 
14 | You should have received a copy of the GNU General Public License
15 | along with this program; if not, write to the Free Software
16 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
17 | */
18 | 
19 | #ifndef DISJOINT_SET_H
20 | #define DISJOINT_SET_H
21 | 
22 | // disjoint-set forests using union-by-rank and path compression (sort of).
23 | 
24 | typedef struct {
25 |   int rank;
26 |   int p;
27 |   int size;
28 | } uni_elt;
29 | 
30 | class universe {
31 | public:
32 |   universe(int elements);
33 |   ~universe();
34 |   int find(int x);  
35 |   void join(int x, int y);
36 |   int size(int x) const { return elts[x].size; }
37 |   int num_sets() const { return num; }
38 | 
39 | private:
40 |   uni_elt *elts;
41 |   int num;
42 | };
43 | 
44 | #endif  // DISJOINT_SET_H
45 | 


--------------------------------------------------------------------------------
/include/namaris/3rd_party/felzenswalb_segmentation/segment-graph.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (C) 2006 Pedro Felzenszwalb
 3 | 
 4 | This program is free software; you can redistribute it and/or modify
 5 | it under the terms of the GNU General Public License as published by
 6 | the Free Software Foundation; either version 2 of the License, or
 7 | (at your option) any later version.
 8 | 
 9 | This program is distributed in the hope that it will be useful,
10 | but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | GNU General Public License for more details.
13 | 
14 | You should have received a copy of the GNU General Public License
15 | along with this program; if not, write to the Free Software
16 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
17 | */
18 | 
19 | #ifndef SEGMENT_GRAPH_H
20 | #define SEGMENT_GRAPH_H
21 | 
22 | #include <algorithm>
23 | #include <cmath>
24 | #include "disjoint-set.hpp"
25 | 
26 | // threshold function
27 | #define THRESHOLD(size, c) (c/size)
28 | 
29 | typedef struct {
30 |   float w;
31 |   int a, b;
32 | } edge;
33 | 
34 | /*
35 |  * Segment a graph
36 |  *
37 |  * Returns a disjoint-set forest representing the segmentation.
38 |  *
39 |  * num_vertices: number of vertices in graph.
40 |  * num_edges: number of edges in graph
41 |  * edges: array of edges.
42 |  * c: constant for treshold function.
43 |  */
44 | universe *segment_graph(int num_vertices, int num_edges, edge *edges, float c);
45 | 
46 | #endif  // SEGMENT_GRAPH_H
47 | 


--------------------------------------------------------------------------------
/sample_pcl_viewer/include/vis.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef VIS_HPP_
 2 | #define VIS_HPP_
 3 | 
 4 | #include <namaris/utilities/pcl_visualization.hpp>
 5 | 
 6 | // State
 7 | struct VisState
 8 | {
 9 |   VisState ()
10 |     : cloudDisplayState_ (CLOUD_NO_COLOR)
11 |     , showNormals_(false)
12 |     , updateDisplay_(true)
13 |     , pointSize_ (4.0)
14 |   { };
15 |   
16 |   enum CloudDisplay { CLOUD_NO_COLOR, CLOUD_COLOR, CLOUD_DISTANCE_TO_ORIGIN };
17 |   
18 |   CloudDisplay cloudDisplayState_;
19 |   bool showNormals_;
20 |   bool updateDisplay_;
21 |   float pointSize_;
22 | };
23 | 
24 | // Callback
25 | void keyboard_callback (const pcl::visualization::KeyboardEvent &event, void *cookie)
26 | {
27 |   VisState* visState = reinterpret_cast<VisState*> (cookie);
28 |   
29 |   if (event.keyUp ())
30 |   {    
31 |     std::string key = event.getKeySym ();
32 | //     cout << key << " key pressed!\n";
33 |     
34 |     visState->updateDisplay_ = true;
35 |     
36 |     if (key == "KP_1")
37 |       visState->cloudDisplayState_ = VisState::CLOUD_NO_COLOR;
38 |     else if (key == "KP_2")
39 |       visState->cloudDisplayState_ = VisState::CLOUD_COLOR;
40 |     else if (key == "KP_3")
41 |       visState->cloudDisplayState_ = VisState::CLOUD_DISTANCE_TO_ORIGIN;
42 | 
43 |     // Point size
44 |     else if (key == "KP_Add")
45 |       visState->pointSize_ += 1.0;
46 |     else if (key == "KP_Subtract")
47 |       visState->pointSize_ = std::max(0.0, visState->pointSize_ - 1.0);    
48 |         
49 |     // More stuff
50 |     else if ((key == "n") || (key == "N"))
51 |       visState->showNormals_ = !visState->showNormals_;
52 |     
53 |     else
54 |       visState->updateDisplay_ = false;
55 |   }
56 | }
57 | 
58 | #endif // VIS_HPP_


--------------------------------------------------------------------------------
/include/namaris/3rd_party/felzenswalb_segmentation/disjoint-set.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (C) 2006 Pedro Felzenszwalb
 3 | 
 4 | This program is free software; you can redistribute it and/or modify
 5 | it under the terms of the GNU General Public License as published by
 6 | the Free Software Foundation; either version 2 of the License, or
 7 | (at your option) any later version.
 8 | 
 9 | This program is distributed in the hope that it will be useful,
10 | but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | GNU General Public License for more details.
13 | 
14 | You should have received a copy of the GNU General Public License
15 | along with this program; if not, write to the Free Software
16 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
17 | */
18 | 
19 | #ifndef DISJOINT_SET_HPP
20 | #define DISJOINT_SET_HPP
21 | 
22 | #include "disjoint-set.h"
23 | 
24 | universe::universe(int elements) {
25 |   elts = new uni_elt[elements];
26 |   num = elements;
27 |   for (int i = 0; i < elements; i++) {
28 |     elts[i].rank = 0;
29 |     elts[i].size = 1;
30 |     elts[i].p = i;
31 |   }
32 | }
33 |   
34 | universe::~universe() {
35 |   delete [] elts;
36 | }
37 | 
38 | int universe::find(int x) {
39 |   int y = x;
40 |   while (y != elts[y].p)
41 |     y = elts[y].p;
42 |   elts[x].p = y;
43 |   return y;
44 | }
45 | 
46 | void universe::join(int x, int y) {
47 |   if (elts[x].rank > elts[y].rank) {
48 |     elts[y].p = x;
49 |     elts[x].size += elts[y].size;
50 |   } else {
51 |     elts[x].p = y;
52 |     elts[y].size += elts[x].size;
53 |     if (elts[x].rank == elts[y].rank)
54 |       elts[y].rank++;
55 |   }
56 |   num--;
57 | }
58 | 
59 | #endif  // DISJOINT_SET_HPP


--------------------------------------------------------------------------------
/include/namaris/3rd_party/felzenswalb_segmentation/segment-graph.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (C) 2006 Pedro Felzenszwalb
 3 | 
 4 | This program is free software; you can redistribute it and/or modify
 5 | it under the terms of the GNU General Public License as published by
 6 | the Free Software Foundation; either version 2 of the License, or
 7 | (at your option) any later version.
 8 | 
 9 | This program is distributed in the hope that it will be useful,
10 | but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | GNU General Public License for more details.
13 | 
14 | You should have received a copy of the GNU General Public License
15 | along with this program; if not, write to the Free Software
16 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
17 | */
18 | 
19 | #ifndef SEGMENT_GRAPH_HPP
20 | #define SEGMENT_GRAPH_HPP
21 | 
22 | #include "segment-graph.h"
23 | 
24 | bool operator<(const edge &a, const edge &b) {
25 |   return a.w < b.w;
26 | }
27 | 
28 | universe *segment_graph(int num_vertices, int num_edges, edge *edges, 
29 |       float c) { 
30 |   // sort edges by weight
31 |   std::sort(edges, edges + num_edges);
32 | 
33 |   // make a disjoint-set forest
34 |   universe *u = new universe(num_vertices);
35 | 
36 |   // init thresholds
37 |   float *threshold = new float[num_vertices];
38 |   for (int i = 0; i < num_vertices; i++)
39 |     threshold[i] = THRESHOLD(1,c);
40 | 
41 |   // for each edge, in non-decreasing weight order...
42 |   for (int i = 0; i < num_edges; i++) {
43 |     edge *pedge = &edges[i];
44 |     
45 |     // components conected by this edge
46 |     int a = u->find(pedge->a);
47 |     int b = u->find(pedge->b);
48 |     if (a != b) {
49 |       if ((pedge->w <= threshold[a]) && (pedge->w <= threshold[b])) {
50 |         u->join(a, b);
51 |         a = u->find(a);
52 |         threshold[a] = pedge->w + THRESHOLD(u->size(a), c);
53 |       }
54 |     }
55 |   }
56 | 
57 |   // free up
58 |   delete threshold;
59 |   return u;
60 | }
61 | 
62 | #endif  // SEGMENT_GRAPH_HPP


--------------------------------------------------------------------------------
/include/namaris/3rd_party/meanshift/Readme.txt:
--------------------------------------------------------------------------------
 1 | ===============================================================================
 2 | Nonlinear Mean Shift over Riemannian Manifolds
 3 | Author: Raghav Subbarao
 4 | Robust Image Understanding Laboratory, Rutgers University
 5 | ===============================================================================
 6 | 
 7 | Implements the Nonlinear Mean Shift algorithm:
 8 | R. Subbarao, P. Meer, "Nonlinear Mean Shift",
 9 | Submitted to, IEEE Transactions on Pattern Analysis and MAchine Intelligence
10 | 
11 | Examples for using the base class for mean shift over SE(3), the essential manifold anf the Grassmann Manifold are provided in the code. The base case Euclidean Mean Shift has also been implemented.
12 | 
13 | Using the binary:
14 | 
15 | Euclidean:
16 | 	newmeanshift -euc [DATAFILE] [band width]
17 | 
18 | 
19 | SE(3):
20 | 	newmeanshift -se3 [DATAFILE] [band width] [Translation scaling]
21 | 
22 | 	The translation scaling can be used to scale the units of translation. This will default to 25 if nothing is given.
23 | 
24 | 
25 | Essential:
26 | 	newmeanshift -ess [DATAFILE] [band width]
27 | 
28 | Grassmann:
29 | 	newmeanshift -gmn [DATAFILE] [band width] [n] [k]
30 | 	The [n] and [k] are the parameters of the Grassmann manifold.
31 | 
32 | DATAFILE format (see also the examples in MeanShift/Data/):
33 | nRows nColumns
34 | input data points, each row is a point.
35 | 
36 | The pruned modes are written out into a file name prunedModes.txt
37 | 
38 | Extending mean shift to new manifolds:
39 | 	Derive a new class from the CGeometry abstract Class for performing operations over manifolds. Three pure virutal 	functions have to be defined for each concrete subclass, expm(manifold exponential), logm(manifold log) and 	g(Riemannian inner product).
40 | 	Derive a new class from the CPointSet class which is a contained for the data. Any perprocessing can be done while 		loading points into this class.
41 | 	The mean shift function can then be used to perform mean shift.
42 | 
43 | 
44 | Raghav Subbarao
45 | rsubbara@caip.rutgers.edu
46 | 
47 | Robust Image Understanding Laboratory
48 | www.caip.rutgers.edu/riul


--------------------------------------------------------------------------------
/include/namaris/algorithms/felzenswalb_graph_segmentation/felzenswalb_graph_segmentation.h:
--------------------------------------------------------------------------------
 1 | #ifndef FELZENSWALB_GRAPH_SEGMENTATION_H
 2 | #define FELZENSWALB_GRAPH_SEGMENTATION_H
 3 | 
 4 | #include <namaris/3rd_party/felzenswalb_segmentation/segment-graph.hpp>
 5 | 
 6 | namespace alg
 7 | {
 8 |   /** \brief @b FelzenswalbGraphSegmentation performs segmentation of a weighted
 9 |    * graph using the approach described in
10 |    * "Efficient Graph-Based Image Segmentation" by Felzenswalb et al.
11 |    */  
12 |   class FelzenswalbGraphSegmentation
13 |   {
14 |   public:
15 |     
16 |     /** \brief Empty constructor. */
17 |     FelzenswalbGraphSegmentation ();
18 |     
19 |     /** \brief Desctructor. */
20 |     ~FelzenswalbGraphSegmentation ();
21 |     
22 |     /** \brief Set the segmentation threshold controlling the over/under segmentation. */
23 |     bool setThreshold ( const float threshold );
24 |     
25 |     /** \brief Set the size of the smallest valid segment. */
26 |     bool setMinSegmentSize ( const size_t min_seg_size );
27 | 
28 |     /** \brief Set the size of the smallest valid segment. */
29 |     bool setMaxSegmentSize ( const size_t max_seg_size );
30 |     
31 |     /** \brief Set the edges describing the graph strucutre.
32 |      *  \param[in]   edges   vector of pairs of integers where integers correspond to indices of of the vertices connected by an edge
33 |      *  \param[in]   weights vector of floats where each float corresponds to a weight of an edge.
34 |      *  \note vertex ids must be positive
35 |      *  \note edges and weights must be same size
36 |      */
37 |     bool setGraph (const std::vector<std::pair<int, int> > &edges, const std::vector<float> &weights);
38 |     
39 |     /** \brief Segment the graph.
40 |      *  \param[out]   segments  a vector of segments where each segment is represented by the indices of points belonging to it
41 |      */    
42 |     bool segment (std::vector<std::vector<int> > &segments);
43 |     
44 |   private:
45 |     
46 |     // Member variables
47 |     int num_edges_;
48 |     int num_vertices_;
49 |     size_t min_seg_size_;
50 |     size_t max_seg_size_;
51 |     float threshold_;
52 |     edge *edges_;
53 |     std::vector<int> vertices_;
54 |   };
55 | }
56 | 
57 | #endif // FELZENSWALB_GRAPH_SEGMENTATION_H
58 | 


--------------------------------------------------------------------------------
/include/namaris/3rd_party/meanshift/VectorPointSet.h:
--------------------------------------------------------------------------------
 1 | /////////////////////////////////////////////////////////////////////////////
 2 | // Name:        VectorPointSet.h
 3 | // Purpose:     Container for points represented as they are
 4 | // Author:      Raghav Subbarao
 5 | // Modified by:
 6 | // Created:     10/01/2008
 7 | // Copyright:   (c) Raghav Subbarao
 8 | // Version:     v0.1
 9 | /////////////////////////////////////////////////////////////////////////////
10 | 
11 | #ifndef _C_VECTOR_POINT_SET_
12 | #define _C_VECTOR_POINT_SET_
13 | 
14 | #include "PointSet.h"
15 | 
16 | template<typename T> class CVectorPointSet : public CPointSet<T>{
17 | 
18 | public:
19 | 
20 | 	// Empty Constructor
21 | 	CVectorPointSet();
22 | 
23 | 	// Standard Constructor
24 | 	// d = dimension of data
25 | 	// n = number of points to allocate memory for.
26 | 	CVectorPointSet(int d, int n);
27 | 
28 | 	// Standard Constructor with data
29 | 	// d = dimension of data
30 | 	// n = number of points to allocate memory for.
31 | 	// data = data to be copied.
32 | 	CVectorPointSet(int d, int n, const T* data);
33 | 
34 | 	// TODO: change for reference coutning
35 | 	// Copy Constructor
36 | 	CVectorPointSet(CVectorPointSet<T>& ps);
37 | 
38 | 	// Destructor
39 | 	virtual ~CVectorPointSet(){};
40 | 
41 | 	// Copies point directly
42 | 	inline void loadPoint(T *x);
43 | 
44 | 	// Copies point directly into x.
45 | 	// Note: assumes space has been allocated for x.
46 | 	inline void returnPoint(T *x, int n);
47 | 
48 | };
49 | 
50 | template<typename T> CVectorPointSet<T>::CVectorPointSet():
51 | CPointSet<T>()
52 | {}
53 | 
54 | template<typename T> CVectorPointSet<T>::CVectorPointSet(int d, int n):
55 | CPointSet<T>(d, d, n)
56 | {}
57 | 
58 | template<typename T> CVectorPointSet<T>::CVectorPointSet(int d, int n, const T *data):
59 | CPointSet<T>(d, d, n, data)
60 | {}
61 | 
62 | template<typename T> CVectorPointSet<T>::CVectorPointSet(CVectorPointSet<T>& ps):
63 | CPointSet<T>(ps.m_ps_dim, ps.m_ps_dim, ps.m_ps_nmax)
64 | {
65 | 	this->m_ps_n			=	ps.m_ps_n;
66 | 	this->m_ps_loc		=	ps.m_ps_loc;
67 | 	memcpy(this->m_ps_points, ps.m_ps_points, sizeof(T) * this->m_ps_n * this->m_ps_dim);
68 | }
69 | 
70 | template<typename T> inline void CVectorPointSet<T>::loadPoint(T* x){
71 | 
72 | 	if(this->m_ps_n == this->m_ps_nmax)
73 | 		return;
74 | 
75 | 	memcpy(this->m_ps_points + this->m_ps_n * this->m_ps_dim, x, sizeof(T) * this->m_ps_dim);
76 | 	this->m_ps_n++;
77 | 
78 | }
79 | 
80 | template<typename T> inline void CVectorPointSet<T>::returnPoint(T* x, int n){
81 | 
82 | 	if(n >= this->m_ps_n)
83 | 		return;
84 | 
85 | 	memcpy(x, this->m_ps_points + n * this->m_ps_dim, sizeof(T) * this->m_ps_dim);
86 | 
87 | }
88 | 
89 | #endif
90 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.8)
 2 | PROJECT( namaris )
 3 | 
 4 | # version (e.g. for packaging)
 5 | set (NAMARIS_MAJOR_VERSION 0)
 6 | set (NAMARIS_MINOR_VERSION 1)
 7 | set (NAMARIS_PATCH_VERSION 0)
 8 | set (NAMARIS_VERSION ${NAMARIS_MAJOR_VERSION}.${NAMARIS_MINOR_VERSION}.${NAMARIS_PATCH_VERSION})
 9 | if (COMMAND cmake_policy)
10 |   cmake_policy (SET CMP0003 NEW)
11 | endif (COMMAND cmake_policy)
12 | 
13 | # Additional CMake modules
14 | SET (CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/CMakeModules")
15 | 
16 | # Set include directories
17 | set (INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/include")
18 | INCLUDE_DIRECTORIES (${INCLUDE_DIRS})
19 | 
20 | # Installation
21 | install (
22 |   DIRECTORY ${PROJECT_SOURCE_DIR}/include/namaris/
23 |   DESTINATION include/namaris
24 |   FILES_MATCHING
25 |     PATTERN "*.hpp"
26 |     PATTERN "*.h" )
27 | 
28 | # uninstall target (this does not delete the directories created during installation)
29 | configure_file (
30 |     "${PROJECT_SOURCE_DIR}/CMakeModules/CMakeUninstall.cmake.in"
31 |     "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
32 |     IMMEDIATE @ONLY )
33 | 
34 | add_custom_target(uninstall
35 |     COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
36 | 
37 | # Export the package for use from the build-tree
38 | # (this registers the build-tree with a global CMake-registry)
39 | export(PACKAGE namaris)
40 | 
41 | # Create a octomap-config.cmake file for the use from the build tree
42 | set(NAMARIS_INCLUDE_DIRS "${INCLUDE_DIRS}")
43 | 
44 | include(CMakePackageConfigHelpers)
45 | 
46 | CONFIGURE_PACKAGE_CONFIG_FILE (
47 |   namaris-config.cmake.in
48 |   "${CMAKE_CURRENT_BINARY_DIR}/namaris-config.cmake"
49 |   PATH_VARS NAMARIS_INCLUDE_DIRS
50 |   INSTALL_DESTINATION ${CMAKE_INSTALL_PREFIX}/share/namaris)
51 | 
52 | WRITE_BASIC_PACKAGE_VERSION_FILE (
53 |   "${CMAKE_CURRENT_BINARY_DIR}/namaris-config-version.cmake"
54 |   VERSION ${NAMARIS_VERSION}
55 |   COMPATIBILITY AnyNewerVersion )
56 | 
57 | # Create a octomap-config.cmake file for the use from the install tree
58 | # and install it
59 | set(NAMARIS_INCLUDE_DIRS "${CMAKE_INSTALL_PREFIX}/include")
60 | 
61 | CONFIGURE_PACKAGE_CONFIG_FILE(
62 |   namaris-config.cmake.in
63 |   "${PROJECT_BINARY_DIR}/InstallFiles/namaris-config.cmake"
64 |   PATH_VARS NAMARIS_INCLUDE_DIRS
65 |   INSTALL_DESTINATION ${CMAKE_INSTALL_PREFIX}/share/namaris)
66 | 
67 | WRITE_BASIC_PACKAGE_VERSION_FILE(
68 |   ${PROJECT_BINARY_DIR}/InstallFiles/namaris-config-version.cmake
69 |   VERSION ${NAMARIS_VERSION}
70 |   COMPATIBILITY AnyNewerVersion)
71 | 
72 | install(FILES
73 |   "${PROJECT_BINARY_DIR}/InstallFiles/namaris-config.cmake"
74 |   "${PROJECT_BINARY_DIR}/InstallFiles/namaris-config-version.cmake" 
75 |   DESTINATION share/namaris/)
76 | 
77 | # Write pkgconfig-file:
78 | include (InstallPkgConfigFile)
79 | install_pkg_config_file (namaris
80 |     CFLAGS
81 |     REQUIRES
82 |     VERSION ${OCTOMAP_VERSION})
83 | 


--------------------------------------------------------------------------------
/CMakeModules/InstallPkgConfigFile.cmake:
--------------------------------------------------------------------------------
 1 | # A Macro to simplify creating a pkg-config file
 2 | 
 3 | # install_pkg_config_file(<package-name> 
 4 | #                              [VERSION <version>]
 5 | #                              [DESCRIPTION <description>]
 6 | #                              [CFLAGS <cflag> ...]
 7 | #                              [LIBS <lflag> ...]
 8 | #                              [REQUIRES <required-package-name> ...])
 9 | # 
10 | # Create and install a pkg-config .pc file to CMAKE_INSTALL_PREFIX/lib/pkgconfig
11 | #	assuming the following install layout:
12 | #	       libraries:   CMAKE_INSTALL_PREFIX/lib
13 | #	       headers  :   CMAKE_INSTALL_PREFIX/include
14 | #
15 | # example:
16 | #    add_library(mylib mylib.c)
17 | #    install_pkg_config_file(mylib 
18 | #			     	DESCRIPTION My Library
19 | #			     	CFLAGS
20 | #			     	LIBS -lmylib 
21 | #			     	REQUIRES glib-2.0 lcm
22 | # 			     	VERSION 0.0.1)
23 | #
24 | #
25 | function(install_pkg_config_file)
26 |     list(GET ARGV 0 pc_name)
27 |     # TODO error check
28 | 
29 |     set(pc_version 0.0.1)
30 |     set(pc_description ${pc_name})
31 |     set(pc_requires "")
32 |     set(pc_libs "")
33 |     set(pc_cflags "")
34 |     set(pc_fname "${CMAKE_BINARY_DIR}/lib/pkgconfig/${pc_name}.pc")
35 |     
36 |     set(modewords LIBS CFLAGS REQUIRES VERSION DESCRIPTION)
37 |     set(curmode "")
38 | 
39 |     # parse function arguments and populate pkg-config parameters
40 |     list(REMOVE_AT ARGV 0)
41 |     foreach(word ${ARGV})
42 |         list(FIND modewords ${word} mode_index)
43 |         if(${mode_index} GREATER -1)
44 |             set(curmode ${word})
45 |         elseif(curmode STREQUAL LIBS)
46 |             set(pc_libs "${pc_libs} ${word}")
47 |         elseif(curmode STREQUAL CFLAGS)
48 |             set(pc_cflags "${pc_cflags} ${word}")
49 |         elseif(curmode STREQUAL REQUIRES)
50 |             set(pc_requires "${pc_requires} ${word}")
51 |         elseif(curmode STREQUAL VERSION)
52 |             set(pc_version ${word})
53 |             set(curmode "")
54 |         elseif(curmode STREQUAL DESCRIPTION)
55 |             set(pc_description "${word}")
56 |             set(curmode "")
57 |         else(${mode_index} GREATER -1)
58 |             message("WARNING incorrect use of install_pkg_config_file (${word})")
59 |             break()
60 |         endif(${mode_index} GREATER -1)
61 |     endforeach(word)
62 | 
63 |     # write the .pc file out
64 |     file(WRITE ${pc_fname}
65 |         "prefix=${CMAKE_INSTALL_PREFIX}\n"
66 |         "libdir=\${prefix}/lib\n"
67 |         "includedir=\${prefix}/include\n"
68 |         "\n"
69 |         "Name: ${pc_name}\n"
70 |         "Description: ${pc_description}\n"
71 |         "Requires: ${pc_requires}\n"
72 |         "Version: ${pc_version}\n"
73 |         "Libs: -L\${libdir} ${pc_libs}\n"
74 |         "Cflags: -I\${includedir} ${pc_cflags}\n")
75 | 
76 |     # mark the .pc file for installation to the lib/pkgconfig directory
77 |     install(FILES ${pc_fname} DESTINATION lib/pkgconfig)    
78 | endfunction(install_pkg_config_file)
79 | 


--------------------------------------------------------------------------------
/include/namaris/utilities/string.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef STRING_UTILITIES_HPP
 2 | #define STRING_UTILITIES_HPP
 3 | 
 4 | // STD includes
 5 | #include <iostream>
 6 | #include <iomanip>
 7 | #include <sstream>
 8 | #include <vector>
 9 | 
10 | namespace utl
11 | {
12 |   namespace str
13 |   {
14 |     /** \brief Convert a decimal number to a string of fixed length with zero padding on the left.
15 |      *  \param[in] decimal_number number to convert to string
16 |      *  \param[in] decimal_places number of decimal places for a floating point number
17 |      */
18 |     template <typename T> inline
19 |     std::string to_padded_string (const  T decimal_number, const unsigned int padded_width = 6)
20 |     {
21 |       std::ostringstream out;
22 |       out << std::setfill('0') << std::setw(padded_width);
23 |       out << decimal_number;
24 |       return out.str();
25 |     }
26 |     /** \brief Convert a number to a string
27 |      *  \param[in] number string which is modified
28 |      *  \param[in] decimal_places number of decimal places for a floating point number
29 |      */
30 |     template <typename T> inline
31 |     std::string to_string (const  T number, const unsigned int decimal_places = 6)
32 |     {
33 |         std::ostringstream out;
34 |         out << std::fixed;
35 |         out << std::setprecision(decimal_places) << number;
36 |         return out.str();
37 |     }    
38 |     
39 |     /** \brief Replace all occurences of a substring with a new substring
40 |      *  \param[in,out] string_in string which is modified
41 |      *  \param[in] substring_orig substring that needs to be replaced
42 |      *  \param[in] substring_new new substring
43 |      */
44 |     inline
45 |     void replaceSubstring(std::string &string_in, const std::string &substring_orig, const std::string &substring_new)
46 |     {
47 |       std::string::size_type n = 0;
48 |       while ( ( n = string_in.find( substring_orig, n ) ) != std::string::npos )
49 |       {
50 |           string_in.replace( n, substring_orig.size(), substring_new );
51 |           n += substring_new.size();
52 |       }  
53 |     }
54 | 
55 |     /** \brief Split a string by a delimeter
56 |      *  \param[in] path string to be separated
57 |      *  \param[in] delimiter delimiter
58 |      *  \return a vector of delimited substrings
59 |      *  \NOTE: http://stackoverflow.com/questions/14265581/parse-split-a-string-in-c-using-string-delimiter-standard-c
60 |      */
61 |     inline
62 |     std::vector<std::string> splitString (const std::string &line, const std::string delimiter = " ")
63 |     {
64 |       std::vector<std::string> substrings;
65 |       
66 |       auto start = 0U;
67 |       auto end = line.find(delimiter);
68 |       while (end != std::string::npos)
69 |       {
70 |         substrings.push_back(line.substr(start, end - start));
71 |         start = end + delimiter.length();
72 |         end = line.find(delimiter, start);
73 |       }
74 |       
75 |       end = line.length();
76 |       substrings.push_back(line.substr(start, end - start));
77 |       
78 |       return substrings;
79 |     }  
80 |   }
81 | }
82 | 
83 | #endif    // STRING_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/pcl_visualization/vtk_colormaps.h:
--------------------------------------------------------------------------------
 1 | #ifndef VTK_COLORMAPS_H
 2 | #define VTK_COLORMAPS_H
 3 | 
 4 | // VTK
 5 | #include <vtkSmartPointer.h>
 6 | #include <vtkLookupTable.h>
 7 | 
 8 | // Eigen
 9 | #include <Eigen/Dense>
10 | 
11 | // CPP tools
12 | #include <namaris/utilities/pcl_visualization/color.h>
13 | #include <namaris/utilities/pcl_visualization/vtk_colormaps_lut.hpp>
14 | 
15 | 
16 | namespace utl
17 | {
18 |   namespace pclvis
19 |   {
20 |     class Colormap
21 |     {    
22 |       public:
23 |         
24 |         enum COLORMAP_TYPE {VTK_DEFAULT, JET, GRAYSCALE};
25 |         
26 |         // Constructors
27 |         Colormap ()
28 |           : colormapType_ (JET)
29 |           , minVal_ (std::numeric_limits<double>::quiet_NaN())
30 |           , maxVal_ (std::numeric_limits<double>::quiet_NaN())
31 |         {};
32 |         
33 |         Colormap (COLORMAP_TYPE colormapType)
34 |           : colormapType_ (colormapType)
35 |           , minVal_ (std::numeric_limits<double>::quiet_NaN())
36 |           , maxVal_ (std::numeric_limits<double>::quiet_NaN())
37 |         {};
38 |         
39 |         Colormap (COLORMAP_TYPE colormapType, double minVal, double maxVal)
40 |           : colormapType_ (colormapType)
41 |           , minVal_ (minVal)
42 |           , maxVal_ (maxVal)
43 |         {};    
44 |               
45 |         // Set colormap type
46 |         void setColormapType (COLORMAP_TYPE colormapType);
47 |         
48 |         // Manually set range limits
49 |         void setRangeLimits (double minVal, double maxVal);
50 |         
51 |         // Reset range limits to undefined
52 |         void resetRangeLimits();
53 | 
54 |         // Set range limits from data
55 |         template <typename Scalar>
56 |         void setRangeLimitsFromData (const Eigen::Matrix<Scalar, 1, Eigen::Dynamic> &data);
57 | 
58 |         // Set range limits from data
59 |         template <typename Scalar>
60 |         void setRangeLimitsFromData (const std::vector<Scalar> &data);
61 | 
62 |         // Get vtk lookup table representing the colormap      
63 |         vtkSmartPointer<vtkLookupTable> getColorLookupTable() const;
64 |         
65 |         // Map each element of a scalar vector to RGB values in [0,1] range given the colormap
66 |         template <typename Scalar>
67 |         Colors getColorsFromData (const Eigen::Matrix<Scalar, 1, Eigen::Dynamic> &data);
68 | 
69 |         // Map each element of a scalar vector to RGB values in [0,1] range given the colormap
70 |         template <typename Scalar>
71 |         Colors getColorsFromData (const std::vector<Scalar> &data);
72 |         
73 |       private:
74 |         
75 |         static const int COLORMAP_NUM_DIVISIONS = 256;
76 |         COLORMAP_TYPE colormapType_;
77 |         double minVal_;
78 |         double maxVal_;
79 |         
80 |         // Get specific colormaps
81 |         vtkSmartPointer<vtkLookupTable> getLUT_Jet () const;
82 |         vtkSmartPointer<vtkLookupTable> getLUT_VtkDefault () const;
83 |         vtkSmartPointer<vtkLookupTable> getLUT_Grayscale () const;
84 |         
85 |     };
86 |   }
87 | }
88 | 
89 | #endif  // VTK_COLORMAPS_H


--------------------------------------------------------------------------------
/sample_pcl_viewer/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 2 | 
 3 | project(sample_pcl_viewer)
 4 | set (CMAKE_BUILD_TYPE Debug)
 5 | set (CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/../bin")
 6 | add_definitions("-Wall -pedantic")
 7 | 
 8 | ### ----------------------------------------------------------------------------
 9 | ### Add CPP tools
10 | ### ----------------------------------------------------------------------------
11 | 
12 | set (CPP_TOOLS_INCLUDE_DIRS "../include")    # Make sure this points to cpp_tools directory
13 | include_directories (${CPP_TOOLS_INCLUDE_DIRS})
14 | 
15 | ### ----------------------------------------------------------------------------
16 | ### PCL dependent options
17 | ### ----------------------------------------------------------------------------
18 | 
19 | find_package(PCL REQUIRED)
20 | message (STATUS "")
21 | if (PCL_FOUND)
22 |   message (STATUS "")
23 |   message (STATUS " -----------------------")
24 | 	message (STATUS " Found PCL version ${PCL_VERSION}")
25 |   message (STATUS "     * include dirs: ${PCL_INCLUDE_DIRS}")
26 |   message (STATUS "     * library dirs: ${PCL_LIBRARY_DIRS}")
27 |   message (STATUS "     * libraries:    ${PCL_LIBRARIES}")
28 | else()
29 | 	message (FATAL_ERROR : "PCL NOT FOUND!")
30 | endif()
31 | message (STATUS "")
32 | 
33 | include_directories(${PCL_INCLUDE_DIRS})
34 | link_directories(${PCL_LIBRARY_DIRS})
35 | add_definitions(${PCL_DEFINITIONS})
36 | 
37 | ### ----------------------------------------------------------------------------
38 | ### BOOST
39 | ### ----------------------------------------------------------------------------
40 | 
41 | find_package(Boost REQUIRED COMPONENTS regex) 
42 | 
43 | if(Boost_FOUND)
44 |   message (STATUS " --------------------------")
45 |   message (STATUS " Found Boost version ${Boost_VERSION}")
46 |   message (STATUS "     * include dirs: ${Boost_INCLUDE_DIRS}")
47 |   message (STATUS "     * libraries:    ${Boost_LIBRARIES}")
48 | endif()
49 | 
50 | ### ----------------------------------------------------------------------------
51 | ### Project dependencies
52 | ### ----------------------------------------------------------------------------
53 | 
54 | set (includes ./include
55 |               ../cpp_tools)
56 | include_directories (${includes})
57 | 
58 | ### ----------------------------------------------------------------------------
59 | ### C++11 support
60 | ### ----------------------------------------------------------------------------
61 | 
62 | include(CheckCXXCompilerFlag)
63 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
64 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
65 | if(COMPILER_SUPPORTS_CXX11)
66 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
67 | elseif(COMPILER_SUPPORTS_CXX0X)
68 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
69 | else()
70 |   message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
71 | endif()
72 | 
73 | ### ----------------------------------------------------------------------------
74 | ### Executables
75 | ### ----------------------------------------------------------------------------
76 | 
77 | add_executable (sample_pcl_viewer src/main.cpp )
78 | target_link_libraries (sample_pcl_viewer ${PCL_LIBRARIES} ${Boost_LIBRARIES})


--------------------------------------------------------------------------------
/include/namaris/algorithms/region_growing_smoothness/region_growing_smoothness.h:
--------------------------------------------------------------------------------
 1 | #ifndef REGION_GROWING_SMOOTHNESS_H
 2 | #define REGION_GROWING_SMOOTHNESS_H
 3 | 
 4 | // PCL includes
 5 | #include <pcl/common/angles.h>
 6 | 
 7 | // Algorithms includes
 8 | #include <namaris/algorithms/region_growing/region_growing.hpp>
 9 | 
10 | namespace alg
11 | {
12 |   /** \brief @b RegionGrowingSmoothness Performs region growing
13 |    * segmentation on a pointcloud using a surface normal smoothness constraint. 
14 |    * This is essintially a @b RegionGrowing class with a predefined binary
15 |    * condition function.
16 |    * 
17 |    * Given a point in the cloud it's neighboring points are found. For all of 
18 |    * them a two conditions are verified:
19 |    *  1) unary condition determining if a neighboring point is a valid candidate
20 |    *  (can bes specified by the user)
21 |    *  2) binary condition between the original point and a neighboring point. 
22 |    *  Points belong to the same cluster if the angle between their surface
23 |    *  normals is smaller than a user specified threshold.
24 |    * 
25 |    * If the fraction of neighbors satisfying the unary constraint is larger than
26 |    * a threshold and the fraction of neighbors satisfying the binary constraint 
27 |    * is larger than (a different) threshold neighboring points satisfying both 
28 |    * conditions are added to the cluster. The same procedure is run on the newly
29 |    * added points. Once this process converges it is run again on the remaining
30 |    * points in the cloud (i.e. points that were not yet assigned to any cluster)
31 |    * .
32 |    */
33 |   template<typename PointT>
34 |   class RegionGrowingSmoothness : public alg::RegionGrowing<PointT>
35 |   {   
36 |     protected:
37 |       using alg::RegionGrowing<PointT>::setBinaryConditionFunction;
38 |       using alg::RegionGrowing<PointT>::input_;
39 |       using alg::RegionGrowing<PointT>::indices_;
40 |       
41 |     public:
42 |       /** \brief Constructor */
43 |       RegionGrowingSmoothness ();
44 |       
45 |       /** \brief Constructor */
46 |       ~RegionGrowingSmoothness ();
47 |       
48 |       /** \brief Set maximum angle between normals of neighbouring points (in radians [0, pi]). */
49 |       inline void
50 |       setNormalAngleThreshold (const float threshold);
51 | 
52 |       /** \brief Get maximum angle between normals of neighbouring points (in radians [0, pi]). */
53 |       inline float
54 |       getNormalAngleThreshold () const;
55 | 
56 |       /** \brief Set if input cloud has consistently oriented normals. */
57 |       inline void
58 |       setConsistentNormals (const bool normals_consistently_oriented_);
59 |       
60 |       /** \brief Check if input cloud is assumed to have consistently oriented normals. */
61 |       inline bool
62 |       getConsistentNormals () const;
63 |       
64 |     private:
65 | 
66 |       /** \brief Update the binary condition function to make sure it corresponds to the value of consistent normals flag. */
67 |       void updateBinaryConditionFunction ();
68 |       
69 |       /** \brief Reorder indices based in ascending order of curvature. */
70 |       void reorderIndicesCurvature ();
71 |       
72 |       /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
73 |        * the current settings. If it is possible then it returns true.
74 |        */
75 |       virtual bool
76 |       prepareForSegmentation ();
77 |       
78 |       /** \brief Normal variation binary condition function for consistently oriented normals. */
79 |       bool binaryConditionNonConsistent (const PointT& p1, const PointT& p2, float dist_squared) const;
80 |             
81 |       /** \brief Normal variation binary condition function for non-consistently oriented normals. */
82 |       bool binaryConditionConsistent    (const PointT& p1, const PointT& p2, float dist_squared) const;
83 | 
84 |   private:
85 |       
86 |       // Maximum angle between normals of neighbouring points
87 |       float normal_angle_threshold_;
88 |       
89 |       // Flag indicating whether input cloud has consistently oriented normals
90 |       bool normals_consistently_oriented_;
91 |       
92 |     public:
93 |       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 |   };
95 | }
96 | 
97 | #endif  // REGION_GROWING_SMOOTHNESS_H


--------------------------------------------------------------------------------
/include/namaris/algorithms/region_growing_normal_variation/region_growing_normal_variation.h:
--------------------------------------------------------------------------------
 1 | #ifndef REGION_GROWING_NORMAL_VARIATION_H
 2 | #define REGION_GROWING_NORMAL_VARIATION_H
 3 | 
 4 | // PCL includes
 5 | #include <pcl/common/angles.h>
 6 | 
 7 | // Algorithms includes
 8 | #include <namaris/algorithms/region_growing/region_growing.hpp>
 9 | 
10 | namespace alg
11 | {
12 |   /** \brief @b RegionGrowingNormalVariation Performs region growing
13 |    * segmentation on a pointcloud using a measure of normal angle change per
14 |    * distance between adjacent points. This is essintially a @b RegionGrowing
15 |    * class with a prespecified binary condition function.
16 |    * 
17 |    * Given a point in the cloud it's neighboring points are found. For all of 
18 |    * them a two conditions are verified:
19 |    *  1) unary condition determining if a neighboring point is a valid candidate
20 |    *  (can bes specified by the user)
21 |    *  2) binary condition between the original point and a neighboring point. 
22 |    *  Points belong to the same cluster if the angle between their normals 
23 |    *  normalized by the distance between them is smaller than a user specified
24 |    *  threshold.
25 |    * 
26 |    * If the fraction of neighbors satisfying the unary constraint is larger than
27 |    * a threshold and the fraction of neighbors satisfying the binary constraint 
28 |    * is larger than (a different) threshold neighboring points satisfying both 
29 |    * conditions are added to the cluster. The same procedure is run on the newly
30 |    * added points. Once this process converges it is run again on the remaining
31 |    * points in the cloud (i.e. points that were not yet assigned to any cluster)
32 |    * .
33 |    */
34 |   template<typename PointT>
35 |   class RegionGrowingNormalVariation : public alg::RegionGrowing<PointT>
36 |   {   
37 |     protected:
38 |       using alg::RegionGrowing<PointT>::setBinaryConditionFunction;
39 |       using alg::RegionGrowing<PointT>::input_;
40 |       using alg::RegionGrowing<PointT>::indices_;
41 |       
42 |     public:
43 |       /** \brief Constructor */
44 |       RegionGrowingNormalVariation ();
45 |       
46 |       /** \brief Constructor */
47 |       ~RegionGrowingNormalVariation ();
48 |       
49 |       /** \brief Set normal variation threshold (in radians per meter). */
50 |       inline void
51 |       setNormalVariationThreshold (const float threshold);
52 | 
53 |       /** \brief Set normal variation threshold (in radians per meter). */
54 |       inline float
55 |       getNormalVariationThreshold () const;
56 | 
57 |       /** \brief Set if input cloud has consistently oriented normals. */
58 |       inline void
59 |       setConsistentNormals (const bool normals_consistently_oriented_);
60 |       
61 |       /** \brief Check if input cloud is assumed to have consistently oriented normals. */
62 |       inline bool
63 |       getConsistentNormals () const;
64 |       
65 |     private:
66 | 
67 |       /** \brief Update the binary condition function to make sure it corresponds to the value of consistent normals flag. */
68 |       void updateBinaryConditionFunction ();
69 |       
70 |       /** \brief Reorder indices based in ascending order of curvature. */
71 |       void reorderIndicesCurvature ();
72 |       
73 |       /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
74 |        * the current settings. If it is possible then it returns true.
75 |        */
76 |       virtual bool
77 |       prepareForSegmentation ();
78 |       
79 |       /** \brief Normal variation binary condition function for consistently oriented normals. */
80 |       bool binaryConditionNonConsistent (const PointT& p1, const PointT& p2, float dist_squared) const;
81 |             
82 |       /** \brief Normal variation binary condition function for non-consistently oriented normals. */
83 |       bool binaryConditionConsistent    (const PointT& p1, const PointT& p2, float dist_squared) const;
84 | 
85 |   private:
86 |       
87 |       // Normal variation threhsold
88 |       float normal_variation_threshold_;
89 |       
90 |       // Flag indicating whether input cloud has consistently oriented normals
91 |       bool normals_consistently_oriented_;
92 |       
93 |     public:
94 |       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
95 |   };
96 | }
97 | 
98 | #endif  // REGION_GROWING_NORMAL_VARIATION_H


--------------------------------------------------------------------------------
/include/namaris/algorithms/meanshift/meanshift.h:
--------------------------------------------------------------------------------
  1 | #ifndef MEANSHIFT_H
  2 | #define MEANSHIFT_H
  3 | 
  4 | // Meanshift
  5 | #include <namaris/3rd_party/meanshift/MeanShift.h>
  6 | #include <namaris/3rd_party/meanshift/VectorPointSet.h>
  7 | #include <namaris/3rd_party/meanshift/EuclideanGeometry.h>
  8 | 
  9 | 
 10 | // #include "meanshiftlib/include/MeanShift.h"
 11 | // #include "meanshiftlib/include/VectorPointSet.h"
 12 | // #include "geometry/include/EuclideanGeometry.h"
 13 | 
 14 | // Eigen
 15 | #include <Eigen/Dense>
 16 | 
 17 | // Boost
 18 | #include <boost/shared_ptr.hpp>
 19 | #include <boost/make_shared.hpp>
 20 | 
 21 | namespace alg
 22 | {
 23 |   /** \brief Class for clustering points in N-dimensional Euclidean spaces using
 24 |    * meanshift approach.
 25 |    * 
 26 |    * Points are assumed to be samples of an unknown probability density
 27 |    * distribution. Clustering is done by recovering the modes of the
 28 |    * distribution. The input to the algorithm is a set of N-dimensional points.
 29 |    * The output is a set of clusters (or modes) with their centeroid information
 30 |    * and a list of the original points belonging to the cluster.
 31 |    * NOTE: this is essentially a wrapper around a modified version of this code:
 32 |    * http://coewww.rutgers.edu/riul/research/code/manifoldMS/index.html
 33 |    */
 34 |   class Meanshift
 35 |   {
 36 |   public:
 37 |     
 38 |     /** \brief Types of kernels used for probability density estimation. */
 39 |     enum KERNEL_TYPE {GAUSSIAN, EPANECHNIKOV};
 40 |     
 41 |     /** \brief Empty constructor. */
 42 |     Meanshift ();
 43 |             
 44 |     /** \brief Set the bandwidth of the kernel used for density estimation.
 45 |      * Larger values make each point influence a larger area around it.
 46 |      *  \param[in] bandwidth   bandwidth of the kernel
 47 |      */
 48 |     void setBandwidth (float bandwidth);
 49 |     
 50 |     /** \brief Type of Kernel used for density estimation
 51 |      *  \param[in] bandwidth   bandwidth of the kernel
 52 |      */          
 53 |     void setKernelType      (KERNEL_TYPE kernelType);
 54 |     
 55 |     /** \brief Set the maximum allowed distance between a mode center and it's
 56 |      * inlier.
 57 |      *  \param[in]  maxModeInlierDistance   maximum distance between mode's
 58 |      * center and it's inlier
 59 |      */              
 60 |     void setMaxModeInlierDistance (float maxModeInlierDistance);
 61 |     
 62 |     /** \brief Set the input data points of the algorithm.
 63 |      *  \param[in]  points    a matrix containing the data points (columns are
 64 |      * points, rows are dimensions)
 65 |      */              
 66 |     bool setInputPoints (const Eigen::MatrixXf &points);
 67 | 
 68 |     /** \brief Get the bandwidth of the kernel. */
 69 |     float getBandwidth () const;
 70 |     
 71 |     /** \brief Get the type of kernel used. */
 72 |     float getKernelType ()  const;
 73 |     
 74 |     /** \brief Get the maximum distance between mode center and its inlier. */
 75 |     float getMaxModeInlierDistance () const;
 76 |     
 77 |     /** \brief Run meanshift procedure on the input points.
 78 |      *  \param[out] unprunedModes   a matrix representing the converged points.
 79 |      */
 80 |     bool cluster (Eigen::MatrixXf &unprunedModes);
 81 |     
 82 |     /** \brief Find dominant modes by prunning local modes that are too close to other modes
 83 |      *  \param[out] unprunedModes   a matrix representing the converged points.
 84 |      */    
 85 |     bool pruneModes (Eigen::MatrixXf &prunedModes, std::vector<std::vector<int> > &prunedModeSupport);    
 86 | 
 87 |   private:
 88 |     
 89 |     // Parameters
 90 |     float bandwidth_;
 91 |     KERNEL_TYPE kernelType_;
 92 |     float maxModeInlierDistance_;
 93 |     
 94 |     // Data and results
 95 |     int dimensionality_;
 96 |     int numPoints_;
 97 |     // NOTE: the reason why these are shared pointers is that if the underlying
 98 |     // class CVectorPointSet is can not be initialized twice (in constructor and
 99 |     // then during processing). It segfaults when class is destrcuted. One
100 |     // option is to use regular pointers but then you have to keep track of
101 |     // which pointers were allocated and bla-bla-bla so just use shared pointers
102 |     // instead.
103 |     boost::shared_ptr<CVectorPointSet<float> > points_;
104 |     boost::shared_ptr<CVectorPointSet<float> > unprunedModes_;
105 |     boost::shared_ptr<CVectorPointSet<float> > prunedModes_;
106 |     std::vector<std::vector<int> > prunedModeSupport_;
107 |     std::vector<double> probs_;
108 |     
109 |     // Meanshift object
110 |     boost::shared_ptr<CEuclideanGeometry<float> > geom_;
111 |     CMeanShift<float> ms_;    
112 |   };
113 | }
114 | 
115 | #endif // MEANSHIFT_H
116 | 


--------------------------------------------------------------------------------
/include/namaris/utilities/eigen.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef EIGEN_UTILITIES_HPP
  2 | #define EIGEN_UTILITIES_HPP
  3 | 
  4 | #include <iostream>
  5 | #include <fstream>
  6 | #include <eigen3/Eigen/Dense>
  7 | 
  8 | namespace utl
  9 | {
 10 |   namespace eigen
 11 |   {
 12 |     /** \brief Write matrix to a file in binary mode.
 13 |      *  \param filename output file name
 14 |      *  \param matrix matrix
 15 |      *  \note http://stackoverflow.com/questions/25389480/how-to-write-read-an-eigen-matrix-from-binary-file
 16 |      */
 17 |     template<class Matrix>
 18 |     inline
 19 |     bool writeBinary(const std::string filename, const Matrix& matrix)
 20 |     {
 21 |       std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
 22 |       if (out.is_open())
 23 |       {
 24 |         typename Matrix::Index rows=matrix.rows(), cols=matrix.cols();
 25 |         out.write((char*) (&rows), sizeof(typename Matrix::Index));
 26 |         out.write((char*) (&cols), sizeof(typename Matrix::Index));
 27 |         out.write((char*) matrix.data(), rows*cols*sizeof(typename Matrix::Scalar) );
 28 |         out.close();
 29 |         return true;
 30 |       }
 31 |       else
 32 |       {
 33 |         std::cout << "[utl::eigen::writeBinary] Colud not open file '" << filename << "' for writing\n";
 34 |         return false;
 35 |       }
 36 |     }
 37 |     
 38 |     /** \brief Read a matrix from a binary file.
 39 |      *  \param filename input file name
 40 |      *  \param matrix matrix
 41 |      *  \note http://stackoverflow.com/questions/25389480/how-to-write-read-an-eigen-matrix-from-binary-file
 42 |      */  
 43 |     template<class Matrix>
 44 |     inline
 45 |     bool readBinary(const std::string filename, Matrix& matrix)
 46 |     {
 47 |       std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
 48 |       if (!in.is_open())
 49 |       {
 50 |         std::cout << "[utl::eigen::readBinary] Colud not open file '" << filename << "' for reading." << std::endl;
 51 |         return false;
 52 |       }
 53 |       
 54 |       typename Matrix::Index rows=0, cols=0;
 55 |       in.read((char*) (&rows),sizeof(typename Matrix::Index));
 56 |       in.read((char*) (&cols),sizeof(typename Matrix::Index));
 57 |       matrix.resize(rows, cols);
 58 |       in.read( (char *) matrix.data() , rows*cols*sizeof(typename Matrix::Scalar) );
 59 |       in.close();
 60 |       return true;
 61 |     }
 62 |     
 63 |     /** \brief Write matrix to a file in ASCII mode.
 64 |      *  \param filename output file name
 65 |      *  \param matrix matrix
 66 |      *  \return TRUE if file written successfully
 67 |      */
 68 |     template<class Matrix>
 69 |     inline
 70 |     bool writeASCII(const std::string filename, const Matrix& matrix)
 71 |     {
 72 |       std::ofstream out(filename.c_str(), std::ios::out);
 73 |       if (out.is_open())
 74 |       {
 75 |         out << matrix << "\n";
 76 |         out.close();
 77 |         return true;
 78 |       }
 79 |       else
 80 |       {
 81 |         std::cout << "[utl::eigen::writeASCII] Colud not open file '" << filename << "' for writing\n";
 82 |         return false;
 83 |       }
 84 |     }
 85 |     
 86 |     /** \brief Read a matrix from an ASCII file.
 87 |      *  \param filename input file name
 88 |      *  \param matrix matrix
 89 |      *  \return TRUE if file read successfully
 90 |      *  \note Adapted from http://perso.ensta-paristech.fr/~stulp/dmpbbo/EigenFileIO_8tpp_source.html
 91 |      */  
 92 |     template<class Matrix>
 93 |     inline
 94 |     bool readASCII(const std::string filename, Matrix& matrix)
 95 |     {
 96 |       std::ifstream in(filename.c_str(), std::ios::in);
 97 |       if (!in.is_open())
 98 |       {
 99 |         std::cout << "[utl::eigen::readASCII] Colud not open file '" << filename << "' for reading." << std::endl;
100 |         return false;
101 |       }
102 |       
103 |       // Read file contents into a vector
104 |       std::string line;
105 |       typename Matrix::Scalar d;
106 |       
107 |       std::vector<typename Matrix::Scalar> v;
108 |       int n_rows = 0;
109 |       while (getline(in, line))
110 |       {
111 |         ++n_rows;
112 |         std::stringstream input_line(line);
113 |         while (!input_line.eof())
114 |         {
115 |           input_line >> d;
116 |           v.push_back(d);
117 |         }
118 |       }
119 |       in.close();
120 |       
121 |       // Construct matrix
122 |       int n_cols = v.size()/n_rows;
123 |       matrix = Eigen::Matrix<typename Matrix::Scalar,Eigen::Dynamic,Eigen::Dynamic>(n_rows,n_cols);
124 |       
125 |       for (int i=0; i<n_rows; i++)
126 |         for (int j=0; j<n_cols; j++)
127 |           matrix(i,j) = v[i*n_cols + j];
128 |         
129 |       return true;
130 |     }    
131 |   }
132 | }
133 | 
134 | #endif  // EIGEN_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/pointcloud_occlusion_boundaries.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef POINCLOUD_OCCLUSION_BOUNDARIES_HPP
  2 | #define POINCLOUD_OCCLUSION_BOUNDARIES_HPP
  3 | 
  4 | // PCL includes
  5 | #include <pcl/point_types.h>
  6 | #include <pcl/point_cloud.h>
  7 | #include <pcl/search/kdtree.h>
  8 | 
  9 | // Utilities
 10 | #include <namaris/utilities/math.hpp>
 11 | #include <namaris/utilities/geometry.hpp>
 12 | #include <namaris/utilities/std_vector.hpp>
 13 | 
 14 | namespace alg
 15 | {
 16 |   template <typename PointT>
 17 |   void pointcloudOcclusionBoundaries  ( const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
 18 |                                         const std::vector<int>                  &indices,
 19 |                                         std::vector<int>                        &boundary_point_ids,
 20 |                                         const double                            radius,
 21 |                                         const float                             max_angle = pcl::deg2rad(135.0)
 22 |                                       )
 23 |   {
 24 |     boundary_point_ids.resize(0);
 25 |     
 26 |     // Prepare search tree
 27 |     pcl::search::KdTree<PointT> searchTree;
 28 |     searchTree.setInputCloud(cloud, boost::make_shared<std::vector<int> > (indices));
 29 | 
 30 |     // Loop over points
 31 |     for (size_t pointIdIt = 0; pointIdIt < indices.size(); pointIdIt++)
 32 |     {
 33 |       int pointId = indices[pointIdIt];
 34 |       PointT curPoint = cloud->points[pointId];
 35 |       
 36 |       // Find neighbours in a radius
 37 |       std::vector<float>  distances;
 38 |       std::vector<int>    neighbours;
 39 |       searchTree.radiusSearch(pointIdIt, radius, neighbours, distances);
 40 |       neighbours.erase(neighbours.begin());     // Remove first element since it is the point itself
 41 |       
 42 |       // If current point has no neighbours mark it as occluded
 43 |       if (neighbours.empty())
 44 |       {
 45 |         boundary_point_ids.push_back(pointId);
 46 |         continue;
 47 |       }
 48 |       
 49 |       // Project them on the tangent plane
 50 |       Eigen::Vector3f planePoint  = curPoint.getVector3fMap();
 51 |       Eigen::Vector3f planeNormal = curPoint.getNormalVector3fMap();
 52 |       std::vector<Eigen::Vector3f> projectedNeighbours(neighbours.size());
 53 |       
 54 |       for (size_t neighbourId = 0; neighbourId < neighbours.size(); neighbourId++)
 55 |       {
 56 |         Eigen::Vector3f neighbourPoint          = cloud->points[neighbours[neighbourId]].getVector3fMap();
 57 |         Eigen::Vector3f neighbourPointProjected = utl::geom::projectPointToPlane<float>(neighbourPoint, planePoint, planeNormal);
 58 |         projectedNeighbours[neighbourId]        = neighbourPointProjected;
 59 |       }
 60 |       
 61 |       // Calculate signed angles between first vector and all other vectors
 62 |       Eigen::Vector3f referenceVector = projectedNeighbours[0] - planePoint;
 63 |       std::vector<float> angles (neighbours.size());
 64 |       for (size_t neighbourId = 0; neighbourId < projectedNeighbours.size(); neighbourId++)
 65 |       {
 66 |         Eigen::Vector3f currentVector = projectedNeighbours[neighbourId] - planePoint;
 67 |         float curAngle = utl::geom::vectorAngleCW<float>(referenceVector, currentVector, planeNormal);
 68 |         angles[neighbourId] = curAngle;
 69 | 
 70 |       }
 71 | 
 72 |       // Calculate difference between consecutinve angles
 73 |       std::sort(angles.begin(), angles.end());
 74 |       std::vector<float> angleDifference(angles.size());
 75 |       for (size_t i = 1; i < angles.size(); i++)
 76 |       {
 77 |         angleDifference[i] = utl::geom::angleDifferenceCCW<float>(angles[i-1], angles[i]);
 78 |       }
 79 |       angleDifference[0] = utl::geom::angleDifferenceCCW<float>(angles[angles.size()-1], angles[0]);
 80 |       
 81 |       // If maximum difference is bigger than threshold mark point as boundary point
 82 |       if (utl::stdvec::vectorMax(angleDifference) > max_angle)
 83 |         boundary_point_ids.push_back(pointId);
 84 |     }  
 85 |   }
 86 |   
 87 |   template <typename PointT>
 88 |   void pointcloudOcclusionBoundaries  ( const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
 89 |                                         std::vector<int>                        &boundary_point_ids,
 90 |                                         const double                            radius,
 91 |                                         const float                             max_angle = pcl::deg2rad(135.0)
 92 |                                       )
 93 |   {
 94 |     std::vector<int> fake_indices (cloud->size());
 95 |     for (size_t pointId = 0; pointId < cloud->size(); pointId++)
 96 |       fake_indices[pointId] = pointId;
 97 |     
 98 |     pointcloudOcclusionBoundaries<PointT>(cloud, fake_indices, boundary_point_ids, radius, max_angle);
 99 |   }
100 | }
101 | 
102 | #endif  // POINCLOUD_OCCLUSION_BOUNDARIES_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/dijkstra_shortest_path.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef DIJKSTRA_SHORTEST_PATH_HPP
  2 | #define DIJKSTRA_SHORTEST_PATH_HPP
  3 | 
  4 | // Boost
  5 | #include <boost/graph/graph_traits.hpp>
  6 | #include <boost/graph/adjacency_list.hpp>
  7 | #include <boost/graph/dijkstra_shortest_paths.hpp>
  8 |   
  9 | namespace alg
 10 | {  
 11 |   typedef boost::adjacency_list < boost::vecS, 
 12 |                                   boost::vecS,
 13 |                                   boost::undirectedS,
 14 |                                   boost::no_property,
 15 |                                   boost::property < boost::edge_weight_t, float > > Graph;
 16 |                                   
 17 |   typedef boost::graph_traits < Graph >::edge_descriptor Edge;
 18 |   typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;
 19 | 
 20 |   /** \brief Compute the shortest path between two nodes in a graph using
 21 |    * Dijkstra's algorithm.
 22 |     * \param[in]  edges         edges of the graph
 23 |     * \param[in]  edge_weights  weights of the edges
 24 |     * \param[in]  start_node_id index of the start node
 25 |     * \param[in]  end_node_id   index of the ned node
 26 |     * \param[out] path          shortest path from start node to end node represented as a sequence of visited vertices
 27 |     * \note edge weights must be non-negative
 28 |     * \note uses boost implemntation of dijkstra's algorithm: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/dijkstra_shortest_paths.html
 29 |     * \note vertex indices do not have to be continuous
 30 |     * \note if not path between start and end node exists the return path is empty and path cost is equal to -1
 31 |     */
 32 |   float getShortestPath ( const std::vector<std::pair<int,int> >  &edges, 
 33 |                           const std::vector<float>                &edge_weights,
 34 |                           const int                               start_node_id,
 35 |                           const int                               end_node_id,
 36 |                           std::vector<int>                        &path
 37 |                         )
 38 |   {
 39 |     //--------------------------------------------------------------------------
 40 |     // Check input
 41 |     //--------------------------------------------------------------------------
 42 |     
 43 |     // Check that edges and edge weights have same size
 44 |     if (edges.size() != edge_weights.size())
 45 |     {
 46 |       std::cout << "[alg::getShortestPath] number of edges and edge weights are different." << std::endl;
 47 |       abort();
 48 |     }
 49 |     
 50 |     // Check that start and end vertices are present in the graph
 51 |     std::vector<int> vertexIds (edges.size() * 2);
 52 |     for (size_t edgeId = 0; edgeId < edges.size(); edgeId++)
 53 |     {
 54 |       vertexIds[edgeId*2]   = edges[edgeId].first;
 55 |       vertexIds[edgeId*2+1] = edges[edgeId].second;
 56 |     }
 57 |     
 58 |     if (  std::find(vertexIds.begin(), vertexIds.end(), start_node_id) == vertexIds.end())
 59 |     {
 60 |       std::cout << "[alg::getShortestPath] graph does not contain start vertex." << std::endl;
 61 |       return -1.0f;
 62 |     }
 63 | 
 64 |     if (  std::find(vertexIds.begin(), vertexIds.end(), end_node_id) == vertexIds.end())
 65 |     {
 66 |       std::cout << "[alg::getShortestPath] graph does not contain end vertex." << std::endl;
 67 |       return -1.0f;
 68 |     }    
 69 |     
 70 |     //--------------------------------------------------------------------------
 71 |     // Convert to Boost grapg
 72 |     //--------------------------------------------------------------------------
 73 |     
 74 |     int numVertices = *std::max_element(vertexIds.begin(), vertexIds.end());
 75 |     int numEdges    = edges.size();
 76 |         
 77 |     Graph g (&edges[0], &edges[0]+numEdges, &edge_weights[0], numVertices);
 78 |     
 79 |     //--------------------------------------------------------------------------
 80 |     // Run Djikstra'sboost 
 81 |     //--------------------------------------------------------------------------
 82 | 
 83 |     Vertex startVtx = boost::vertex(start_node_id, g);
 84 |     Vertex endVtx   = boost::vertex(end_node_id, g);
 85 |     std::vector<Vertex> parents(boost::num_vertices(g));
 86 |     std::vector<float> distances(boost::num_vertices(g));
 87 |  
 88 |     // Compute shortest paths from v0 to all vertices, and store the output in parents and distances
 89 |     boost::dijkstra_shortest_paths(g, startVtx, boost::predecessor_map(&parents[0]).distance_map(&distances[0]));
 90 |     
 91 |     //--------------------------------------------------------------------------
 92 |     // Get output
 93 |     //--------------------------------------------------------------------------
 94 |     
 95 |     path.resize(0);
 96 |     
 97 |     if ((distances[end_node_id] == std::numeric_limits<float>::max()))
 98 |     {
 99 |       return -1.0f;
100 |     }
101 |     else
102 |     {
103 |       Vertex vtxIterator = endVtx;
104 |       while ( vtxIterator !=  startVtx  )
105 |       {
106 |         path.push_back(vtxIterator);
107 |         vtxIterator = parents[vtxIterator];
108 |       }
109 |       path.push_back(startVtx);
110 |       std::reverse(path.begin(), path.end());
111 |       
112 |       return distances[end_node_id];
113 |     }
114 |   }
115 | }
116 | 
117 | #endif  // DIJKSTRA_SHORTEST_PATH_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/lccp_segmentation.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef LCCP_SEGMETTION_HPP
  2 | #define LCCP_SEGMETTION_HPP
  3 | 
  4 | // PCL
  5 | #include <pcl/segmentation/lccp_segmentation.h>
  6 | 
  7 | // CPP tools
  8 | #include <namaris/utilities/std_vector.hpp>
  9 | #include <namaris/utilities/pcl_typedefs.hpp>
 10 | #include <namaris/utilities/graph.hpp>
 11 | #include <namaris/utilities/map.hpp>
 12 | 
 13 | namespace alg
 14 | {
 15 |   /** \brief Apply LCCP segmentation algorithm to a set of connected oriented supervoxels.
 16 |    *  \param[in]  centroids             pointcloud represening the supervoxel cetroid locations and orientations
 17 |    *  \param[in]  adjacency             adjacency between the supervoxels
 18 |    *  \param[in]  concavity_threshold   concavity threshold of the LCCP algorithm 
 19 |    *  \param[out] segments_to_patches   map from segment indices to supervoxels indices
 20 |    *  \param[in]  min_segment_size      segments containing fewer than this many patches are merged with the largest adjacent segment
 21 |    */
 22 |   void lccpSegmentation ( const pcl::PointCloud<PointN> &centroids,
 23 |                           const utl::graph::Graph       &adjacency,
 24 |                           const float                   concavity_threshold,
 25 |                           utl::map::Map                 &segments_to_svoxels,
 26 |                           const int                     &min_segment_size = 0
 27 |                         )
 28 |   { 
 29 |     //--------------------------------------------------------------------------
 30 |     // Convert segmentation information
 31 |     //--------------------------------------------------------------------------
 32 |     
 33 |     // Adjacency
 34 |     std::multimap<uint32_t, uint32_t>   label_adjacency;
 35 |     std::vector<std::pair<int,int> > adjacency_edges = utl::graph::graph2EdgePairs(adjacency);
 36 |     
 37 |     for (size_t edgeId = 0; edgeId < adjacency_edges.size(); edgeId++)
 38 |     {
 39 |       label_adjacency.insert(std::pair<uint32_t,uint32_t>(static_cast<uint32_t>(adjacency_edges[edgeId].first),   static_cast<uint32_t>(adjacency_edges[edgeId].second)));
 40 |       label_adjacency.insert(std::pair<uint32_t,uint32_t>(static_cast<uint32_t>(adjacency_edges[edgeId].second),  static_cast<uint32_t>(adjacency_edges[edgeId].first)));
 41 |     }
 42 | 
 43 |     // Centroids
 44 |     std::map <uint32_t, pcl::Supervoxel<Point>::Ptr >  supervoxel_clusters;
 45 |     for (size_t clusterId = 0; clusterId < centroids.size(); clusterId++)
 46 |     {
 47 |       pcl::Supervoxel<Point>::Ptr curSupervoxel (new pcl::Supervoxel<Point>);
 48 |       curSupervoxel->normal_.getNormalVector3fMap() = centroids.points[clusterId].getNormalVector3fMap();
 49 |       curSupervoxel->centroid_.getVector3fMap()     = centroids.points[clusterId].getVector3fMap();
 50 |       supervoxel_clusters.insert(std::pair<uint32_t, pcl::Supervoxel<Point>::Ptr>(static_cast<uint32_t>(clusterId), curSupervoxel));
 51 |     }
 52 |       
 53 |     //--------------------------------------------------------------------------
 54 |     // Segment
 55 |     //--------------------------------------------------------------------------
 56 |     
 57 |     std::map<uint32_t, std::set<uint32_t> > segmentToSupervoxelMap;
 58 |     
 59 |     pcl::LCCPSegmentation<Point> lccp;
 60 |     lccp.setConcavityToleranceThreshold(concavity_threshold);
 61 |     lccp.setInputSupervoxels(supervoxel_clusters, label_adjacency);
 62 |     lccp.setMinSegmentSize(min_segment_size);
 63 |     lccp.segment();
 64 |     lccp.getSegmentToSupervoxelMap(segmentToSupervoxelMap);
 65 |     
 66 | //     // PCL 1.7.2
 67 | //     std::map<uint32_t, std::vector<uint32_t> > segmentSupervoxelMap;
 68 | //     lccp.segment(supervoxel_clusters, label_adjacency);        
 69 | //     lccp.removeSmallSegments(min_segment_size);
 70 | //     lccp.getSegmentSupervoxelMap(segmentSupervoxelMap);
 71 |     
 72 |     //--------------------------------------------------------------------------
 73 |     // Convert labelled cloud to segmentation map
 74 |     //--------------------------------------------------------------------------
 75 | 
 76 |     segments_to_svoxels.resize(0);
 77 |     for (auto segIt = segmentToSupervoxelMap.begin(); segIt != segmentToSupervoxelMap.end(); segIt++)
 78 |       if (segIt->second.size() > 1)
 79 |         segments_to_svoxels.push_back(std::vector<int>(segIt->second.begin(), segIt->second.end()));      
 80 |   }
 81 |   
 82 |   /** \brief Remap LCCP segmentation to full cloud segmentation
 83 |    *  \param[in]  svoxels                 map from supervoxels to points
 84 |    *  \param[in]  segment_to_svoxel_map   map from segments to supervoxels
 85 |    *  \param[out]  lccp_segmentation      final segmentation (map from segments to points)
 86 |    */  
 87 |   void remapSvoxelsToLCCPSegments ( const utl::map::Map &svoxels,
 88 |                                     const utl::map::Map &segment_to_svoxel_map,
 89 |                                     utl::map::Map       &lccp_segmentation
 90 |                                   )
 91 |   {
 92 |     lccp_segmentation.resize(segment_to_svoxel_map.size());
 93 |     
 94 |     for (size_t segId = 0; segId < segment_to_svoxel_map.size(); segId++)
 95 |     {
 96 |       for (size_t svoxelIdIt = 0; svoxelIdIt < segment_to_svoxel_map[segId].size(); svoxelIdIt++)
 97 |       {
 98 |         int svoxelId = segment_to_svoxel_map[segId][svoxelIdIt];
 99 |         utl::stdvec::vectorAppend<int>(lccp_segmentation[segId], svoxels[svoxelId]);
100 |       }
101 |     }
102 |   }  
103 | }
104 | 
105 | #endif // LCCP_SEGMETTION_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/opencv.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef OPENCV_UTILITIES_HPP
  2 | #define OPENCV_UTILITIES_HPP
  3 | 
  4 | // STD
  5 | #include <iostream>
  6 | 
  7 | // OpenCV
  8 | #include <opencv2/imgproc/imgproc.hpp>
  9 | #include <opencv2/highgui/highgui.hpp>
 10 | 
 11 | namespace utl
 12 | {
 13 |   namespace ocv
 14 |   {    
 15 |     /** \brief Create a lookup table for the blue to red colormap.
 16 |      *  \return a lookup table for the colormap
 17 |      */
 18 |     inline
 19 |     cv::Mat b2rColormapLUT ()
 20 |     {
 21 |       cv::Mat lut (256, 1, CV_8UC3);
 22 |       
 23 |       cv::Scalar blue   (255, 0, 0);
 24 |       cv::Scalar red    (0, 0, 255);
 25 |       cv::Scalar white  (255, 255, 255);
 26 |       
 27 |       for (size_t i = 0; i < 128; i++)
 28 |       {
 29 |         double weight = static_cast<float>(i) / 128;
 30 |         cv::Scalar curColor = white * weight + blue * (1 - weight);
 31 |         
 32 |         lut.at<cv::Vec3b>(i)[0] = static_cast<uchar>(curColor(0));
 33 |         lut.at<cv::Vec3b>(i)[1] = static_cast<uchar>(curColor(1));
 34 |         lut.at<cv::Vec3b>(i)[2] = static_cast<uchar>(curColor(2));
 35 |       }
 36 |       
 37 |       for (size_t i = 128; i < 256; i++)
 38 |       {
 39 |         double weight = static_cast<float>(i-128) / 128;
 40 |         cv::Scalar curColor = red * weight + white * (1 - weight);
 41 | 
 42 |         lut.at<cv::Vec3b>(i)[0] = static_cast<uchar>(curColor(0));
 43 |         lut.at<cv::Vec3b>(i)[1] = static_cast<uchar>(curColor(1));
 44 |         lut.at<cv::Vec3b>(i)[2] = static_cast<uchar>(curColor(2));
 45 |       }
 46 |       
 47 |       return lut;
 48 |     }
 49 | 
 50 |     /** \brief Apply colormap to a matrix. For now only color colormaps are supported.
 51 |      *  \param[in] src  source matrix. Must be either CV_8UC1 or CV_8UC3. If it is CV_8UC3 colormap is applied to grayscale image obtained from color image
 52 |      *  \param[in] dst  colormaped matrix CV_8UC1
 53 |      *  \param[in] lut  colormap represented as a 256x1 or 1x256 CV_8UC1 matrix
 54 |      */
 55 |     inline
 56 |     void applyColormap  (const cv::Mat &src, cv::Mat &dst, const cv::Mat &lut)
 57 |     {
 58 |       // Check input
 59 |       if (lut.type() != CV_8UC3)
 60 |       {
 61 |         std::cout << "[utl::ocv::applyColormap] colormap must by of type CV_8UC3." << std::endl;
 62 |         abort();
 63 |       }
 64 |       
 65 |       if (lut.size() != cv::Size(256, 1) && lut.size() != cv::Size(1, 256))
 66 |       {
 67 |         std::cout << "[utl::ocv::applyColormap] colormap must have size 1x256 or 256x1." << std::endl;
 68 |         abort();
 69 |       }
 70 |       
 71 |       if (src.type() != CV_8UC1 && src.type() != CV_8UC3)
 72 |       {
 73 |         std::cout << "[utl::ocv::applyColormap] source matrix must be of type CV_8U or CV_8UC3." << std::endl;
 74 |         abort();        
 75 |       }
 76 |       
 77 |       
 78 |       cv::Mat srcCopy = src.clone();
 79 |       
 80 |       if (src.type() == CV_8UC3)
 81 |       {
 82 |         cv::cvtColor(srcCopy.clone(), srcCopy, CV_BGR2GRAY);
 83 |       }
 84 |       
 85 |       // Apply colormap
 86 |       cv::cvtColor(srcCopy.clone(), srcCopy, CV_GRAY2BGR);      
 87 |       cv::LUT(srcCopy, lut, dst);
 88 |     }    
 89 |     
 90 |     /** \brief Generate an image with a visualization of a colormap
 91 |      *  \param[in] colormap a lookup table representing the colormap. Must have size 256x1 or 1x256 and be of type CV_8UC3
 92 |      *  \param[in] width    width of a bar representing each color
 93 |      *  \param[in] height   height of a bar representing each color
 94 |      */
 95 |     inline
 96 |     cv::Mat visualizeColormap  (const cv::Mat &colormap, const int width = 5, const int height = 100)
 97 |     {
 98 |       cv::Mat mat (height, width * 256, CV_8U);
 99 |   
100 |       for (size_t i = 0; i < 256; i++)
101 |       {
102 |         cv::Mat roi(mat(cv::Rect(i *width, 0, width, height)));
103 |         roi.setTo(i);
104 |       }
105 |   
106 |       cv::Mat matColormapped;
107 |       utl::ocv::applyColormap(mat, matColormapped, colormap);
108 |       
109 |       return matColormapped;
110 |     }
111 |     
112 |     /** \brief Visualize matrix by converting it to CV_8U and rescaling values
113 |      * such that minimum value corresponds to 0 and maximum value corresponds to
114 |      * 255.
115 |      *  \param[in] mat  input matrix
116 |      *  \return matrix that can be visualized using cv::imshow()
117 |      */    
118 |     inline
119 |     cv::Mat visualizeMatrix (const cv::Mat &im)
120 |     {
121 |       cv::Mat im_normalized;
122 |       im.convertTo(im_normalized, CV_8U);
123 |       cv::normalize(im_normalized.clone(), im_normalized, 0, 255, cv::NORM_MINMAX);
124 |       return im_normalized;
125 |     }
126 | 
127 |     /** \brief Rescale an image to a desired width. Both X and Y dimensions are
128 |      * scaled by the same factor (up to discretization error).
129 |      *  \param[in] image  image
130 |      *  \param[in] new_width desired width of the image
131 |      *  \return rescaled image
132 |      */
133 |     inline
134 |     cv::Mat resize ( cv::Mat &image, const int new_width)
135 |     {
136 |       cv::Mat resized_image;
137 |       if (image.cols > new_width)
138 |       {
139 |         float scaleFactor = static_cast<float>(new_width) / static_cast<float>(image.cols);
140 |         int targetHeight = static_cast<int>(static_cast<float>(image.rows) * scaleFactor);
141 |         cv::resize(image, resized_image, cv::Size(new_width, targetHeight));
142 |       }
143 |       else
144 |       {
145 |         resized_image = image.clone();
146 |       }
147 |       
148 |       return resized_image;
149 |     }
150 |   }
151 | }
152 | 
153 | 
154 | #endif  // OPENCV_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/region_growing_smoothness/region_growing_smoothness.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef REGION_GROWING_SMOOTHNESS_HPP
  2 | #define REGION_GROWING_SMOOTHNESS_HPP
  3 | 
  4 | #include "region_growing_smoothness.h"
  5 | 
  6 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  7 | template <typename PointT>
  8 | alg::RegionGrowingSmoothness<PointT>::RegionGrowingSmoothness () :
  9 |   normal_angle_threshold_ (0.0f),
 10 |   normals_consistently_oriented_ (false)
 11 | {
 12 |   updateBinaryConditionFunction();  
 13 | }
 14 |       
 15 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 16 | template <typename PointT>
 17 | alg::RegionGrowingSmoothness<PointT>::~RegionGrowingSmoothness ()
 18 | {
 19 | }
 20 | 
 21 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 22 | template <typename PointT> inline void
 23 | alg::RegionGrowingSmoothness<PointT>::setNormalAngleThreshold (const float threshold)
 24 | {
 25 |   normal_angle_threshold_ = threshold;
 26 | }
 27 | 
 28 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 29 | template <typename PointT> inline float
 30 | alg::RegionGrowingSmoothness<PointT>::getNormalAngleThreshold () const
 31 | {
 32 |   return normal_angle_threshold_;
 33 | }
 34 | 
 35 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 36 | template <typename PointT> inline void
 37 | alg::RegionGrowingSmoothness<PointT>::setConsistentNormals (const bool normals_consistently_oriented)
 38 | {
 39 |   normals_consistently_oriented_ = normals_consistently_oriented;
 40 |   updateBinaryConditionFunction();
 41 | }
 42 | 
 43 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 44 | template <typename PointT> inline bool
 45 | alg::RegionGrowingSmoothness<PointT>::getConsistentNormals () const
 46 | {
 47 |   return normals_consistently_oriented_;
 48 | }
 49 | 
 50 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 51 | template <typename PointT> inline void
 52 | alg::RegionGrowingSmoothness<PointT>::updateBinaryConditionFunction ()
 53 | {
 54 |   if (normals_consistently_oriented_)
 55 |     setBinaryConditionFunction( boost::bind(&RegionGrowingSmoothness<PointT>::binaryConditionConsistent, this, _1, _2, _3) );
 56 |   else
 57 |     setBinaryConditionFunction( boost::bind(&RegionGrowingSmoothness<PointT>::binaryConditionNonConsistent, this, _1, _2, _3) );
 58 | }
 59 | 
 60 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 61 | template <typename PointT> inline void
 62 | alg::RegionGrowingSmoothness<PointT>::reorderIndicesCurvature ()
 63 | {
 64 |   if (!indices_ || indices_->empty())
 65 |   {
 66 |     indices_->resize(input_->size());
 67 |     for (size_t pointId = 0; pointId < indices_->size(); pointId++)
 68 |       (*indices_)[pointId] = pointId;
 69 |   }
 70 |   
 71 |   // Extract curvature values into a vector
 72 |   std::vector<float> curvature (indices_->size());
 73 |   for (size_t pointIdIt = 0; pointIdIt < indices_->size(); pointIdIt++)
 74 |     curvature[pointIdIt] = input_->points[(*indices_)[pointIdIt]].curvature;
 75 |     
 76 |   // Reorder indices in increasing order of curvature
 77 |   std::vector<float> curvature_sorted;
 78 |   std::vector<size_t> order;
 79 |   utl::stdvec::sort(curvature, curvature_sorted, order, utl::stdvec::ASCENDING);
 80 |   *indices_ = utl::stdvec::reorder(*indices_, order);  
 81 | }
 82 | 
 83 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 84 | template <typename PointT> bool
 85 | alg::RegionGrowingSmoothness<PointT>::binaryConditionConsistent (const PointT& p1, const PointT& p2, float dist_squared) const
 86 | {
 87 |   Eigen::Vector3f n1 = p1.getNormalVector3fMap();
 88 |   Eigen::Vector3f n2 = p2.getNormalVector3fMap();
 89 |     
 90 |   float angle = std::acos (utl::math::clampValue (n1.dot (n2), 0.0f, 1.0f));
 91 |   
 92 |   return angle < normal_angle_threshold_;
 93 | }
 94 | 
 95 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 96 | template <typename PointT> bool
 97 | alg::RegionGrowingSmoothness<PointT>::binaryConditionNonConsistent (const PointT& p1, const PointT& p2, float dist_squared) const
 98 | {
 99 |   Eigen::Vector3f n1 = p1.getNormalVector3fMap();
100 |   Eigen::Vector3f n2 = p2.getNormalVector3fMap();
101 |     
102 |   float angle = std::acos (utl::math::clampValue (std::abs (n1.dot (n2)), 0.0f, 1.0f));
103 |   return angle < normal_angle_threshold_;  
104 | }
105 | 
106 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
107 | template <typename PointT> inline bool
108 | alg::RegionGrowingSmoothness<PointT>::prepareForSegmentation ()
109 | {  
110 |   // If user forgot to pass point cloud or if it is empty
111 |   if ( normal_angle_threshold_ < 0.0f )
112 |   {
113 |     std::cout << "[alg::RegionGrowingSmoothness::prepareForSegmentation] normal variation threhsold must be non-negative!" << std::endl;
114 |     return (false);
115 |   }
116 |   
117 |   bool good = alg::RegionGrowing<PointT>::prepareForSegmentation();
118 |   
119 |   if (good)
120 |   {
121 |     this->reorderIndicesCurvature();
122 |     return true;
123 |   }
124 |   else
125 |   {
126 |     return false;
127 |   }
128 | }
129 | 
130 | #endif  // REGION_GROWING_SMOOTHNESS_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/felzenswalb_graph_segmentation/felzenswalb_graph_segmentation.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef FELZENSWALB_GRAPH_SEGMENTATION_HPP
  2 | #define FELZENSWALB_GRAPH_SEGMENTATION_HPP
  3 | 
  4 | // Utilities
  5 | #include <namaris/utilities/std_vector.hpp>
  6 | 
  7 | // Algorithms
  8 | #include <namaris/algorithms/felzenswalb_graph_segmentation/felzenswalb_graph_segmentation.h>
  9 | 
 10 | ////////////////////////////////////////////////////////////////////////////////
 11 | alg::FelzenswalbGraphSegmentation::FelzenswalbGraphSegmentation()
 12 |   : num_edges_ (0)
 13 |   , num_vertices_ (0)
 14 |   , min_seg_size_ (1)
 15 |   , max_seg_size_ (std::numeric_limits<size_t>::max())
 16 |   , threshold_ (-1.0)
 17 |   , edges_ (NULL)
 18 | { }
 19 | 
 20 | ////////////////////////////////////////////////////////////////////////////////
 21 | alg::FelzenswalbGraphSegmentation::~FelzenswalbGraphSegmentation()
 22 | {
 23 |   delete [] edges_;
 24 | }
 25 | 
 26 | ////////////////////////////////////////////////////////////////////////////////
 27 | bool alg::FelzenswalbGraphSegmentation::setThreshold ( const float threshold )
 28 | {
 29 |   // Check input
 30 |   if (threshold < 0)
 31 |   {
 32 |     std::cout << "[alg::FelzenswalbGraphSegmentation::setThreshold] threshold must be non-negative." << std::endl;
 33 |     return false;
 34 |   }
 35 |       
 36 |   threshold_ = threshold;
 37 |   return true;
 38 | }
 39 | 
 40 | ////////////////////////////////////////////////////////////////////////////////
 41 | bool alg::FelzenswalbGraphSegmentation::setMinSegmentSize(const size_t min_seg_size)
 42 | {
 43 |   // Check input
 44 |   if (min_seg_size < 1)
 45 |   {
 46 |     std::cout << "[alg::FelzenswalbGraphSegmentation::setMinSegmentSize] segments must contain at least one point." << std::endl;
 47 |     return false;
 48 |   }    
 49 |   
 50 |   min_seg_size_ = min_seg_size;
 51 |   return true;
 52 | }
 53 | 
 54 | ////////////////////////////////////////////////////////////////////////////////
 55 | bool alg::FelzenswalbGraphSegmentation::setMaxSegmentSize(const size_t max_seg_size)
 56 | {
 57 |   // Check input
 58 |   if (max_seg_size < 1)
 59 |   {
 60 |     std::cout << "[alg::FelzenswalbGraphSegmentation::setMaxSegmentSize] segments must contain at least one point." << std::endl;
 61 |     return false;
 62 |   }    
 63 |   
 64 |   max_seg_size_ = max_seg_size;
 65 |   return true;
 66 | }
 67 | 
 68 | ////////////////////////////////////////////////////////////////////////////////
 69 | bool alg::FelzenswalbGraphSegmentation::setGraph ( const std::vector< std::pair< int, int > >& edges,
 70 |                                               const std::vector< float >& weights
 71 |                                             )
 72 | {
 73 |   if (edges.size() == 0)
 74 |   {
 75 |     std::cout << "[alg::FelzenswalbGraphSegmentation::setGraph] input edges are empty!" << std::endl;
 76 |     return false;
 77 |   }
 78 |   
 79 |   // Check input
 80 |   if (edges.size() != weights.size())
 81 |   {
 82 |     std::cout << "[alg::FelzenswalbGraphSegmentation::setEdges] number of edges and number of weights must be the same." << std::endl;
 83 |     return false;
 84 |   }
 85 |   
 86 |   // Fill graph structure
 87 |   num_edges_ = edges.size();
 88 |   edges_ = new edge[num_edges_];
 89 |   for (size_t edgeId = 0; edgeId < num_edges_; edgeId++)
 90 |   {
 91 |     edges_[edgeId].a = edges[edgeId].first;
 92 |     edges_[edgeId].b = edges[edgeId].second;
 93 |     edges_[edgeId].w = weights[edgeId];
 94 |   }
 95 |   
 96 |   // Find number of vertices
 97 |   vertices_.clear();
 98 |   for (size_t edgeId = 0; edgeId < num_edges_; edgeId++)
 99 |   {
100 |     vertices_.push_back(edges[edgeId].first);
101 |     vertices_.push_back(edges[edgeId].second);
102 |   }
103 |   
104 |   utl::stdvec::uniqueVector(vertices_);
105 |   num_vertices_ = utl::stdvec::vectorMax(vertices_)+1;
106 |   
107 |   return true;
108 | }
109 | 
110 | ////////////////////////////////////////////////////////////////////////////////
111 | bool alg::FelzenswalbGraphSegmentation::segment(std::vector< std::vector< int > >& segments)
112 | {
113 |   // Check segmentation conditions
114 |   if (edges_ == NULL)
115 |   {
116 |     std::cout << "[alg::FelzenswalbGraphSegmentation::segment] edges are not set." << std::endl;
117 |     return false;
118 |   }
119 |   
120 |   if (threshold_ == -1.0)
121 |   {
122 |     std::cout << "[alg::FelzenswalbGraphSegmentation::segment] threhsold is not set." << std::endl;
123 |     return false;
124 |   }
125 |   
126 |   // Segment
127 |   universe *segmentation = segment_graph(num_vertices_, num_edges_, edges_, threshold_);
128 |   
129 |   // Extract results
130 |   segments.clear();
131 |   
132 |   std::vector<int> segIdMap;
133 |   for (size_t vtxIdIt = 0; vtxIdIt < vertices_.size(); vtxIdIt++)
134 |   {
135 |     int vtxId = vertices_[vtxIdIt];
136 |     
137 |     // Get segment labels and check that it it's size is appropriate
138 |     int segIdOriginal = segmentation->find(vtxId);
139 |     if (segmentation->size(segIdOriginal) < min_seg_size_ || segmentation->size(segIdOriginal) > max_seg_size_)
140 |       continue;
141 |     
142 |     // Remap original segment label to continuous integer range
143 |     int segIdMapped;
144 |     std::vector<int>::iterator segIdIt = std::find(segIdMap.begin(), segIdMap.end(), segIdOriginal);
145 |     if (segIdIt == segIdMap.end())
146 |     {
147 |       segIdMapped = segIdMap.size();
148 |       segIdMap.push_back(segIdOriginal);
149 |       segments.push_back(std::vector<int> ());
150 |     }
151 |     else
152 |     {
153 |       segIdMapped = std::distance(segIdMap.begin(), segIdIt);
154 |     }
155 |     
156 |     segments[segIdMapped].push_back(vtxId);
157 |   }
158 |   
159 |   delete segmentation;
160 |   return true;
161 | }
162 | 
163 | #endif  // FELZENSWALB_GRAPH_SEGMENTATION_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/pointcloud_alignment.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef POINTCLOUD_ALIGNMENT_HPP
  2 | #define POINTCLOUD_ALIGNMENT_HPP
  3 | 
  4 | // PCL includes
  5 | #include <pcl/registration/icp.h>
  6 | #include <pcl/registration/transformation_estimation_point_to_plane.h>
  7 | #include <pcl/registration/correspondence_rejection_distance.h>
  8 | #include <pcl/registration/correspondence_rejection_one_to_one.h>
  9 | #include <pcl/registration/correspondence_rejection_surface_normal.h>
 10 | 
 11 | namespace alg
 12 | {
 13 |   /** \brief Align two clouds the ICP algorithm.
 14 |    *  Disstance metric:         point-to-plane
 15 |    *  Optimization method:      Levenberg-Marquardt
 16 |    *  Available correspondence rejectors:
 17 |    *    - max correspondence distance
 18 |    *    - max correspondence surface normal angle
 19 |    *    - one-to-one
 20 |    *  \param[in]  cloud_src           cloud that needs to be aligned
 21 |    *  \param[in]  cloud_tgt           cload that source will be aligned to
 22 |    *  \param[out] cloud_src_aligned   aligned source cloud
 23 |    *  \param[out] transform           transformation that takes source cloud points to aligned cloud points
 24 |    *  \param[in]  max_correspondence_distance             max correspondance distance threshold
 25 |    *  \param[in]  max_correspondece_surface_normal_angle  max correspondence normal angle threshold
 26 |    *  \param[in]  use_one_to_one      flag specifying if one-to-one rejection is used
 27 |    *  \param[in]  max_iterations      maximum number of ICP internal iterations
 28 |    *  \return distance between two clouds
 29 |    */
 30 |   template <typename PointT>
 31 |   inline
 32 |   bool alignClouds  ( const typename pcl::PointCloud<PointT>::ConstPtr &cloud_src,
 33 |                       const typename pcl::PointCloud<PointT>::ConstPtr &cloud_tgt,
 34 |                       typename pcl::PointCloud<PointT>::Ptr &cloud_src_aligned,
 35 |                       Eigen::Affine3f &transform,
 36 |                       pcl::CorrespondencesPtr &correspondences,
 37 |                       const float max_correspondence_distance = 0.0f,
 38 |                       const float max_correspondece_surface_normal_angle = 0.0f,
 39 |                       const bool use_one_to_one = true,
 40 |                       const int max_iterations = 100
 41 |                     )
 42 |   {
 43 |     //--------------------------------------------------------------------------
 44 |     // Typedefs
 45 |     //--------------------------------------------------------------------------
 46 |     
 47 |     typedef pcl::registration::CorrespondenceEstimation<PointT, PointT, float>                ClosestPoint;
 48 |     typedef pcl::registration::TransformationEstimationPointToPlane<PointT, PointT, float>    PointToPlaneLM;    // Point to plane using Levenberg-Marquardt
 49 |     typedef pcl::registration::CorrespondenceRejectorDistance                                 DistanceRejector;
 50 |     typedef pcl::registration::CorrespondenceRejectorOneToOne                                 OneToOneRejector;
 51 |     typedef pcl::registration::CorrespondenceRejectorSurfaceNormal                            SurfaceNormalRejector;
 52 |     
 53 |     //----------------------------------------------------------------------------
 54 |     // Prepare variables
 55 |     //----------------------------------------------------------------------------
 56 |     
 57 |     cloud_src_aligned.reset(new pcl::PointCloud<PointT>);
 58 |     
 59 |     //--------------------------------------------------------------------------
 60 |     // Run ICP
 61 |     //--------------------------------------------------------------------------
 62 |     
 63 |     pcl::IterativeClosestPoint<PointT, PointT, float> icp;
 64 |     icp.setInputSource(cloud_src);
 65 |     icp.setInputTarget(cloud_tgt);
 66 |     
 67 |     // Set correspondence estimation method
 68 |     boost::shared_ptr<ClosestPoint> correspondece_estimation (new ClosestPoint);
 69 |     icp.setCorrespondenceEstimation(correspondece_estimation);
 70 | 
 71 |     // Set error metric and minimization
 72 |     boost::shared_ptr<PointToPlaneLM> transformation_estimation (new PointToPlaneLM);
 73 |     icp.setTransformationEstimation (transformation_estimation);
 74 |     
 75 |     // Add max correspondence distance rejector
 76 |     if (max_correspondence_distance > 0.0f)
 77 |     {
 78 |       boost::shared_ptr<DistanceRejector> distance_rejector (new DistanceRejector);
 79 |       distance_rejector->setMaximumDistance(max_correspondence_distance);
 80 |       icp.addCorrespondenceRejector(distance_rejector);
 81 |     }
 82 | 
 83 |     // Add surface normal angle rejector
 84 |     if (max_correspondece_surface_normal_angle > 0.0f)
 85 |     {
 86 |       boost::shared_ptr<SurfaceNormalRejector> surface_normal_rejector (new SurfaceNormalRejector);
 87 |       surface_normal_rejector->setThreshold(std::cos(max_correspondece_surface_normal_angle));
 88 |       icp.addCorrespondenceRejector(surface_normal_rejector);
 89 |     }
 90 | 
 91 |     // Add ono-to-one rejector
 92 |     if (use_one_to_one)
 93 |     {
 94 |       boost::shared_ptr<OneToOneRejector> one_to_one_rejector (new OneToOneRejector);
 95 |       icp.addCorrespondenceRejector(one_to_one_rejector);
 96 |     }
 97 |     
 98 |     // Set convergence criteria
 99 |     icp.setMaximumIterations(max_iterations);
100 |     icp.setTransformationEpsilon(1e-100);
101 |     icp.setEuclideanFitnessEpsilon(1e-100);
102 |     
103 |     // Run ICP
104 |     icp.align (*cloud_src_aligned);
105 |     transform.matrix() = icp.getFinalTransformation();
106 |     correspondences = icp.correspondences_;
107 |     
108 |     // Check if ICP has converged
109 |     if (!icp.hasConverged())
110 |       return false;
111 |     
112 |     return true;
113 |   }
114 | }
115 | 
116 | #endif  // POINTCLOUD_ALIGNMENT_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/felzenswalb_segmentation/felzenswalb_segmentation.h:
--------------------------------------------------------------------------------
  1 | #ifndef FELZENSWALB_SEGMENTATION_H
  2 | #define FELZENSWALB_SEGMENTATION_H
  3 | 
  4 | namespace alg
  5 | {
  6 |   /** \brief @b FelzenswalbSegmentation
  7 |    * Segments a pointcloud with normals using the graph segmentation approach
  8 |    * described in "Efficient Graph-Based Image Segmentation" by Felzenswalb et
  9 |    * al.
 10 |    * 
 11 |    * The algorithm works a follows. First a graph is defined on the pointcloud 
 12 |    * by connecting each point to k of it's nearest neighbors that are within a 
 13 |    * radius r from it. The weight of each edge is calculated based on the 
 14 |    * difference of normals between two neighboring points as well as the 
 15 |    * distance between them. The resulting weighted graph is then segmented using
 16 |    * the approach of Felzenswalb et al.
 17 |    */
 18 |   template <typename PointT>
 19 |   class FelzenswalbSegmentation : public pcl::PCLBase<PointT>
 20 |   {
 21 |     public:
 22 |       
 23 |       typedef boost::function<float (const PointT&, const PointT&)> edgeWeightFunction;
 24 | 
 25 |       using pcl::PCLBase <PointT>::input_;
 26 |       using pcl::PCLBase <PointT>::indices_;
 27 |       using pcl::PCLBase <PointT>::initCompute;
 28 |       using pcl::PCLBase <PointT>::deinitCompute;
 29 | 
 30 |     public:
 31 | 
 32 |       /** \brief Constructor that sets default values for member variables. */
 33 |       FelzenswalbSegmentation ();
 34 | 
 35 |       /** \brief This destructor destroys the cloud, normals and search method used for
 36 |         * finding KNN. In other words it frees memory.
 37 |         */
 38 |       virtual
 39 |       ~FelzenswalbSegmentation ();
 40 | 
 41 |       /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
 42 |       int
 43 |       getMinSegmentSize ()  const;
 44 | 
 45 |       /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. */
 46 |       void
 47 |       setMinSegmentSize (const int min_cluster_size);
 48 | 
 49 |       /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
 50 |       int
 51 |       getMaxSegmentSize () const;
 52 | 
 53 |       /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. */
 54 |       void
 55 |       setMaxSegmentSize (const int max_cluster_size);
 56 | 
 57 |       /** \brief Returns the number of nearest neighbours used for KNN. */
 58 |       unsigned int
 59 |       getNumberOfNeighbors () const;
 60 | 
 61 |       /** \brief Set maximum number of neighbors for each point. */
 62 |       void
 63 |       setNumberOfNeighbors ( const unsigned int num_neighbors_);      
 64 | 
 65 |       /** \brief Returns the maximum distance between point and it's neighbor. */
 66 |       float
 67 |       getNeighborRadius () const;
 68 | 
 69 |       /** \brief Set the maximum distance between point and it's neighbor. */
 70 |       void
 71 |       setNeighborRadius ( const float num_neighbors_);
 72 |       
 73 |       /** \brief Returns the segmentation threshold. */
 74 |       float
 75 |       getThreshold () const;
 76 | 
 77 |       /** \brief Set the segmentation threshold. */
 78 |       void
 79 |       setThreshold ( const float seg_threshold);
 80 |             
 81 |       /** \brief Set condition function that needs to be satisfied for two neighbouring points to be in the same cluster */
 82 |       void
 83 |       setEdgeWeightFunction (float (*edge_weight_function) (const PointT&, const PointT&));
 84 |       
 85 |       /** \brief Returns the graph generated on the pointcloud.
 86 |        * \note this method should be run after segmentation process.
 87 |        */
 88 |       void
 89 |       getPointGraph (std::vector<std::pair<int, int> > &edges, std::vector<float> &edge_weights) const;
 90 | 
 91 |       /** \brief Segment the pointcloud.
 92 |         * \param[out] segments  a vector of segments where each segment is represented by the indices of points belonging to it
 93 |         */
 94 |       virtual void
 95 |       segment (std::vector<std::vector<int> > &segments);
 96 | 
 97 |     protected:
 98 | 
 99 |       /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
100 |         * the current settings. If it is possible then it returns true.
101 |         */
102 |       virtual bool
103 |       prepareForSegmentation ();
104 |       
105 |       /** \brief Calculate the weights of the edges connecting points
106 |         */
107 |       virtual void
108 |       calculateEdgeWeights ();
109 | 
110 |     protected:
111 | 
112 |       /** \brief Stores the minimum number of points that a cluster needs to contain in order to be considered valid. */
113 |       int min_pts_per_segment_;
114 | 
115 |       /** \brief Stores the maximum number of points that a cluster needs to contain in order to be considered valid. */
116 |       int max_pts_per_segment_;
117 | 
118 |       /** \brief Segmentation threshold. */
119 |       float seg_threshold_;
120 | 
121 |       /** \brief Number of neighbors for each point. */
122 |       unsigned int num_neighbors_;
123 |       
124 |       /** \brief Maximum distance between a point and it's neighbor. */
125 |       float neighbor_radius_;
126 |       
127 |       /** \brief The condition function between two points that needs to to be satisfied for two neighboring points to belong to the same cluster.  */
128 |       edgeWeightFunction edge_weight_function_;
129 |             
130 |       /** \brief Point graph edges. */
131 |       std::vector<std::pair<int, int> > edges_;
132 |       
133 |       /** \brief Point graph edge weights. */
134 |       std::vector<float> edge_weights_;
135 |       
136 |       /** \brief Variable indicating if point graph should be recomputed. */
137 |       bool recompute_graph_;
138 | 
139 |     public:
140 |       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
141 |   };
142 | }
143 | 
144 | #endif
145 | 


--------------------------------------------------------------------------------
/include/namaris/utilities/alphanum/alphanum.cpp:
--------------------------------------------------------------------------------
  1 | // This library is free software; you can redistribute it and/or
  2 | // modify it under the terms of the GNU Lesser General Public
  3 | // License as published by the Free Software Foundation; either
  4 | // version 2.1 of the License, or any later version.
  5 | //
  6 | // This library is distributed in the hope that it will be useful,
  7 | // but WITHOUT ANY WARRANTY; without even the implied warranty of
  8 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  9 | // Lesser General Public License for more details.
 10 | //
 11 | // You should have received a copy of the GNU Lesser General Public
 12 | // License along with this library; if not, write to the Free Software
 13 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 14 | 
 15 | 
 16 | //  Contributed by Ted Romaniszyn
 17 | //
 18 | //  I found your Perl script for a alpha-numeric sort for a job I'm doing right
 19 | //  now and have ported it to a MFC class
 20 | //  so that I can easily incorporate it into my VC++ application.
 21 | //
 22 | //  My class is modified version of the CSortStringArray class described in the
 23 | //  Microsoft Knowledge Base
 24 | //  Article ID: Q120961 "How to Sort a CStringArrary in MFC": <a href="ftp://ftp.microsoft.com/developr/visual_c/kb/q120/9/61.txt">ftp://ftp.microsoft.com/developr/visual_c/kb/q120/9/61.txt</a>
 25 | //
 26 | //  I am using this new class to sort a list of files selected in a multi-select
 27 | //  CFileDialog.  I populate the CSortStringArray object with the selected files
 28 | //  and then call the "Sort" method to sort the array.
 29 | //
 30 | //  Here is the source code for the modified "CompareAndSwap" method that
 31 | //  incorporates your improved sorting algorithm.  You can
 32 | //  put this code on your WEB page for other people to use.
 33 | 
 34 |   BOOL CSortStringArray::CompareAndSwap( int pos )
 35 |   {
 36 |       // The Alphanum Algorithm is an improved sorting algorithm for strings
 37 |       // containing numbers.  Instead of sorting numbers in ASCII order like
 38 |       // a standard sort, this algorithm sorts numbers in numeric order.
 39 |       //
 40 |       // The Alphanum Algorithm is discussed at http://www.DaveKoelle.com
 41 |       //
 42 |       // This code is a MFC port of the Perl script Alphanum.pl
 43 | 
 44 |       static const TCHAR szNumbers[] = _T("0123456789");
 45 |       int posFirst = pos;
 46 |       int posNext  = pos + 1;
 47 | 
 48 |       //Get the strings to compare and make them both lower case
 49 |       CString strFirst = GetAt(posFirst);     strFirst.MakeLower();
 50 |       CString strNext  = GetAt(posNext);      strNext.MakeLower();
 51 | 
 52 |       int n( 0 );
 53 |       while ( n == 0 )        //Get next "chunk"
 54 |       {
 55 |           //A chunk is either a group of letters or a group of numbers
 56 |           CString strFirstChunk;
 57 |           CString strFirstChar( strFirst[0] );
 58 |           if ( strFirstChar.FindOneOf( szNumbers ) != -1 )
 59 |           {
 60 |               strFirstChunk = strFirst.SpanIncluding( szNumbers );
 61 |           }
 62 |           else
 63 |           {
 64 |               strFirstChunk = strFirst.SpanExcluding( szNumbers );
 65 |           }
 66 | 
 67 |           CString strNextChunk;
 68 |           strFirstChar = strNext[0];
 69 |           if ( strFirstChar.FindOneOf( szNumbers ) != -1 )
 70 |           {
 71 |               strNextChunk = strNext.SpanIncluding( szNumbers );
 72 |           }
 73 |           else
 74 |           {
 75 |               strNextChunk = strNext.SpanExcluding( szNumbers );
 76 |           }
 77 | 
 78 |           //Remove the extracted chunks from the strings
 79 |           strFirst = strFirst.Mid(strFirstChunk.GetLength(),
 80 |                          strFirst.GetLength() - strFirstChunk.GetLength() );
 81 |           strNext  = strNext.Mid(strNextChunk.GetLength(),
 82 |                          strNext.GetLength() - strNextChunk.GetLength() );
 83 | 
 84 |           // Now Compare the chunks
 85 |           //# Case 1: They both contain letters
 86 |           if ( strFirstChunk.FindOneOf( szNumbers ) == -1 &&
 87 |                strNextChunk.FindOneOf( szNumbers )  == -1 )
 88 |           {
 89 |               n = strFirstChunk.Compare( strNextChunk );
 90 |           }
 91 |           else
 92 |           {
 93 |               //Case 2: They both contain numbers
 94 |               if ( strFirstChunk.FindOneOf( szNumbers ) != -1 &&
 95 |                    strNextChunk.FindOneOf( szNumbers )  != -1)
 96 |               {
 97 |                   //Convert the numeric strings to long values
 98 |                   long lFirst = atol( strFirstChunk );
 99 |                   long lNext = atol( strNextChunk );
100 | 
101 |                   n = 0;
102 |                   if ( lFirst > lNext )
103 |                   {
104 |                       n = 1;
105 |                   }
106 |                   else if ( lFirst < lNext )
107 |                   {
108 |                       n = -1;
109 | 
110 |                   }
111 |               }
112 |               else
113 |               {
114 |                   // Case 3: One has letters, one has numbers;
115 |                   // or one is empty
116 |                   n = strFirstChunk.Compare( strNextChunk );
117 | 
118 |                   // If these are equal, make one (which one
119 |                   // is arbitrary) come before
120 |                   // the other   (or else we will be stuck
121 |                   // in this "while n==0" loop)
122 |                   if ( n == 0 )
123 |                   {
124 |                       n = 1;
125 | 
126 |                   }
127 |               }
128 |           }
129 |       }
130 | 
131 |       //Now swap the strings in the array
132 |       if ( n != -1 )
133 |       {
134 |           CString temp = GetAt(posFirst);
135 | 
136 |           SetAt( posFirst, GetAt(posNext) );
137 |           SetAt( posNext, temp );
138 |           return TRUE;
139 |       }
140 |       else
141 |       {
142 |           return FALSE;
143 |       }
144 |   }


--------------------------------------------------------------------------------
/include/namaris/algorithms/region_growing_normal_variation/region_growing_normal_variation.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef REGION_GROWING_NORMAL_SEGMENTATION_HPP
  2 | #define REGION_GROWING_NORMAL_SEGMENTATION_HPP
  3 | 
  4 | #include "region_growing_normal_variation.h"
  5 | 
  6 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  7 | template <typename PointT>
  8 | alg::RegionGrowingNormalVariation<PointT>::RegionGrowingNormalVariation () :
  9 |   normal_variation_threshold_ (0.0f),
 10 |   normals_consistently_oriented_ (false)
 11 | {
 12 |   updateBinaryConditionFunction();  
 13 | }
 14 |       
 15 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 16 | template <typename PointT>
 17 | alg::RegionGrowingNormalVariation<PointT>::~RegionGrowingNormalVariation ()
 18 | {
 19 | }
 20 | 
 21 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 22 | template <typename PointT> inline void
 23 | alg::RegionGrowingNormalVariation<PointT>::setNormalVariationThreshold (const float threshold)
 24 | {
 25 |   normal_variation_threshold_ = threshold;
 26 | }
 27 | 
 28 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 29 | template <typename PointT> inline float
 30 | alg::RegionGrowingNormalVariation<PointT>::getNormalVariationThreshold () const
 31 | {
 32 |   return normal_variation_threshold_;
 33 | }
 34 | 
 35 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 36 | template <typename PointT> inline void
 37 | alg::RegionGrowingNormalVariation<PointT>::setConsistentNormals (const bool normals_consistently_oriented)
 38 | {
 39 |   normals_consistently_oriented_ = normals_consistently_oriented;
 40 |   updateBinaryConditionFunction();
 41 | }
 42 | 
 43 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 44 | template <typename PointT> inline bool
 45 | alg::RegionGrowingNormalVariation<PointT>::getConsistentNormals () const
 46 | {
 47 |   return normals_consistently_oriented_;
 48 | }
 49 | 
 50 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 51 | template <typename PointT> inline void
 52 | alg::RegionGrowingNormalVariation<PointT>::updateBinaryConditionFunction ()
 53 | {
 54 |   if (normals_consistently_oriented_)
 55 |     setBinaryConditionFunction( boost::bind(&RegionGrowingNormalVariation<PointT>::binaryConditionConsistent, this, _1, _2, _3) );
 56 |   else
 57 |     setBinaryConditionFunction( boost::bind(&RegionGrowingNormalVariation<PointT>::binaryConditionNonConsistent, this, _1, _2, _3) );
 58 | }
 59 | 
 60 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 61 | template <typename PointT> inline void
 62 | alg::RegionGrowingNormalVariation<PointT>::reorderIndicesCurvature ()
 63 | {
 64 |   if (!indices_ || indices_->empty())
 65 |   {
 66 |     indices_->resize(input_->size());
 67 |     for (size_t pointId = 0; pointId < indices_->size(); pointId++)
 68 |       (*indices_)[pointId] = pointId;
 69 |   }
 70 |   
 71 |   // Extract curvature values into a vector
 72 |   std::vector<float> curvature (indices_->size());
 73 |   for (size_t pointIdIt = 0; pointIdIt < indices_->size(); pointIdIt++)
 74 |     curvature[pointIdIt] = input_->points[(*indices_)[pointIdIt]].curvature;
 75 |     
 76 |   // Reorder indices in increasing order of curvature
 77 |   std::vector<float> curvature_sorted;
 78 |   std::vector<size_t> order;
 79 |   utl::stdvec::sort(curvature, curvature_sorted, order, utl::stdvec::ASCENDING);
 80 |   *indices_ = utl::stdvec::reorder(*indices_, order);  
 81 | }
 82 | 
 83 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 84 | template <typename PointT> bool
 85 | alg::RegionGrowingNormalVariation<PointT>::binaryConditionConsistent (const PointT& p1, const PointT& p2, float dist_squared) const
 86 | {
 87 |   Eigen::Vector3f n1 = p1.getNormalVector3fMap();
 88 |   Eigen::Vector3f n2 = p2.getNormalVector3fMap();
 89 |     
 90 |   float angle = std::acos(utl::math::clampValue(n1.dot(n2), 0.0f, 1.0f));
 91 |   float distance = std::sqrt(dist_squared);
 92 |   float anglePerDistance = angle/distance;
 93 |   return anglePerDistance < normal_variation_threshold_;  
 94 | }
 95 | 
 96 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 97 | template <typename PointT> bool
 98 | alg::RegionGrowingNormalVariation<PointT>::binaryConditionNonConsistent (const PointT& p1, const PointT& p2, float dist_squared) const
 99 | {
100 |   Eigen::Vector3f n1 = p1.getNormalVector3fMap();
101 |   Eigen::Vector3f n2 = p2.getNormalVector3fMap();
102 |     
103 |   float angle = std::acos(utl::math::clampValue(std::abs(n1.dot(n2)), 0.0f, 1.0f));
104 |   float distance = std::sqrt(dist_squared);
105 |   float anglePerDistance = angle/distance;
106 |   return anglePerDistance < normal_variation_threshold_;  
107 | }
108 | 
109 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110 | template <typename PointT> inline bool
111 | alg::RegionGrowingNormalVariation<PointT>::prepareForSegmentation ()
112 | {  
113 |   // If user forgot to pass point cloud or if it is empty
114 |   if ( normal_variation_threshold_ < 0.0f )
115 |   {
116 |     std::cout << "[alg::RegionGrowingNormalVariation::prepareForSegmentation] normal variation threhsold must be non-negative!" << std::endl;
117 |     return (false);
118 |   }
119 |   
120 |   bool good = alg::RegionGrowing<PointT>::prepareForSegmentation();
121 |   
122 |   if (good)
123 |   {
124 |     this->reorderIndicesCurvature();
125 |     return true;
126 |   }
127 |   else
128 |   {
129 |     return false;
130 |   }
131 | }
132 | 
133 | #endif  // REGION_GROWING_NORMAL_SEGMENTATION_HPP


--------------------------------------------------------------------------------
/sample_pcl_viewer/src/main.cpp:
--------------------------------------------------------------------------------
  1 | // STD includes
  2 | #include <iostream>
  3 | 
  4 | // PCL
  5 | #include <pcl/point_cloud.h>
  6 | #include <pcl/point_types.h>
  7 | #include <pcl/io/pcd_io.h>
  8 | 
  9 | // Utilities
 10 | 
 11 | // Project includes
 12 | #include "vis.hpp"
 13 | 
 14 | ////////////////////////////////////////////////////////////////////////////////
 15 | void parseCommandLine(int argc, char** argv, std::string &cloudFilename)
 16 | {
 17 |   cloudFilename = "";
 18 |   
 19 |   // Check parameters
 20 |   for (size_t i = 1; i < static_cast<size_t>(argc); i++)
 21 |   {
 22 |     std::string curParameter (argv[i]);
 23 |     cloudFilename = curParameter;    
 24 |   }  
 25 | }
 26 | 
 27 | ////////////////////////////////////////////////////////////////////////////////
 28 | int main(int argc, char** argv)
 29 | {    
 30 |   //----------------------------------------------------------------------------
 31 |   // Parse command line
 32 |   //----------------------------------------------------------------------------
 33 | 
 34 |   std::string cloudFilename;
 35 |   parseCommandLine(argc, argv, cloudFilename);
 36 |   
 37 |   //----------------------------------------------------------------------------
 38 |   // Load PCD file
 39 |   //----------------------------------------------------------------------------
 40 |   
 41 |   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);  
 42 |   if (pcl::io::loadPCDFile (cloudFilename, *cloud) == -1)
 43 |   {
 44 |     std::cout << "Could not read pointcloud file '" + cloudFilename + "'" << std::endl;
 45 |     return -1;
 46 |   }
 47 |   
 48 |   //----------------------------------------------------------------------------
 49 |   // Calculate distance to origin for each point
 50 |   //----------------------------------------------------------------------------
 51 |   
 52 |   std::vector<float> distanceToOrigin (cloud->size());
 53 |   for (size_t pointId = 0; pointId < cloud->size(); pointId++)
 54 |   {
 55 |     distanceToOrigin[pointId] = cloud->points[pointId].getVector3fMap().norm();
 56 |   }
 57 | 
 58 |   //----------------------------------------------------------------------------
 59 |   // Print instructions
 60 |   //----------------------------------------------------------------------------
 61 |   
 62 |   std::cout << "This sample code shows how to visualize pointclouds using PCL library." << std::endl;
 63 |   std::cout << "There are 3 visualizations available:" << std::endl;
 64 |   std::cout << " 1. Pointcloud without color" << std::endl;
 65 |   std::cout << " 2. Pointcloud with color" << std::endl;
 66 |   std::cout << " 3. Points colored according to their Euclidean distance from the origin" << std::endl;
 67 |   std::cout << std::endl;
 68 |   std::cout << "|         Key         |             Function            |" << std::endl;
 69 |   std::cout << "|---------------------|---------------------------------|" << std::endl;
 70 |   std::cout << "| Keypad number keys  | change cloud visualization type |" << std::endl;
 71 |   std::cout << "| N                   | show normals                    |" << std::endl;
 72 |   std::cout << "| U                   | show colorbar                   |" << std::endl;
 73 |   std::cout << "| Keypad +/-          | change point size               |" << std::endl;
 74 |   std::cout << "| Q                   | quit                            |" << std::endl;
 75 |   
 76 |   //----------------------------------------------------------------------------
 77 |   // Display the pointcloud
 78 |   //----------------------------------------------------------------------------
 79 |   
 80 |   VisState visState;
 81 |   pcl::visualization::PCLVisualizer visualizer;
 82 |   visualizer.setCameraPosition ( 0.0, 0.0,  0.0,   // camera position
 83 |                                  0.0, 0.0,  1.0,   // focal point
 84 |                                  0.0, -1.0, 0.0,   // normal
 85 |                                  0.0);             // viewport  
 86 |   visualizer.setBackgroundColor (utl::pclvis::bgColor.r, utl::pclvis::bgColor.g, utl::pclvis::bgColor.b);
 87 |   visualizer.registerKeyboardCallback(keyboard_callback, (void*)(&visState));
 88 |   
 89 |   visState.updateDisplay_ = true;
 90 |   
 91 |   while (!visualizer.wasStopped())
 92 |   {
 93 |     // Update display if needed
 94 |     if (visState.updateDisplay_)
 95 |     {     
 96 |       // First remove everything
 97 |       visualizer.removeAllPointClouds();
 98 |       visualizer.removeAllShapes();
 99 |       visualizer.removeAllCoordinateSystems();
100 |       visState.updateDisplay_ = false;
101 | 
102 |       // Show pointcloud without color color
103 |       if (visState.cloudDisplayState_ == VisState::CLOUD_NO_COLOR)
104 |       {
105 |         utl::pclvis::showPointCloud<pcl::PointXYZRGBNormal>(visualizer, cloud, "cloud", visState.pointSize_, utl::pclvis::Color(0.5, 0.5, 0.5));
106 |         visualizer.addText("Cloud without color", 0, 100, 24, 1.0, 1.0, 1.0);
107 |       }
108 |       
109 |       // Show pointcloud with color
110 |       if (visState.cloudDisplayState_ == VisState::CLOUD_COLOR)
111 |       {
112 |         utl::pclvis::showPointCloudColor<pcl::PointXYZRGBNormal>(visualizer, cloud, "cloud", visState.pointSize_);
113 |         visualizer.addText("Cloud with color", 0, 100, 24, 1.0, 1.0, 1.0);
114 |       }
115 |       
116 |       // Color points according to their distance from origin
117 |       if (visState.cloudDisplayState_ == VisState::CLOUD_DISTANCE_TO_ORIGIN)
118 |       {
119 |         utl::pclvis::showPointCloudWithData<pcl::PointXYZRGBNormal, float>(visualizer, cloud, distanceToOrigin, "cloud_curvature", visState.pointSize_);
120 |         visualizer.addCoordinateSystem(0.2, "origin");
121 |         visualizer.addText("Point distance to origin", 0, 100, 24, 1.0, 1.0, 1.0);
122 |       }
123 |       
124 |       // Add normals
125 |       if (visState.showNormals_)
126 |         utl::pclvis::showNormalCloud<pcl::PointXYZRGBNormal>(visualizer, cloud, 10, 0.02, "cloud_normals", visState.pointSize_, utl::pclvis::Color(0.0, 1.0, 0.0));      
127 |     }
128 | 
129 |     // Spin once
130 |     visualizer.spinOnce();
131 |     boost::this_thread::sleep (boost::posix_time::milliseconds (50));
132 |   }
133 |   
134 |   return 0;
135 | }


--------------------------------------------------------------------------------
/include/namaris/utilities/map.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef MAP_UTILITIES_HPP
  2 | #define MAP_UTILITIES_HPP
  3 | 
  4 | 
  5 | // CPP tools
  6 | #include <namaris/utilities/std_vector.hpp>
  7 | 
  8 | namespace utl
  9 | {  
 10 |   namespace map
 11 |   {
 12 |     /** \brief A one to many map.
 13 |      * 
 14 |      * Given two sets of unsigned integers a map defines a correspondence
 15 |      * between the elements of the first set (domain) and elements of the second
 16 |      * set (range). The outer vector corresponds to domain while inner vectors 
 17 |      * correspond to range.
 18 |      */
 19 |     typedef std::vector<std::vector<int> > Map;    // A map from points to segments
 20 | 
 21 |     /** \brief Get maximum domain element id
 22 |      *  \param[in]  map  input map
 23 |      *  \return     domain maximum element id
 24 |      */
 25 |     inline
 26 |     int domainMax (const Map &map)
 27 |     {
 28 |       return map.size();
 29 |     }
 30 | 
 31 |     /** \brief Get maximum domain element id
 32 |      *  \param[in]  map  input map
 33 |      *  \return     domain maximum element id
 34 |      */
 35 |     inline
 36 |     int rangeMax (const Map &map)
 37 |     {
 38 |       int maxId = 0;
 39 |       for (size_t i = 0; i < map.size(); i++)
 40 |         if (map[i].size() > 0)
 41 |           maxId = std::max(maxId, stdvec::vectorMax<int>(map[i]));
 42 |       
 43 |       return maxId;
 44 |     }
 45 | 
 46 |     /** \brief Get minimum domain element id
 47 |      *  \param[in]  map  input map
 48 |      *  \return     domain minimum element id
 49 |      */
 50 |     inline
 51 |     int rangeMin (const Map &map)
 52 |     {
 53 |       int minId = 0;
 54 |       for (size_t i = 0; i < map.size(); i++)
 55 |         if (map[i].size() > 0)
 56 |           minId = std::min(minId, stdvec::vectorMin<int>(map[i]));
 57 |       
 58 |       return minId;
 59 |     }
 60 |         
 61 |     /** \brief Checks if a map is injective i.e. if for each domain element 
 62 |      * there is only one corresponding range element
 63 |      *  \param[in]  map input map
 64 |      *  \return     true if map is injective
 65 |      */
 66 |     inline
 67 |     bool isInjective (const Map &map)
 68 |     {
 69 |       // If any of the mapped values correspond to 
 70 |       for (size_t mappedId = 0; mappedId < map.size(); mappedId++)
 71 |         if (map[mappedId].size() > 1)
 72 |           return false;
 73 |         
 74 |       return true;
 75 |     }    
 76 |     
 77 |     /** \brief Invert a map.
 78 |      *  \param[in]  map input map
 79 |      *  \return     domain maximum element id
 80 |      */
 81 |     inline
 82 |     Map invertMap (const Map &map)
 83 |     {
 84 |       // Initialize map
 85 |       int maxRangeId = rangeMax(map);
 86 |       Map inverseMap (maxRangeId+1);
 87 |       if (maxRangeId < 1)
 88 |       {
 89 |         std::cout << "[utl::map::inverse] maximum range element id is 0. Inverse map is empty." << std::endl;
 90 |         abort();
 91 |       }
 92 |       
 93 |       for (size_t domId = 0; domId < map.size(); domId++)
 94 |         for (size_t rangeIdIt = 0; rangeIdIt < map[domId].size(); rangeIdIt++)
 95 |         {
 96 |           int rangeId = map[domId][rangeIdIt];
 97 |           if (rangeId < 0)
 98 |           {
 99 |             std::cout << "[utl::map::inverse] range element value is negative. Cannot invert." << std::endl;
100 |             abort();
101 |           }
102 |           inverseMap[rangeId].push_back(domId);
103 |         }
104 |       
105 |       return inverseMap;
106 |     }
107 |     
108 |     /** \brief Remap values in a vector using a map
109 |      *  \param[in]  v input vector
110 |      *  \param[in]  map input map
111 |      *  \param[in]  check_injective bool indicating whether to check if input mapping is injective
112 |      *  \return     mapped vector
113 |      */
114 |     inline
115 |     std::vector<int> remapVector (const std::vector<int> &v, const Map &map, bool check_injective = true)
116 |     { 
117 |       // First check that remapping is possible
118 |       if (check_injective && !isInjective(map))
119 |       {
120 |         std::cout << "[utl::map::mapVector] mapping is not injective, mapping is not possible." << std::endl;
121 |         abort();
122 |       }
123 |       
124 |       int vectorMax     = utl::stdvec::vectorMax<int>(v);
125 |       int vectorMin     = utl::stdvec::vectorMin<int>(v);
126 |       int mapDomainMax  = domainMax(map);
127 |       
128 |       if (vectorMin < 0)
129 |       {
130 |         std::cout << "[utl::map::mapVector] vector has negative values, mapping is not possible." << std::endl;
131 |         abort();
132 |       }
133 |       
134 |       if (vectorMax > mapDomainMax)
135 |       {
136 |         std::cout << "[utl::map::mapVector] vector maximum value is greater than map domain maximum value, mapping is not possible." << std::endl;
137 |         abort();
138 |       }
139 |       
140 |       // Map!
141 |       std::vector<int> v_mapped (v.size());
142 |       for (size_t vIt = 0; vIt < v.size(); vIt++)
143 |       {
144 |         int domainVal = v[vIt];
145 |         if (map[domainVal].size() == 0)
146 |         {
147 |           std::cout << "[utl::map::mapVector] map has no range value for domain value, mapping not possible " << domainVal << std::endl;
148 |           abort();
149 |         }
150 |         v_mapped[vIt] = map[domainVal][0];
151 |       }
152 |       
153 |       return v_mapped;
154 |     }
155 |     
156 |     /** \brief Remap range values of a map using another map
157 |      *  \param[in]  map_src map to be remapped
158 |      *  \param[in]  map map used for remapping
159 |      *  \return     remapped source map
160 |      */
161 |     inline
162 |     std::vector<std::vector<int> > remapMap (const Map &map_src, const Map &map)
163 |     { 
164 |       // First check that remapping is possible
165 |       if (!isInjective(map))
166 |       {
167 |         std::cout << "[utl::map::mapMap] mapping is not injective, mapping is not possible." << std::endl;
168 |         abort();
169 |       }
170 |       
171 |       // Map!
172 |       Map mas_src_remapped (map_src.size());
173 |       for (size_t vId = 0; vId < map_src.size(); vId++)
174 |         mas_src_remapped[vId] = remapVector(map_src[vId], map, false);
175 |       
176 |       return mas_src_remapped;
177 |     }    
178 |   }
179 | }
180 | 
181 | #endif  // MAP_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/pcl_visualization/color.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef PCL_VISUALIZATION_COLOR_HPP
  2 | #define PCL_VISUALIZATION_COLOR_HPP
  3 | 
  4 | // STD
  5 | #include <iostream>
  6 | 
  7 | // PCL
  8 | #include <pcl/io/point_cloud_image_extractors.h>
  9 | 
 10 | // IF PCL my version
 11 | //#include <pcl/io/impl/point_cloud_image_extractors.hpp>
 12 | 
 13 | // IF PCL 1.8 trunk
 14 | #include <pcl/common/colors.h>
 15 | 
 16 | // CPP tools
 17 | #include <namaris/utilities/pcl_visualization/color.h>
 18 | 
 19 | namespace utl
 20 | {  
 21 |   namespace pclvis
 22 |   {
 23 |   
 24 |     //--------------------------------------------------------------------------
 25 |     // Color generators
 26 |     //--------------------------------------------------------------------------
 27 | 
 28 |     /** \brief Generate a random color
 29 |      *  \return color      RGB color triplet corresponding to the element. Colors are in [0,1] range
 30 |      */
 31 |     inline
 32 |     Color getRandomColor()
 33 |     {
 34 |       Color color;
 35 |       std::cout << "[utl::pclvis::getRandomColor] not implemented yet!" << std::endl;
 36 |       std::abort();
 37 |       
 38 |       return color;
 39 |     }
 40 |     
 41 |     /** \brief Generate a random color
 42 |      *  \param[in]  id      index of the element
 43 |      *  \return color      RGB color triplet corresponding to the element. Colors are in [0,1] range
 44 |      */
 45 |     inline
 46 |     Color getGlasbeyColor(const int id)
 47 |     {
 48 |       Color color;
 49 |       
 50 | //       if (maxId > static_cast<int>(pcl::GLASBEY_LUT_SIZE))
 51 | //       {
 52 | //         std::cout << "[smt::pclvis::getGlasbeyColor] Maximum number of Glasbey colors is " << pcl::GLASBEY_LUT_SIZE << ", you requested " << maxId << std::endl;
 53 | //         std::cout << "[smt::pclvis::getGlasbeyColor] Some ids might have same colors." << std::endl;
 54 | //       }
 55 | 
 56 | //       // IF PCL my version
 57 | //       int tmpId = id % pcl::GLASBEY_LUT_SIZE;
 58 | //       color.r = static_cast<float>(pcl::GLASBEY_LUT[tmpId * 3 + 0]) / 255;
 59 | //       color.g = static_cast<float>(pcl::GLASBEY_LUT[tmpId * 3 + 1]) / 255;
 60 | //       color.b = static_cast<float>(pcl::GLASBEY_LUT[tmpId * 3 + 2]) / 255;
 61 |       
 62 |       // IF PCL 1.8 trunk      
 63 |       int tmpId = id % pcl::GlasbeyLUT().size();
 64 |       pcl::RGB color_pcl = pcl::GlasbeyLUT().at(tmpId);
 65 |       color.r = static_cast<float>(color_pcl.r) / 255;
 66 |       color.g = static_cast<float>(color_pcl.g) / 255;
 67 |       color.b = static_cast<float>(color_pcl.b) / 255;
 68 |       
 69 |       return color;
 70 |     }
 71 | 
 72 |     /** \brief Convert a single point with RGB information to grayscale
 73 |      *  \param[in,out]  point point to be converted to grayscale
 74 |      *  \note conversion is done using the formula used in OpenCV (http://docs.opencv.org/modules/imgproc/doc/miscellaneous_transformations.html)
 75 |      */
 76 |     template <typename PointT>
 77 |     inline
 78 |     void rgb2gray(PointT &point)
 79 |     {
 80 |       uint8_t gray = static_cast<uint8_t> ( static_cast<float>(point.r) * 0.299 +
 81 |                                             static_cast<float>(point.r) * 0.587 +
 82 |                                             static_cast<float>(point.r) * 0.114 );
 83 |       point.r = gray;
 84 |       point.g = gray;
 85 |       point.b = gray;
 86 |     }
 87 |         
 88 |     /** \brief Convert color in pointcloud to grayscale
 89 |      *  \param[in,out]  cloud cloud to be converted to grayscale
 90 |      *  \note conversion is done using the formula used in OpenCV (http://docs.opencv.org/modules/imgproc/doc/miscellaneous_transformations.html)
 91 |      */
 92 |     template <typename PointT>
 93 |     inline
 94 |     void rgb2gray(pcl::PointCloud<PointT> &cloud)
 95 |     {
 96 |       for (size_t pointId = 0; pointId < cloud.size(); pointId++)
 97 |         rgb2gray<PointT>(cloud.points[pointId]);
 98 |     }
 99 |     
100 |     /** \brief Change the tint of a point to a given color
101 |      *  \param[in,out]  point point to be converted to grayscale
102 |      *  \param[in]      color color of the tint
103 |      *  \param[in]      alpha amount of tint. 0 leaves point color unchanged while 1 replace it by input color. Default 0.5
104 |      */
105 |     template <typename PointT>
106 |     inline
107 |     void tintPoint(PointT &point, const Color &color, const float alpha = 0.3)
108 |     {      
109 |       point.r = static_cast<uint8_t>(std::min(255.0f, static_cast<float>(point.r * (1-alpha)  +  color.r * alpha * 255.0f)));
110 |       point.g = static_cast<uint8_t>(std::min(255.0f, static_cast<float>(point.g * (1-alpha)  +  color.g * alpha * 255.0f)));
111 |       point.b = static_cast<uint8_t>(std::min(255.0f, static_cast<float>(point.b * (1-alpha)  +  color.b * alpha * 255.0f)));
112 |     }
113 |     
114 |     /** \brief Change the tint of a pointcloud to a given color
115 |      *  \param[in,out]  cloud pointcloud to be converted to grayscale
116 |      *  \param[in]      color color of the tint
117 |      *  \param[in]      alpha amount of tint. 0 leaves point color unchanged while 1 replace it by input color. Default 0.5
118 |      */
119 |     template <typename PointT>
120 |     inline
121 |     void tintPointCloud(pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, const Color &color, const float alpha = 0.3)
122 |     {
123 |       for (size_t pointIdIt = 0; pointIdIt < indices.size(); pointIdIt++)
124 |         tintPoint<PointT>(cloud.points[indices[pointIdIt]], color, alpha);
125 |     }
126 |     
127 |     /** \brief Change the tint of a pointcloud to a given color
128 |      *  \param[in,out]  cloud pointcloud to be converted to grayscale
129 |      *  \param[in]      color color of the tint
130 |      *  \param[in]      alpha amount of tint. 0 leaves point color unchanged while 1 replace it by input color. Default 0.5
131 |      */
132 |     template <typename PointT>
133 |     inline
134 |     void tintPointCloud(pcl::PointCloud<PointT> &cloud, const Color &color, const float alpha = 0.3)
135 |     {
136 |       std::vector<int> indices (cloud.size());
137 |       for (size_t pointId = 0; pointId < cloud.size(); pointId++)
138 |         indices[pointId] = pointId;
139 |       
140 |       tintPointCloud<PointT>(cloud, indices, color, alpha);
141 |     }
142 |   }
143 | }
144 | 
145 | 
146 | #endif  // PCL_VISUALIZATION_COLOR_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/mst.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef MST_HPP
  2 | #define MST_HPP
  3 | 
  4 | // Boost includes
  5 | #include <boost/graph/adjacency_list.hpp>
  6 | #include <boost/graph/kruskal_min_spanning_tree.hpp>
  7 | #include <boost/graph/connected_components.hpp>
  8 | 
  9 | // CPP Tools
 10 | #include <namaris/utilities/graph.hpp>
 11 | 
 12 | namespace alg
 13 | {
 14 |   // Typedefs
 15 |   typedef boost::adjacency_list < boost::vecS, 
 16 |                                   boost::vecS,
 17 |                                   boost::undirectedS,
 18 |                                   boost::no_property,
 19 |                                   boost::property < boost::edge_weight_t, float > > Graph;
 20 |                                   
 21 |   typedef boost::graph_traits < Graph >::edge_descriptor Edge;
 22 |   typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;
 23 |                                       
 24 |   /** \brief Compute the minimum spaning tree of a graph using Kruskal's algorithm
 25 |     * \param[in]  edges         edges of the graph.
 26 |     * \param[in]  edge_weights  weights of the edges
 27 |     * \param[out] msts          MST for each connected component of the original graph
 28 |     * \note http://www.boost.org/doc/libs/1_57_0/libs/graph/doc/kruskal_min_spanning_tree.html
 29 |     * \note * if input graph has multiple connected components - an MST of each
 30 |     * one of them will be returned. Isolated vertices however are not considered
 31 |     * as valid MSTs and are not returned. 
 32 |     * \note * vertex indices do not have to be continuous
 33 |     * \note * isolated vertices are not considered 
 34 |     */
 35 |   bool getGraphMST  ( const utl::graph::Graph         &graph, 
 36 |                       const utl::graph::GraphWeights  &graph_weights,
 37 |                       std::vector<utl::graph::Graph>  &msts
 38 |                     )
 39 |   {
 40 |     //////////////////////////////////////////////////////////////////////////////
 41 |     // Check that edges and weights have same dimensions
 42 |     
 43 |     if (graph.size() != graph_weights.size())
 44 |     {
 45 |       std::cout << "[alg::getGraphMST] number of input edges and edge weights are not equal." << std::endl;
 46 |       return false;
 47 |     }
 48 |     
 49 |     for (size_t sourceId = 0; sourceId < graph.size(); sourceId++)
 50 |     {
 51 |       if (graph[sourceId].size() != graph_weights[sourceId].size())
 52 |       {
 53 |         std::cout << "[alg::getGraphMST] number of input edges and edge weights are not equal." << std::endl;
 54 |         return false;
 55 |       }      
 56 |     }
 57 |     
 58 |     //////////////////////////////////////////////////////////////////////////////
 59 |     // Extract edges from graph
 60 |     
 61 |     std::vector<std::pair<int, int> > edges;
 62 |     std::vector<float> edge_weights;
 63 |     
 64 |     utl::graph::graphWeighted2EdgePairs(graph, graph_weights, edges, edge_weights);
 65 |           
 66 |     //////////////////////////////////////////////////////////////////////////////
 67 |     // Create graph
 68 |     
 69 |     int numVertices = graph.size();
 70 |     Graph g (&edges[0], &edges[0]+edges.size(), &edge_weights[0], numVertices);
 71 |       
 72 |     //////////////////////////////////////////////////////////////////////////////
 73 |     // Find MST
 74 | 
 75 |     std::vector < Edge > spanning_tree;
 76 |     kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));
 77 |     
 78 |     //////////////////////////////////////////////////////////////////////////////
 79 |     // Find connected components of the graph
 80 |     
 81 |     // Find connected components
 82 |     std::vector<int> CC(numVertices);
 83 |     int numCCs = boost::connected_components(g, &CC[0]);
 84 |     
 85 |     // Remove connected components containing a single vertex
 86 |     std::vector<int> CCRemap (numCCs, -1);
 87 |     int numValidCC = 0;
 88 |     for (size_t i = 0; i < numCCs; i++)
 89 |     {
 90 |       int CCSize = std::count(CC.begin(), CC.end(), static_cast<int>(i));
 91 |       
 92 |       if (CCSize > 1)
 93 |         CCRemap[i] = numValidCC++;
 94 |     }
 95 |     
 96 |     //////////////////////////////////////////////////////////////////////////////
 97 |     // Prepare output
 98 | 
 99 |     msts.clear();
100 |     msts.resize(numValidCC);
101 |     for (size_t CC = 0; CC < numValidCC; CC++)
102 |       msts[CC].resize(numVertices);
103 |     
104 |     // Go through all edges of the MST and add them to the corresponding connected component  
105 |     for (size_t i = 0; i < spanning_tree.size(); i++)
106 |     {
107 |       int v1    = boost::source(spanning_tree[i], g);
108 |       int v2    = boost::target(spanning_tree[i], g);
109 |       int CCid  = CCRemap[CC[v1]];
110 |       
111 |       utl::graph::addEdge(v1, v2, msts[CCid]);
112 |     }
113 |       
114 |     //////////////////////////////////////////////////////////////////////////////
115 |     // Debug
116 |     
117 |     // Print the MST
118 |   //   boost::property_map < Graph, boost::edge_weight_t >::type weight = get(boost::edge_weight, g);    
119 |   //   std::cout << "Print the edges in the MST:" << std::endl;
120 |   //   for (std::vector < Edge >::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei)
121 |   //   {
122 |   //     std::cout << boost::source(*ei, g) << " <--> " << boost::target(*ei, g) << " with weight of " << weight[*ei] << std::endl;
123 |   //   }
124 | 
125 |   //   // Print connected component remap
126 |   //   for (size_t i = 0; i < numCCs; i++)
127 |   //     std::cout << "Connected component " << i << " -> " << CCRemap[i] << std::endl;
128 | 
129 |     
130 |   //   // Print connected components of the graph
131 |   //   std::vector<int>::size_type i;
132 |   //   std::cout << "Total number of components: " << numCCs << std::endl;
133 |   //   for (i = 0; i != CC.size(); ++i)
134 |   //   {
135 |   //     std::cout << "Vertex " << i <<" is in component " << CC[i] << std::endl;
136 |   //   }
137 |   //   std::cout << std::endl;  
138 |   //     
139 |   //   // Print MSTs
140 |   //   for (size_t i = 0; i < msts.size(); i++)
141 |   //   {
142 |   //     std::cout << "MST " << i << ":" << std::endl;
143 |   //     for (size_t j = 0; j < msts[i].size(); j++)
144 |   //       std::cout<< "   " << msts[i][j].first << ", " << msts[i][j].second << std::endl;
145 |   //   }
146 |     
147 |     return true;
148 |   }
149 | }
150 |   
151 | # endif // MST_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/pcl_visualization/vtk_colormaps.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef VTK_COLORMAPS_HPP
  2 | #define VTK_COLORMAPS_HPP
  3 | 
  4 | #include <namaris/utilities/pcl_visualization/vtk_colormaps.h>
  5 | 
  6 | namespace utl
  7 | {
  8 |   namespace pclvis
  9 |   {
 10 |     //////////////////////////////////////////////////////////////////////////////
 11 |     void Colormap::setColormapType (COLORMAP_TYPE colormapType)
 12 |     {
 13 |       colormapType_ = colormapType;
 14 |     }
 15 |     
 16 |     //////////////////////////////////////////////////////////////////////////////
 17 |     void Colormap::setRangeLimits (double minVal, double maxVal)
 18 |     {
 19 |       minVal_ = minVal;
 20 |       maxVal_ = maxVal;
 21 |     }
 22 |     
 23 |     //////////////////////////////////////////////////////////////////////////////
 24 |     void Colormap::resetRangeLimits()
 25 |     {
 26 |       minVal_ = std::numeric_limits<double>::quiet_NaN();
 27 |       maxVal_ = std::numeric_limits<double>::quiet_NaN();
 28 |     }
 29 |     
 30 |     //////////////////////////////////////////////////////////////////////////////
 31 |     template <typename Scalar>
 32 |     void Colormap::setRangeLimitsFromData (const Eigen::Matrix<Scalar, 1, Eigen::Dynamic> &data)
 33 |     {
 34 |       if (data.size() > 0)
 35 |       {
 36 |         maxVal_ = static_cast<double>(data.maxCoeff());
 37 |         minVal_ = static_cast<double>(data.minCoeff());
 38 |       }
 39 |       else
 40 |       {
 41 |         maxVal_ = 0;
 42 |         minVal_ = 0;
 43 |       }      
 44 |     }
 45 |     
 46 |     //////////////////////////////////////////////////////////////////////////////
 47 |     template <typename Scalar>
 48 |     void Colormap::setRangeLimitsFromData (const std::vector<Scalar> &data)
 49 |     {
 50 |       if (data.size() > 0)
 51 |       {
 52 |         maxVal_ = static_cast<double>(*std::max_element(data.begin(), data.end()));
 53 |         minVal_ = static_cast<double>(*std::min_element(data.begin(), data.end()));
 54 |       }
 55 |       else
 56 |       {
 57 |         maxVal_ = 0;
 58 |         minVal_ = 0;
 59 |       }
 60 |     }
 61 | 
 62 |     //////////////////////////////////////////////////////////////////////////////
 63 |     vtkSmartPointer<vtkLookupTable> Colormap::getColorLookupTable() const
 64 |     { 
 65 |       vtkSmartPointer<vtkLookupTable> colorLookupTable;
 66 |       
 67 |       // Generate lookuptable
 68 |       switch (colormapType_)
 69 |       {
 70 |         case VTK_DEFAULT:
 71 |           colorLookupTable = getLUT_VtkDefault();
 72 |           break;
 73 | 
 74 |         case GRAYSCALE:
 75 |           colorLookupTable = getLUT_Grayscale();
 76 |           break;
 77 |           
 78 |         case JET:
 79 |           colorLookupTable = getLUT_Jet();
 80 |           break;
 81 |           
 82 |         default:
 83 |           std::cout << "[utl::getColormap] Unknown colormap\n";
 84 |           return NULL;
 85 |           
 86 |       }
 87 |     
 88 |       // Set range
 89 |       colorLookupTable->SetTableRange(minVal_, maxVal_);
 90 |       
 91 |       return colorLookupTable;
 92 |     }
 93 |       
 94 |     //////////////////////////////////////////////////////////////////////////////
 95 |     template <typename Scalar>
 96 |     Colors Colormap::getColorsFromData(const Eigen::Matrix<Scalar, 1, Eigen::Dynamic> &data)
 97 |     {
 98 |       // Set range limits if they were not set before
 99 |       if (std::isnan(minVal_) || std::isnan(maxVal_))
100 |         setRangeLimitsFromData<Scalar>(data);
101 |       
102 |       // Get lookup table
103 |       vtkSmartPointer<vtkLookupTable> colorLUT = getColorLookupTable();
104 |       
105 |       // Get colors
106 |       Colors colors;
107 |       colors.resize(data.size());
108 |       for (size_t i = 0; i < data.size(); i++)
109 |       {
110 |         double rgb[3];
111 |         colorLUT->GetColor(data[i], rgb);
112 |         Color color;
113 |         color.r = rgb[0];
114 |         color.g = rgb[1];
115 |         color.b = rgb[2];
116 |         colors[i] = color;
117 |       }
118 |       
119 |       return colors;
120 |     }  
121 |     
122 |     //////////////////////////////////////////////////////////////////////////////
123 |     template <typename Scalar>
124 |     Colors Colormap::getColorsFromData(const std::vector<Scalar> &data)
125 |     {
126 |       // Set range limits if they were not set before
127 |       if (std::isnan(minVal_) || std::isnan(maxVal_))
128 |         setRangeLimitsFromData<Scalar>(data);
129 |       
130 |       // Get lookup table
131 |       vtkSmartPointer<vtkLookupTable> colorLUT = getColorLookupTable();
132 |       
133 |       // Get colors
134 |       Colors colors;
135 |       colors.resize(data.size());
136 |       for (size_t i = 0; i < data.size(); i++)
137 |       {
138 |         double rgb[3];
139 |         colorLUT->GetColor(data[i], rgb);
140 |         Color color;
141 |         color.r = rgb[0];
142 |         color.g = rgb[1];
143 |         color.b = rgb[2];
144 |         colors[i] = color;
145 |       }
146 |       
147 |       return colors;
148 |     }
149 | 
150 |     //////////////////////////////////////////////////////////////////////////////
151 |     vtkSmartPointer<vtkLookupTable> Colormap::getLUT_Jet () const
152 |     {
153 |       vtkSmartPointer<vtkLookupTable> colormap = vtkSmartPointer<vtkLookupTable>::New();    
154 |       colormap->SetNumberOfTableValues(COLORMAP_NUM_DIVISIONS);
155 | 
156 |       for (size_t i = 0; i < COLORMAP_NUM_DIVISIONS; i++)
157 |         colormap->SetTableValue(i,  JET_LUT[i][0], JET_LUT[i][1], JET_LUT[i][2]);
158 |       
159 |       return colormap;
160 |     }
161 |     
162 |     //////////////////////////////////////////////////////////////////////////////
163 |     vtkSmartPointer<vtkLookupTable> Colormap::getLUT_Grayscale () const
164 |     {
165 |       vtkSmartPointer<vtkLookupTable> colormap = vtkSmartPointer<vtkLookupTable>::New();
166 |       colormap->SetNumberOfTableValues(COLORMAP_NUM_DIVISIONS);
167 |       
168 |       for (size_t i = 0; i < COLORMAP_NUM_DIVISIONS; i++)
169 |       {
170 |         double value = 1.0/(COLORMAP_NUM_DIVISIONS-1) * i;
171 |         colormap->SetTableValue(i,  value, value, value);
172 |       }    
173 |       
174 |       return colormap;
175 |     }
176 | 
177 |     //////////////////////////////////////////////////////////////////////////////
178 |     vtkSmartPointer<vtkLookupTable> Colormap::getLUT_VtkDefault () const
179 |     {
180 |       vtkSmartPointer<vtkLookupTable> colormap = vtkSmartPointer<vtkLookupTable>::New();
181 |       colormap->Build();
182 | 
183 |       return colormap;
184 |     }
185 |   }
186 | }
187 | 
188 | #endif // VTK_COLORMAPS_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/pointcloud_segmentation_io.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef POINTCLOUD_SEGMENTATION_IO_HPP
  2 | #define POINTCLOUD_SEGMENTATION_IO_HPP
  3 | 
  4 | // STD includes
  5 | #include <fstream>
  6 | 
  7 | // My includes
  8 | #include <namaris/utilities/map.hpp>
  9 | 
 10 | namespace utl
 11 | {
 12 |   namespace cloud
 13 |   {
 14 |     bool writeSegmentation (const std::string &filename, const utl::map::Map &segmentation)
 15 |     {
 16 |       //------------------------------------------------------------------------
 17 |       // Check that segmentation is not empty
 18 |       //------------------------------------------------------------------------
 19 |       
 20 |       if (segmentation.size() == 0)
 21 |       {
 22 |         std::cout << "[utl::cloud::writeSegmentation] Segmentation is empty, nothing to save." << std::endl;
 23 | //         return false;
 24 |       }
 25 |       
 26 |       //------------------------------------------------------------------------
 27 |       // Open file for writing
 28 |       //------------------------------------------------------------------------
 29 |       
 30 |       std::ofstream file(filename);
 31 |       if (!file.is_open())
 32 |       {
 33 |         std::cout << "[utl::cloud::writeSegmentation] Could not open file for writing ('" << filename << "')." << std::endl;
 34 |         return false;
 35 |       }
 36 |       
 37 |       //------------------------------------------------------------------------
 38 |       // Write header
 39 |       //------------------------------------------------------------------------
 40 |       
 41 |       file << segmentation.size() << " segments" << std::endl << std::endl;
 42 |       
 43 |       //------------------------------------------------------------------------
 44 |       // Write segment information
 45 |       //------------------------------------------------------------------------
 46 |       
 47 |       for (size_t segId = 0; segId < segmentation.size(); segId++)
 48 |       {
 49 |         file << "--------------------" << std::endl;
 50 |         file << segId << std::endl;
 51 |         file << "--------------------" << std::endl;
 52 |         
 53 |         for (size_t pointIdIt = 0; pointIdIt < segmentation[segId].size(); pointIdIt++)
 54 |           file << segmentation[segId][pointIdIt] << std::endl;
 55 |         
 56 |         file << std::endl;
 57 |       }
 58 |       
 59 |       return true;
 60 |     }
 61 |     
 62 |     bool readSegmentation (const std::string &filename, utl::map::Map &segmentation)
 63 |     {
 64 |       //------------------------------------------------------------------------
 65 |       // Open file for reading
 66 |       //------------------------------------------------------------------------
 67 |       
 68 |       std::ifstream file(filename);
 69 |       if (!file.is_open())
 70 |       {
 71 |         std::cout << "[utl::cloud::readSegmentation] Could not open file for reading ('" << filename << "')." << std::endl;
 72 |         return false;
 73 |       }
 74 | 
 75 |       segmentation.resize(0);
 76 |       
 77 |       //------------------------------------------------------------------------
 78 |       // Read header
 79 |       //------------------------------------------------------------------------
 80 |       
 81 |       int numSegments;
 82 |       std::string line;
 83 |       file >> numSegments;
 84 |       
 85 |       if (numSegments < 0)
 86 |       {
 87 |         std::cout << "[utl::cloud::readSegmentation] Number of segments is smaller than 0, segmentation file is corrupted." << std::endl;
 88 |         std::cout << "[utl::cloud::readSegmentation] In file: " << filename << std::endl;
 89 |         return false;
 90 |       }
 91 |       else if (numSegments == 0)
 92 |       {
 93 |         std::cout << "[utl::cloud::readSegmentation] File contains 0 segments." << std::endl;
 94 |         std::cout << "[utl::cloud::readSegmentation] In file: " << filename << std::endl;
 95 |         return true;        
 96 |       }
 97 |       
 98 |       file >> line;
 99 |       
100 |       //------------------------------------------------------------------------
101 |       // Initialize segmentation file
102 |       //------------------------------------------------------------------------
103 |       
104 |       // Read segment point indices
105 |       int numSegmentsRead = 0;
106 |       bool done = false;
107 |       file >> line;
108 |       
109 |       while (!done)
110 |       {
111 |         
112 |         //----------------------------------------------------------------------
113 |         // Read segment header
114 |         
115 |         int segIdRead;
116 |         file >> segIdRead;
117 |         
118 |         if (segIdRead != numSegmentsRead)
119 |         {
120 |           std::cout << "[utl::cloud::readSegmentation] Unexpected segment id. Expected " << numSegmentsRead << ", got " << segIdRead << std::endl;
121 |           std::cout << "[utl::cloud::readSegmentation] Segmentation file is corrupted." << std::endl;
122 |           std::cout << "[utl::cloud::readSegmentation] In file: " << filename << std::endl;
123 |           return false;
124 |         }
125 |         
126 |         numSegmentsRead++;
127 |         segmentation.push_back(std::vector<int> ());
128 |         file >> line;
129 |         
130 |         //------------------------------------------------------------------------
131 |         // Read point indices
132 |       
133 |         while (!file.eof())
134 |         {
135 |           file >> line;
136 |           if (line == "--------------------" || line == "")
137 |             break;
138 |           else
139 |           {
140 |             int pointId = std::stoi(line);
141 |             segmentation[segIdRead].push_back(pointId);
142 |             line.clear();
143 |           }
144 |         }
145 |         
146 |         if (file.eof())
147 |           done = true;
148 |       }
149 |       
150 |       //------------------------------------------------------------------------      
151 |       // Check that number of segments read is the same as number of segments in the header
152 |       //------------------------------------------------------------------------
153 |       
154 |       if (numSegments != numSegmentsRead)
155 |       {
156 |         std::cout << "[utl::cloud::readSegmentation] Expected " << numSegments << " segments, was able to read " << numSegmentsRead << " segments." << std::endl;
157 |         std::cout << "[utl::cloud::readSegmentation] Segmentation file is corrupted." << std::endl;
158 |         std::cout << "[utl::cloud::readSegmentation] In file: " << filename << std::endl;
159 |       }
160 |       
161 |       return true;
162 |     }
163 |   }
164 | }
165 | 
166 | #endif  // POINTCLOUD_SEGMENTATION_IO_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/min_cut.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef MIN_CUT_HPP
  2 | #define MIN_CUT_HPP
  3 | 
  4 | // Includes
  5 | #include <boost/graph/adjacency_list.hpp>
  6 | #include <boost/graph/boykov_kolmogorov_max_flow.hpp>
  7 | 
  8 | // Typedefs
  9 | typedef boost::adjacency_list_traits  < boost::vecS, boost::vecS, boost::directedS > Traits;
 10 | typedef boost::adjacency_list         < boost::vecS,                                                                            // Container used for vertices
 11 |                                         boost::vecS,                                                                            // Container used for edges
 12 |                                         boost::directedS,                                                                       // Directional graph
 13 |                                         boost::property < boost::vertex_name_t, std::string,                                    // Vertex properties
 14 |                                         boost::property < boost::vertex_index_t, long,
 15 |                                         boost::property < boost::vertex_color_t, boost::default_color_type,
 16 |                                         boost::property < boost::vertex_distance_t, long,
 17 |                                         boost::property < boost::vertex_predecessor_t, Traits::edge_descriptor > > > > >,
 18 | 
 19 |                                         boost::property < boost::edge_capacity_t, float,                                       // Edge properties
 20 |                                         boost::property < boost::edge_residual_capacity_t, float,
 21 |                                         boost::property < boost::edge_reverse_t, Traits::edge_descriptor > > > > Graph;
 22 | 
 23 | typedef boost::property_map< Graph, boost::edge_capacity_t >::type CapacityMap;
 24 | typedef boost::property_map< Graph, boost::edge_reverse_t>::type ReverseEdgeMap;
 25 | typedef boost::property_map< Graph, boost::vertex_color_t, boost::default_color_type>::type VertexColorMap;
 26 | 
 27 | ////////////////////////////////////////////////////////////////////////////////
 28 | bool addEdge (Traits::vertex_descriptor &v1, Traits::vertex_descriptor &v2, Graph &graph, const float weight, CapacityMap &capacity_map, ReverseEdgeMap &reverse_edge_map)
 29 | {
 30 |   Traits::edge_descriptor edge, reverse_edge;
 31 |   bool edge_was_added, reverse_edge_was_added;
 32 | 
 33 |   boost::tie (edge, edge_was_added) = boost::add_edge ( v1, v2, graph );
 34 |   boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( v2, v1, graph );
 35 |   if ( !edge_was_added || !reverse_edge_was_added )
 36 |     return (false);
 37 |   
 38 |   capacity_map[edge] = weight;
 39 |   capacity_map[reverse_edge] = weight;
 40 |   reverse_edge_map[edge] = reverse_edge;
 41 |   reverse_edge_map[reverse_edge] = edge;
 42 |   
 43 |   return true;
 44 | }
 45 |                                     
 46 | namespace alg
 47 | {
 48 |   /** \brief Perform a min cut on a graph
 49 |    *  \param[in]  source_potentials   weights between nodes and source node
 50 |    *  \param[in]  sink_potentials     weights between nodes and sink node
 51 |    *  \param[in]  edges               link structure between nodes
 52 |    *  \param[in]  binary_potentials   node link potentials
 53 |    *  \param[out] source_points       points belonging to source
 54 |    *  \param[out] sink_points         points belonging to sink
 55 |    */
 56 |   double mincut ( const std::vector<float> &source_potentials, 
 57 |                 const std::vector<float> &sink_potentials, 
 58 |                 const std::vector<std::vector<int> > &edges,
 59 |                 const std::vector <std::vector<float> > &binary_potentials,
 60 |                 std::vector<int> &source_points,
 61 |                 std::vector<int> &sink_points
 62 |               )
 63 |   {
 64 |     //////////////////////////////////////////////////////////////////////////////
 65 |     // Check input
 66 |     if (! (   (source_potentials.size() == sink_potentials.size()) && 
 67 |               (source_potentials.size() == edges.size()) && 
 68 |               (source_potentials.size() == binary_potentials.size())))
 69 |     {
 70 |       std::cout << "[alg::minCut] number of input source potentials, sink potentials edges and binary potentials are not equal." << std::endl;
 71 |       abort();
 72 |     }
 73 |     
 74 |     //////////////////////////////////////////////////////////////////////////////
 75 |     // Build graph
 76 |     
 77 |     int numVertices = source_potentials.size();
 78 |     Graph graph;
 79 |     std::vector< Traits::vertex_descriptor > vertices;
 80 |     Traits::vertex_descriptor source;
 81 |     Traits::vertex_descriptor sink;
 82 |     CapacityMap capacity          = boost::get (boost::edge_capacity, graph);
 83 |     ReverseEdgeMap reverseEdgeMap = boost::get (boost::edge_reverse, graph);
 84 |     VertexColorMap vertexColorMap = boost::get (boost::vertex_color, graph);
 85 | 
 86 |       // Add vertices
 87 |     vertices.resize(numVertices + 2);
 88 |     for (size_t i = 0; i < static_cast<size_t>(numVertices + 2); i++)
 89 |       vertices[i] = boost::add_vertex(graph);
 90 |     
 91 |     source  = vertices[source_potentials.size()];
 92 |     sink    = vertices[source_potentials.size()+1];
 93 |     
 94 |     // Add source and sink edges
 95 |     for (size_t i = 0; i < static_cast<size_t>(numVertices); i++)
 96 |     {
 97 |       addEdge(vertices[i], source, graph, source_potentials[i], capacity, reverseEdgeMap);
 98 |       addEdge(vertices[i], sink,   graph, sink_potentials[i], capacity, reverseEdgeMap);
 99 |     }
100 |     
101 |     // Add binary edges
102 |     for (size_t i = 0; i < edges.size(); i++)
103 |     {
104 |       for (size_t j = 0; j < edges[i].size(); j++)
105 |       {
106 |         Traits::vertex_descriptor v1 = vertices[i];
107 |         Traits::vertex_descriptor v2 = vertices[edges[i][j]];
108 |         float weight = binary_potentials[i][j];
109 |         addEdge(v1, v2, graph, weight, capacity, reverseEdgeMap);
110 |       }
111 |     }
112 |     
113 |     //////////////////////////////////////////////////////////////////////////////  
114 |     // Compute maximim flow
115 |     
116 |     double flow = boost::boykov_kolmogorov_max_flow(graph, source, sink);
117 |     
118 |     //////////////////////////////////////////////////////////////////////////////  
119 |     // Find foreground and background points
120 |     
121 |     source_points.clear();
122 |     sink_points.clear();
123 |     
124 |     for (size_t i = 0; i < static_cast<size_t>(numVertices); i++)
125 |     {    
126 |       if (vertexColorMap(vertices[i]) == 0)
127 |         source_points.push_back(i);
128 |       else
129 |         sink_points.push_back(i);
130 |     }
131 |     
132 |     return flow;
133 |   }
134 | }
135 | 
136 | # endif // MIN_CUT_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/meanshift/meanshift.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef MEANSHIFT_HPP
  2 | #define MEANSHIFT_HPP
  3 | 
  4 | #include "meanshift.h"
  5 | 
  6 | /** \brief Custom comparator for sorting a vector of doubles and preserving
  7 |   * sort order. */
  8 | bool pairCompare(const std::pair<double, int>& firstPair, const std::pair<double, int>& secondPair)
  9 | {
 10 |   return firstPair.first > secondPair.first;
 11 | }
 12 | 
 13 | ////////////////////////////////////////////////////////////////////////////////
 14 | alg::Meanshift::Meanshift()
 15 |   : bandwidth_ (0.02)
 16 |   , kernelType_ (GAUSSIAN)
 17 |   , maxModeInlierDistance_ (0.02)
 18 |   , dimensionality_ (0)
 19 |   , numPoints_ (0)
 20 | { }
 21 | 
 22 | ////////////////////////////////////////////////////////////////////////////////
 23 | void alg::Meanshift::setBandwidth(float bandwidth)
 24 | {
 25 |   bandwidth_ = bandwidth;
 26 | }
 27 | 
 28 | ////////////////////////////////////////////////////////////////////////////////
 29 | float alg::Meanshift::getBandwidth() const
 30 | {
 31 |   return bandwidth_;
 32 | }
 33 | 
 34 | ////////////////////////////////////////////////////////////////////////////////
 35 | void alg::Meanshift::setKernelType(alg::Meanshift::KERNEL_TYPE kernelType)
 36 | {
 37 |   kernelType_ = kernelType;
 38 | }
 39 | 
 40 | ////////////////////////////////////////////////////////////////////////////////
 41 | float alg::Meanshift::getKernelType() const
 42 | {
 43 |   return kernelType_;
 44 | }
 45 | 
 46 | ////////////////////////////////////////////////////////////////////////////////
 47 | void alg::Meanshift::setMaxModeInlierDistance(float maxModeInlierDistance)
 48 | {
 49 |   maxModeInlierDistance_ = maxModeInlierDistance;
 50 | }
 51 | 
 52 | ////////////////////////////////////////////////////////////////////////////////
 53 | float alg::Meanshift::getMaxModeInlierDistance() const
 54 | {
 55 |   return maxModeInlierDistance_;
 56 | }
 57 |  
 58 | ////////////////////////////////////////////////////////////////////////////////
 59 | bool alg::Meanshift::setInputPoints(const Eigen::MatrixXf& points)
 60 | {
 61 |   dimensionality_ = points.rows();
 62 |   numPoints_      = points.cols();
 63 | 
 64 |   if (dimensionality_ == 0 || numPoints_ == 0)
 65 |   {
 66 |     std::cout << "[utl::Meanshift::cluster] input data was not set!" << std::endl;
 67 |     return false;
 68 |   }
 69 | 
 70 |   points_ = boost::shared_ptr<CVectorPointSet<float> > (new CVectorPointSet<float> (dimensionality_, numPoints_, points.data()));
 71 |   geom_ = boost::make_shared<CEuclideanGeometry<float> >(dimensionality_);                                      // Define the type of geometry used (with dimensionality)
 72 |   return true;
 73 | }
 74 |   
 75 | ////////////////////////////////////////////////////////////////////////////////
 76 | bool alg::Meanshift::cluster(Eigen::MatrixXf &unprunedModes)
 77 | {
 78 |   //----------------------------------------------------------------------------
 79 |   // Check that data was set
 80 |   if (dimensionality_ == 0 || numPoints_ == 0)
 81 |   {
 82 |     std::cout << "[utl::Meanshift::cluster] input data was not set!" << std::endl;
 83 |     return false;
 84 |   }
 85 |   
 86 |   //----------------------------------------------------------------------------
 87 |   // Setup meanshift object    
 88 |   if (kernelType_ == GAUSSIAN)
 89 |     ms_ = CMeanShift< float >::EGaussianKernel;
 90 |   else if (kernelType_ == EPANECHNIKOV)
 91 |     ms_ = CMeanShift< float >::EEpanechnikovKernel;
 92 |   else
 93 |   {
 94 |     std::cout << "[utl::Meanshift::cluster] unknown kernel type '" << kernelType_ << "'. This is a bug." << std::endl;
 95 |     return false;
 96 |   }
 97 |   
 98 |   ms_.setBandwidth(bandwidth_);
 99 | 
100 |   //----------------------------------------------------------------------------
101 |   // Prepare arrays
102 |   unprunedModes_ = boost::shared_ptr<CVectorPointSet<float> > (new CVectorPointSet<float> (dimensionality_, numPoints_));
103 |   
104 |   //----------------------------------------------------------------------------
105 |   // Cluster time!
106 |   ms_.doMeanShift(*geom_, *points_, *unprunedModes_, probs_);                     // Perform meanshift. Result is a list of peaks where particles have convererged to
107 |   
108 |   //----------------------------------------------------------------------------
109 |   // Convert to Eigen
110 |   unprunedModes = Eigen::MatrixXf (dimensionality_, numPoints_);
111 |   for (size_t pointId = 0; pointId < static_cast<size_t>(numPoints_); pointId++)
112 |     for (size_t dId = 0; dId < static_cast<size_t>(dimensionality_); dId++)
113 |       unprunedModes(dId,pointId) = *((*unprunedModes_)[pointId]+dId);
114 |   
115 |   return true;
116 | }
117 | 
118 | ////////////////////////////////////////////////////////////////////////////////
119 | bool alg::Meanshift::pruneModes(Eigen::MatrixXf &prunedModes, std::vector<std::vector<int> > &prunedModeSupport)
120 | {
121 |   //----------------------------------------------------------------------------
122 |   // Rearange modes in decreasing order of their probabilities
123 |   
124 |   // Get order
125 |   std::vector<std::pair<double, int> > modeOrder (numPoints_);
126 |   for (size_t pointId = 0; pointId < static_cast<size_t>(numPoints_); pointId++)
127 |     modeOrder[pointId] = std::pair<double, int> (probs_[pointId], pointId);
128 |   std::sort(modeOrder.begin(), modeOrder.end(), pairCompare);
129 |       
130 |   // Rearrange
131 |   CVectorPointSet<float> unprunedModes_sorted (dimensionality_, numPoints_);
132 |   for (size_t pointId = 0; pointId < static_cast<size_t>(numPoints_); pointId++)
133 |     unprunedModes_sorted.addPoint((*unprunedModes_)[modeOrder[pointId].second]);        
134 | 
135 |   //----------------------------------------------------------------------------
136 |   // Prune modes
137 |   prunedModes_ = boost::make_shared<CVectorPointSet<float> >(dimensionality_, numPoints_);
138 |   ms_.pruneModes(*geom_, unprunedModes_sorted, *prunedModes_, prunedModeSupport_, 1, maxModeInlierDistance_);
139 |           
140 |   //----------------------------------------------------------------------------
141 |   // Convert pruned modes to eigen
142 |   prunedModes = Eigen::MatrixXf (dimensionality_, prunedModes_->size());
143 |   for (size_t pointId = 0; pointId < static_cast<size_t>(prunedModes_->size()); pointId++)
144 |     for (size_t dId = 0; dId < static_cast<size_t>(dimensionality_); dId++)
145 |       prunedModes(dId,pointId) = *((*prunedModes_)[pointId]+dId);
146 |   
147 |   //----------------------------------------------------------------------------
148 |   // Rearrange pruned mode support
149 |   for (size_t modeId = 0; modeId < prunedModeSupport_.size(); modeId++)    
150 |     for (size_t pointId = 0; pointId < prunedModeSupport_[modeId].size(); pointId++)
151 |       prunedModeSupport_[modeId][pointId] = modeOrder[prunedModeSupport_[modeId][pointId]].second;
152 |   prunedModeSupport = prunedModeSupport_;
153 |   
154 |   return true;
155 | }
156 | 
157 | #endif    // MEANSHIFT_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/octomap_pcl_conversion.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef OCTOMAP_PCL_CONVERSION_UTILITIES_HPP
  2 | #define OCTOMAP_PCL_CONVERSION_UTILITIES_HPP
  3 | 
  4 | // Octomap
  5 | #include <octomap/octomap.h>
  6 | 
  7 | 
  8 | namespace utl
  9 | {
 10 |   namespace oct
 11 |   {
 12 |     /** \brief Extract the pointcloud contatining centers of occupied voxels in 
 13 |      * an occupancy map.
 14 |      * \param[in] occupancy_map occupancy map
 15 |      * \param[out] cloud  pointcloud
 16 |      * \param[in] bbox_min minimum coordinate of a bounding box in the scene
 17 |      * \param[in] bbox_max maximum coordinate of a bounding box in the scene
 18 |      * \param[in] depth octree depth at which occupancy is evaluated
 19 |      */ 
 20 |     template<typename PointT>
 21 |     void getOccupiedCloud ( const octomap::OcTree &occupancy_map,
 22 |                             pcl::PointCloud<PointT> &cloud,
 23 |                             const Eigen::Vector3f  &bbox_min = Eigen::Vector3f::Ones() * std::numeric_limits<float>::quiet_NaN(),
 24 |                             const Eigen::Vector3f  &bbox_max = Eigen::Vector3f::Ones() * std::numeric_limits<float>::quiet_NaN(),
 25 |                             const unsigned int depth = 0
 26 |                           )
 27 |     {
 28 |       // Prepare output cloud
 29 |       cloud.resize(0);
 30 |       
 31 |       // Get bounding box
 32 |       octomap::point3d bbox_min_oct;
 33 |       if (bbox_min == bbox_min)
 34 |       {
 35 |         bbox_min_oct.x() = bbox_min[0]; bbox_min_oct.y() = bbox_min[1]; bbox_min_oct.z() = bbox_min[2];
 36 |       }
 37 |       else
 38 |       {
 39 |         double v[3];
 40 |         occupancy_map.getMetricMin(v[0], v[1], v[2]);
 41 |         bbox_min_oct = octomap::point3d(v[0], v[1], v[2]);
 42 |       }
 43 |       
 44 |       octomap::point3d bbox_max_oct;
 45 |       if (bbox_max == bbox_max)
 46 |       {
 47 |         bbox_max_oct.x() = bbox_max[0]; bbox_max_oct.y() = bbox_max[1]; bbox_max_oct.z() = bbox_max[2];
 48 |       }
 49 |       else
 50 |       {
 51 |         double v[3];
 52 |         occupancy_map.getMetricMax(v[0], v[1], v[2]);
 53 |         bbox_max_oct = octomap::point3d(v[0], v[1], v[2]);        
 54 |       }
 55 | 
 56 |       // Extract occupied voxel centers and convert to PCL pointcloud
 57 |       for(auto leafIt = occupancy_map.begin_leafs_bbx(bbox_min_oct, bbox_max_oct, depth), leafEnd=occupancy_map.end_leafs_bbx(); leafIt!= leafEnd; leafIt++)
 58 |       {    
 59 |         // If it is not occupied - skip it
 60 |         if (occupancy_map.isNodeOccupied(*leafIt))
 61 |         {
 62 |           PointT curPoint;
 63 |           curPoint.x = leafIt.getX();
 64 |           curPoint.y = leafIt.getY();
 65 |           curPoint.z = leafIt.getZ();      
 66 |           cloud.push_back(curPoint);
 67 |         }
 68 |       }
 69 |     }
 70 |     
 71 |     /** \brief Extract the pointcloud contatining centers of occluded voxels in
 72 |      * an occupancy map.
 73 |      * \param[in] occupancy_map occupancy map
 74 |      * \param[out] cloud  pointcloud
 75 |      * \param[in] bbox_min minimum coordinate of a bounding box in the scene
 76 |      * \param[in] bbox_max maximum coordinate of a bounding box in the scene
 77 |      * \param[in] depth octree depth at which occupancy is evaluated
 78 |      */ 
 79 |     template<typename PointT>
 80 |     void getOccludedCloud ( const octomap::OcTree &occupancy_map,
 81 |                             pcl::PointCloud<PointT> &cloud,
 82 |                             const Eigen::Vector3f  &bbox_min = Eigen::Vector3f::Ones() * std::numeric_limits<float>::quiet_NaN(),
 83 |                             const Eigen::Vector3f  &bbox_max = Eigen::Vector3f::Ones() * std::numeric_limits<float>::quiet_NaN(),
 84 |                             const unsigned int depth = 0
 85 |                           )
 86 |     {
 87 |       // Prepare output cloud
 88 |       cloud.resize(0);
 89 |       
 90 |       // Get bounding box
 91 |       octomap::point3d bbox_min_oct;
 92 |       if (bbox_min == bbox_min)
 93 |       {
 94 |         bbox_min_oct.x() = bbox_min[0]; bbox_min_oct.y() = bbox_min[1]; bbox_min_oct.z() = bbox_min[2];
 95 |       }
 96 |       else
 97 |       {
 98 |         double v[3];
 99 |         occupancy_map.getMetricMin(v[0], v[1], v[2]);
100 |         bbox_min_oct = octomap::point3d(v[0], v[1], v[2]);
101 |       }
102 |       
103 |       octomap::point3d bbox_max_oct;
104 |       if (bbox_max == bbox_max)
105 |       {
106 |         bbox_max_oct.x() = bbox_max[0]; bbox_max_oct.y() = bbox_max[1]; bbox_max_oct.z() = bbox_max[2];
107 |       }
108 |       else
109 |       {
110 |         double v[3];
111 |         occupancy_map.getMetricMax(v[0], v[1], v[2]);
112 |         bbox_max_oct = octomap::point3d(v[0], v[1], v[2]);        
113 |       }
114 | 
115 |       // Find unknown leafs in the occupancy map
116 |       octomap::point3d_list occludedLeafs;
117 |       occupancy_map.getUnknownLeafCenters(occludedLeafs, bbox_min_oct, bbox_max_oct, depth);
118 |         
119 |       // Convert to PCL pointcloud
120 |       cloud.points.resize(occludedLeafs.size());
121 |       int pointId = 0;
122 |       for (auto leafIt = occludedLeafs.begin(); leafIt != occludedLeafs.end(); leafIt++, pointId++)
123 |       {
124 |         cloud.points[pointId].x = leafIt->x();
125 |         cloud.points[pointId].y = leafIt->y();
126 |         cloud.points[pointId].z = leafIt->z();
127 |       }
128 |     }
129 |     
130 |     /** \brief Extract the pointcloud contatining centers of voxels that are 
131 |      * marked as either occluded or occupied in the occupancy map.
132 |      * \param[in] occupancy_map occupancy map
133 |      * \param[out] cloud  pointcloud
134 |      * \param[in] bbox_min minimum coordinate of a bounding box in the scene
135 |      * \param[in] bbox_max maximum coordinate of a bounding box in the scene
136 |      * \param[in] depth octree depth at which occupancy is evaluated
137 |      */ 
138 |     template<typename PointT>
139 |     void getOccludedOccupiedCloud ( const octomap::OcTree &occupancy_map,
140 |                                     pcl::PointCloud<PointT> &cloud,
141 |                                     const Eigen::Vector3f  &bbox_min = Eigen::Vector3f::Ones() * std::numeric_limits<float>::quiet_NaN(),
142 |                                     const Eigen::Vector3f  &bbox_max = Eigen::Vector3f::Ones() * std::numeric_limits<float>::quiet_NaN(),
143 |                                     const unsigned int depth = 0
144 |                                   )
145 |     {
146 |       // Extract occluded and occupied clouds
147 |       pcl::PointCloud<PointT> occludedCloud;
148 |       pcl::PointCloud<PointT> occupiedCloud;
149 |       
150 |       getOccludedCloud<PointT>(occupancy_map, occludedCloud, bbox_min, bbox_max, depth);
151 |       getOccupiedCloud<PointT>(occupancy_map, occupiedCloud, bbox_min, bbox_max, depth);
152 |       
153 |       // Combine them into one cloud
154 |       cloud.resize(0);
155 |       cloud += occludedCloud;
156 |       cloud += occupiedCloud;
157 |     }
158 | 
159 |     /** \brief Convert Octomap cloud to a PCL cloud.
160 |      * \param[in] octo_cloud Octomap cloud
161 |      * \param[out] pcl_cloud  PCL cloud
162 |      */ 
163 |     template <typename PointT>
164 |     void octoCloud2pclCloud (const octomap::Pointcloud &octo_cloud, pcl::PointCloud<PointT> &pcl_cloud)
165 |     {
166 |       pcl_cloud.resize(octo_cloud.size());
167 |       pcl_cloud.is_dense = false;
168 |       
169 |       for (size_t i = 0; i < octo_cloud.size(); i++)
170 |       {
171 |         pcl_cloud.points[i].x = octo_cloud[i].x();
172 |         pcl_cloud.points[i].y = octo_cloud[i].y();
173 |         pcl_cloud.points[i].z = octo_cloud[i].z();
174 |       }
175 |     }
176 |   }
177 | }
178 |   
179 | #endif // OCTOMAP_PCL_CONVERSION_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/reconstruction.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef RECONSTRUCTION_UTILITIES_HPP
  2 | #define RECONSTRUCTION_UTILITIES_HPP
  3 | 
  4 | #include <calibu/cam/Rectify.h>
  5 | #include <calibu/utils/Range.h>
  6 | 
  7 | namespace utl 
  8 | {
  9 |   namespace rec
 10 |   {
 11 |     //////////////////////////////////////////////////////////////////////////////
 12 |     // Create lookup table for rectification
 13 |     void CreateLookupTableMod(
 14 |         const calibu::CameraModelInterface& cam_from,
 15 |         const Eigen::Matrix3d& R_onKinv,
 16 |         calibu::LookupTable& lut
 17 |         )
 18 |     {
 19 |       const int w = cam_from.Width();
 20 |       const int h = cam_from.Height();
 21 | 
 22 |       for( int r = 0; r < h; ++r) {
 23 |         for( int c = 0; c < w; ++c) {
 24 |           // Remap
 25 |           const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c,r,1);
 26 |           Eigen::Vector2d p_warped = cam_from.Project(p_o);
 27 | 
 28 |           bool outside = (p_warped[0] > w - 1.0 || p_warped[0] < 0.0 || p_warped[1] > h - 1.0 || p_warped[1] < 0.0);
 29 |           
 30 |           // Clamp to valid image coords. This will cause out of image
 31 |           // data to be stretched from nearest valid coords with
 32 |           // no branching in rectify function.
 33 |           p_warped[0] = std::min(std::max(0.0, p_warped[0]), w - 1.0 );
 34 |           p_warped[1] = std::min(std::max(0.0, p_warped[1]), h - 1.0 );
 35 | 
 36 |           // Truncates the values for the left image
 37 |           int u  = (int) p_warped[0];
 38 |           int v  = (int) p_warped[1];
 39 |           float su = p_warped[0] - (double)u;
 40 |           float sv = p_warped[1] - (double)v;
 41 | 
 42 |           // Fix pixel access for last row/column to ensure all are in bounds
 43 |           if(u == (w-1)) {
 44 |             u -= 1;
 45 |             su = 1.0;
 46 |           }
 47 |           if(v == (w-1)) {
 48 |             v -= 1;
 49 |             sv = 1.0;
 50 |           }
 51 | 
 52 |           if (outside)
 53 |           {
 54 |             calibu::BilinearLutPoint p;
 55 |             p.idx0 = u + v*w;
 56 |             p.idx1 = u + v*w + w;
 57 |             p.w00  = 0;
 58 |             p.w01  = 0;
 59 |             p.w10  = 0;
 60 |             p.w11  = 0;
 61 |             lut.SetPoint( r, c, p );          
 62 |           }
 63 |           else
 64 |           {
 65 |             // Pre-compute the bilinear interpolation weights
 66 |             calibu::BilinearLutPoint p;
 67 |             p.idx0 = u + v*w;
 68 |             p.idx1 = u + v*w + w;
 69 |             p.w00  = (1-su)*(1-sv);
 70 |             p.w01  =    su *(1-sv);
 71 |             p.w10  = (1-su)*sv;
 72 |             p.w11  =     su*sv;
 73 |             lut.SetPoint( r, c, p );
 74 |           }
 75 |         }
 76 |       }
 77 |     }
 78 |     
 79 |     //////////////////////////////////////////////////////////////////////////////
 80 |     // Rectify an OpenCV image
 81 |     template <typename T>
 82 |     void Rectify(
 83 |         const calibu::LookupTable& lut,
 84 |         const cv::Mat inputImage,
 85 |         cv::Mat outputImage
 86 |         )
 87 |     {
 88 |       // Check input data
 89 |       assert(inputImage.type() == outputImage.type());
 90 |       assert(inputImage.channels() == 1);
 91 | 
 92 |       
 93 |       // Make the most of the continuous block of memory!
 94 |       const calibu::BilinearLutPoint* ptr   = &lut.m_vLutPixels[0];
 95 | 
 96 |       const int nHeight = lut.Height();
 97 |       const int nWidth  = lut.Width();
 98 | 
 99 |       // Make sure we have been given a correct lookup table.
100 |       assert(inputImage.cols == nWidth && inputImage.rows == nHeight);
101 |       
102 |       const T* pInputImageData = reinterpret_cast<T*>(inputImage.data);
103 |       T* pOutputRectImageData = reinterpret_cast<T*>(outputImage.data);
104 |       
105 |       for( int nRow = 0; nRow < nHeight; nRow++ ) {
106 |         for( int nCol = 0; nCol < nWidth; nCol++ ) {
107 |           *pOutputRectImageData++ =
108 |             (T) ( ptr->w00 * pInputImageData[ ptr->idx0 ] +
109 |                 ptr->w01 * pInputImageData[ ptr->idx0 + 1 ] +
110 |                 ptr->w10 * pInputImageData[ ptr->idx1 ] +
111 |                 ptr->w11 * pInputImageData[ ptr->idx1 + 1 ] );
112 |           ptr++;
113 |         }
114 |       }
115 |     }
116 |     
117 |     //////////////////////////////////////////////////////////////////////////////
118 |     // Rectify an OpenCV image preserving depth discontinuities. If any of the 
119 |     // pixels in the original image used to contruct a pixel in the rectified
120 |     // image are occlusion pixels, that rectified pixel is marked as occlusion.
121 |     template <typename T>
122 |     void RectifyDepth(
123 |         const calibu::LookupTable& lut,
124 |         const cv::Mat inputImage,
125 |         const cv::Mat disc,
126 |         cv::Mat outputImage
127 |         )
128 |     {
129 |       // Check input data
130 |       assert(inputImage.type() == outputImage.type());
131 |       assert(inputImage.channels() == 1);
132 | 
133 |       
134 |       // Make the most of the continuous block of memory!
135 |       const calibu::BilinearLutPoint* ptr   = &lut.m_vLutPixels[0];
136 | 
137 |       const int nHeight = lut.Height();
138 |       const int nWidth  = lut.Width();
139 | 
140 |       // Make sure we have been given a correct lookup table.
141 |       assert(inputImage.cols == nWidth && inputImage.rows == nHeight);
142 |       
143 |       const T* pInputImageData      = reinterpret_cast<T*>(inputImage.data);
144 |       T* pOutputRectImageData       = reinterpret_cast<T*>(outputImage.data);
145 |       unsigned char* pDiscImageData = disc.data;
146 |       
147 |       for( int nRow = 0; nRow < nHeight; nRow++ ) {
148 |         for( int nCol = 0; nCol < nWidth; nCol++ ) {
149 |           if (pDiscImageData[ ptr->idx0 ] > 0 || pDiscImageData[ ptr->idx1 ] > 0)
150 |             *pOutputRectImageData++ = (T) (0);
151 |           else
152 |             *pOutputRectImageData++ =
153 |               (T) ( ptr->w00 * pInputImageData[ ptr->idx0 ] +
154 |                   ptr->w01 * pInputImageData[ ptr->idx0 + 1 ] +
155 |                   ptr->w10 * pInputImageData[ ptr->idx1 ] +
156 |                   ptr->w11 * pInputImageData[ ptr->idx1 + 1 ] );
157 |           ptr++;
158 |         }
159 |       }
160 |     }
161 | 
162 |     bool saveOpenNICalibParamsSupplementary ( const std::string filename,
163 |                                               const double depth_ir_dx,
164 |                                               const double depth_ir_dy,
165 |                                               const double depth_scaling
166 |                                             )
167 |     {
168 |       cv::FileStorage fs( filename, cv::FileStorage::WRITE );
169 | 
170 |       if (fs.isOpened())
171 |       {
172 |         fs << "depth_ir_dx" << depth_ir_dx;
173 |         fs << "depth_ir_dy" << depth_ir_dy;
174 |         fs << "depth_scaling" << depth_scaling;
175 |         
176 |         fs.release();
177 |         return true;
178 |       }
179 |       
180 |       fs.release();
181 |       return false;
182 |     }
183 | 
184 |     bool loadOpenNICalibParamsSupplementary ( const std::string filename,
185 |                                               double &depth_ir_dx,
186 |                                               double &depth_ir_dy,
187 |                                               double &depth_scaling
188 |                                             )
189 |     {
190 |       cv::FileStorage fs;
191 |       if (!fs.open(filename, cv::FileStorage::READ))
192 |       {
193 |         fs.release();
194 |         return false;
195 |       }
196 |       
197 |       fs["depth_ir_dx"] >> depth_ir_dx;
198 |       fs["depth_ir_dy"] >> depth_ir_dy;
199 |       fs["depth_scaling"] >> depth_scaling;
200 |       
201 |       fs.release();
202 |       return true;
203 |     }
204 |   }
205 | } 
206 | 
207 | #endif    // RECONSTRUCTION_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/region_growing/region_growing.h:
--------------------------------------------------------------------------------
  1 | #ifndef REGION_GROWING_H
  2 | #define REGION_GROWING_H
  3 | 
  4 | // PCL includes
  5 | #include <pcl/pcl_base.h>
  6 | #include <pcl/search/pcl_search.h>
  7 | 
  8 | // Utilities includes
  9 | #include <namaris/utilities/std_vector.hpp>
 10 | #include <namaris/utilities/math.hpp>
 11 | 
 12 | namespace alg
 13 | {
 14 |   typedef std::vector<pcl::PointIndices> IndicesClusters;
 15 |   typedef boost::shared_ptr<std::vector<pcl::PointIndices> > IndicesClustersPtr;
 16 | 
 17 |   /** \brief @b RegionGrowing Performs region growing segmentation on a
 18 |    * pointcloud using a user-defined condition function.
 19 |    * 
 20 |    * Given a point in the cloud it's neighboring points are found. For all of 
 21 |    * them a two conditions are verified:
 22 |    *  1) unary condition determining if a neighboring point is a valid candidate
 23 |    *  2) binary condition between the original point and a neighboring point 
 24 |    *  determining if they make a valid pair
 25 |    * 
 26 |    * If the fraction of neighbors satisfying the unary constraint is larger than
 27 |    * a threshold and the fraction of neighbors satisfying the binary constraint 
 28 |    * is larger than (a different) threshold neighboring points satisfying both 
 29 |    * conditions are added to the cluster. The same procedure is run on the newly
 30 |    * added points. Once this process converges it is run again on the remaining points in the 
 31 |    * cloud (i.e. points that were not yet assigned to any cluster).
 32 |    */
 33 |   template<typename PointT>
 34 |   class RegionGrowing : public pcl::PCLBase<PointT>
 35 |   {
 36 |     protected:
 37 |       typedef typename pcl::search::Search<PointT>::Ptr SearcherPtr;
 38 |       typedef boost::function<bool (const PointT&)>                       unaryFunction;
 39 |       typedef boost::function<bool (const PointT&, const PointT&, float)> binaryFunction;
 40 | 
 41 |       using pcl::PCLBase<PointT>::input_;
 42 |       using pcl::PCLBase<PointT>::indices_;
 43 |       using pcl::PCLBase<PointT>::fake_indices_;
 44 |       using pcl::PCLBase<PointT>::initCompute;
 45 |       using pcl::PCLBase<PointT>::deinitCompute;
 46 | 
 47 |     public:
 48 |       typedef pcl::PointCloud<PointT> PointCloud;
 49 |       typedef typename PointCloud::Ptr PointCloudPtr;
 50 |       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
 51 |       
 52 |     public:
 53 |       /** \brief Constructor */
 54 |       RegionGrowing ();
 55 |       
 56 |       /** \brief Constructor */
 57 |       ~RegionGrowing ();
 58 |       
 59 |       /** \brief Provide a pointer to the input cloud. */
 60 |       void
 61 |       setInputCloud (const PointCloudConstPtr &cloud);
 62 | 
 63 |       /** \brief Set condition function that needs to be satisfied for a point to belong to a cluster */
 64 |       inline void
 65 |       setUnaryConditionFunction (bool (*unary_condition_function) (const PointT&));
 66 | 
 67 |       /** \brief Set condition function that needs to be satisfied for a point to belong to a cluster */
 68 |       inline void
 69 |       setUnaryConditionFunction (unaryFunction unary_condition_function);
 70 |       
 71 |       /** \brief Set condition function that needs to be satisfied for two neighbouring points to be in the same cluster */
 72 |       inline void
 73 |       setBinaryConditionFunction (bool (*binary_condition_function) (const PointT&, const PointT&, float));      
 74 | 
 75 |       /** \brief Set condition function that needs to be satisfied for two neighbouring points to be in the same cluster */
 76 |       inline void
 77 |       setBinaryConditionFunction (binaryFunction binary_condition_function);
 78 |       
 79 |       /** \brief Set the search radius for finding point neighors. */
 80 |       inline void
 81 |       setSearchRadius (float search_radius);
 82 | 
 83 |       /** \brief Set the search radius for expanding the segments. */
 84 |       inline float
 85 |       getSearchRadius () const;
 86 | 
 87 |       /** \brief Set maximum number of neighbors for each point. */
 88 |       inline void
 89 |       setNumberOfNeighbors (int num_neighbors);
 90 | 
 91 |       /** \brief Set the search radius for expanding the segments. */
 92 |       inline int
 93 |       getNumberOfNeighbors () const;
 94 | 
 95 |       /** \brief Set minimum fraction of neighbors required to satisfy unary constraint to grow the cluster. */
 96 |       inline void
 97 |       setMinValidUnaryNeighborsFraction (float valid_unary_neighbor_fraction);
 98 | 
 99 |       /** \brief Get minimum fraction of neighbors required to satisfy unary constraint to grow the cluster. */
100 |       inline float
101 |       getMinValidUnaryNeighborsFraction () const;
102 |             
103 |       /** \brief Set fraction of neighbors required to satisfy the condition to grow the cluster. */
104 |       inline void
105 |       setMinValidBinaryNeighborsFraction (float valid_binary_neighbor_fraction);
106 | 
107 |       /** \brief Get minimum fraction of neighbors required to satisfy unary constraint to grow the cluster. */
108 |       inline float
109 |       getMinValidBinaryNeighborsFraction () const;
110 | 
111 |       /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. */
112 |       inline void
113 |       setMinSegmentSize (const int min_cluster_size);
114 |       
115 |       /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
116 |       inline int
117 |       getMinSegmentSize ()  const;
118 | 
119 |       /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. */
120 |       inline void
121 |       setMaxSegmentSize (const int max_cluster_size);      
122 | 
123 |       /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
124 |       inline int
125 |       getMaxSegmentSize () const;
126 |             
127 |       /** \brief Segment the pointcloud.
128 |        * \param[out] segments  a vector of segments where each segment is represented by the indices of points belonging to it
129 |        */
130 |       void
131 |       segment (std::vector<std::vector<int> > &clusters);
132 | 
133 |     protected:
134 |       
135 |       /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
136 |        * the current settings. If it is possible then it returns true.
137 |        */
138 |       virtual bool
139 |       prepareForSegmentation ();
140 |       
141 |     private:
142 |       /** \brief A pointer to the spatial search object */
143 |       SearcherPtr searcher_;
144 | 
145 |       /** \brief Search radius for finding point neighbors (default = -1.0) */
146 |       float search_radius_;
147 |       
148 |       /** \brief Number of nearest neighbors to analyzed (default = all) */
149 |       int num_neighbors_;
150 |             
151 |       /** \brief The condition function for a point that needs to be satisfied for a point to belong to a cluster */
152 |       unaryFunction unary_condition_function_;
153 |       
154 |       /** \brief The condition function between two points that needs to to be satisfied for two neighboring points to belong to the same cluster.  */
155 |       binaryFunction binary_condition_function_;
156 | 
157 |       /** \brief Minimum fraction of neighboring points that have to satisfy unary constraint to add neighbors to cluster. */
158 |       float min_valid_unary_neighbor_fraction_;
159 | 
160 |       /** \brief Minimum fraction of neighboring points that have to satisfy binary constraint to add neighbors to cluster. */
161 |       float min_valid_binary_neighbor_fraction_;
162 |       
163 |       /** \brief The minimum cluster size (default = 1) */
164 |       int min_cluster_size_;
165 | 
166 |       /** \brief The maximum cluster size (default = unlimited) */
167 |       int max_cluster_size_;
168 | 
169 |     public:
170 |       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
171 |   };
172 | }
173 | 
174 | #endif  // REGION_GROWING_H


--------------------------------------------------------------------------------
/include/namaris/algorithms/min_cut_plane_segmentation.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef MIN_CUT_PLANE_SEGMENTATION_HPP
  2 | #define MIN_CUT_PLANE_SEGMENTATION_HPP
  3 | 
  4 | // PCL includes
  5 | #include <pcl/common/angles.h>
  6 | 
  7 | // Utilities
  8 | #include <namaris/utilities/geometry.hpp>
  9 | #include <namaris/utilities/pointcloud.hpp>
 10 | 
 11 | // Algorithms
 12 | #include <namaris/algorithms/min_cut.hpp>
 13 | 
 14 | namespace alg
 15 | {
 16 |   /** \brief Segment out points that belong to a planar model using min cut segmentation
 17 |     * \param[in] cloud input cloud
 18 |     * \param[in] plane_normal plane normal
 19 |     * \param[in] plane_point plane point
 20 |     * \param[out] plane_points segmented plane points
 21 |     */
 22 |   template <typename PointT>
 23 |   bool minCutPlaneSegmentation  ( const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
 24 |                                   const Eigen::VectorXf plane_coefficients,
 25 |                                   std::vector<int> &plane_points
 26 |                                 )
 27 |   {
 28 |     //----------------------------------------------------------------------------
 29 |     // Check input and convert plane parameters
 30 |     //----------------------------------------------------------------------------
 31 |     
 32 |     if (plane_coefficients.size() != 4)
 33 |     {
 34 |       std::cout << "[alg::minCutPlaneSegmentation] plane coefficients must be length 4, instead they are length " << plane_coefficients.size() << "." << std::endl;
 35 |       return false;
 36 |     }
 37 |     
 38 |     Eigen::Vector3f planeNormal (plane_coefficients[0], plane_coefficients[1], plane_coefficients[2]);
 39 |     Eigen::Vector3f planePoint  ( - plane_coefficients[3] / plane_coefficients[0], 0.0f, 0.0f);
 40 |     
 41 |     planePoint = utl::geom::projectPointToPlane<float>(cloud->at(0).getVector3fMap(), planePoint, planeNormal);
 42 |     
 43 |     //----------------------------------------------------------------------------
 44 |     // Calculate unary potentials
 45 |     //----------------------------------------------------------------------------
 46 |     
 47 |     Eigen::Vector2f fgNormalAngleThresh (pcl::deg2rad(0.0),   pcl::deg2rad(10.0));
 48 |     Eigen::Vector2f bgNormalAngleThresh (pcl::deg2rad(30.0),  pcl::deg2rad(45.0));
 49 |     
 50 |     Eigen::Vector2f fgDistanceThresh (0.00f, 0.03f);
 51 |     Eigen::Vector2f bgDistanceThresh (0.05f, 0.2f);
 52 | 
 53 |     
 54 |     
 55 |     std::vector<float> fgWeights (cloud->size(), 0);
 56 |     std::vector<float> bgWeights (cloud->size(), 0);
 57 |     
 58 |     for (size_t pointId = 0; pointId < cloud->size(); pointId++)
 59 |     {
 60 |       // Get normal weight
 61 |       float curFgWeightNormal = 0;
 62 |       float curBgWeightNormal = 0;
 63 |       
 64 |       float dotProd = utl::math::clampValue(cloud->at(pointId).getNormalVector3fMap().dot(planeNormal), -1.0f, 1.0f);
 65 |       float normalAngleDiff = std::acos(dotProd);
 66 |       curFgWeightNormal = 1 - (utl::math::clampValue(normalAngleDiff, fgNormalAngleThresh[0], fgNormalAngleThresh[1]) - fgNormalAngleThresh[0]) / (fgNormalAngleThresh[1] - fgNormalAngleThresh[0]);
 67 |       curBgWeightNormal =     (utl::math::clampValue(normalAngleDiff, bgNormalAngleThresh[0], bgNormalAngleThresh[1]) - bgNormalAngleThresh[0]) / (bgNormalAngleThresh[1] - bgNormalAngleThresh[0]);
 68 |       
 69 |       // Get distance weight
 70 |       float curFgWeightDistance = 0;
 71 |       float curBgWeightDistance = 0;
 72 |       
 73 |       float distance = std::abs(utl::geom::pointToPlaneDistance<float>(cloud->at(pointId).getVector3fMap(), planePoint, planeNormal));
 74 |       curFgWeightDistance = 1 - (utl::math::clampValue(distance, fgDistanceThresh[0], fgDistanceThresh[1]) - fgDistanceThresh[0]) / (fgDistanceThresh[1] - fgDistanceThresh[0]);
 75 |       curBgWeightDistance =     (utl::math::clampValue(distance, bgDistanceThresh[0], bgDistanceThresh[1]) - bgDistanceThresh[0]) / (bgDistanceThresh[1] - bgDistanceThresh[0]);
 76 |       
 77 | 
 78 |       // Combine weights
 79 |       fgWeights[pointId] = curFgWeightNormal * curFgWeightDistance;
 80 |       bgWeights[pointId] = std::max(curBgWeightNormal, curBgWeightDistance);
 81 |       
 82 | //       if ((fgWeights[pointId] < 0) || (bgWeights[pointId] < 0) || std::isnan(fgWeights[pointId]) || std::isnan(bgWeights[pointId]))
 83 | //       {
 84 | //         cout << pointId << ", " << fgWeights[pointId] << ", " << bgWeights[pointId] << ", " << curFgWeightNormal << ", " << curBgWeightNormal << ", " << normalAngleDiff << ", " << distance << endl;
 85 | //         
 86 | //       }
 87 |     }
 88 |     
 89 |     //----------------------------------------------------------------------------
 90 |     // Find connectivity of the graph
 91 |     //----------------------------------------------------------------------------
 92 | 
 93 |     int numNeighbours = 6;
 94 |     
 95 |     pcl::search::KdTree<PointT> searchTree;
 96 |     searchTree.setInputCloud(cloud);  
 97 |     utl::graph::Graph adjacency (cloud->size());
 98 |     
 99 |     for (size_t point1Id = 0; point1Id < cloud->size(); point1Id++)
100 |     {
101 |       // Find nearest neighbour
102 |       std::vector<int> neighbours (numNeighbours);
103 |       std::vector<float> distance (numNeighbours);    
104 |       searchTree.nearestKSearch(point1Id, numNeighbours, neighbours, distance);
105 |       
106 |       for (size_t nbrIdIt = 1; nbrIdIt < neighbours.size(); nbrIdIt++)
107 |         utl::graph::addEdge(point1Id, neighbours[nbrIdIt], adjacency);
108 |     }
109 |       
110 |     //----------------------------------------------------------------------------
111 |     // Calcualte binary weights
112 |     //----------------------------------------------------------------------------
113 | 
114 |     utl::graph::GraphWeights binaryWeights = utl::graph::createGraphWeights(adjacency);
115 |     
116 |     for (size_t point1Id = 0; point1Id < cloud->size(); point1Id++)
117 |     {
118 |       Eigen::Vector3f point1Normal = cloud->at(point1Id).getNormalVector3fMap();
119 |       
120 |       for (size_t point2It = 0; point2It < adjacency[point1Id].size(); point2It++)
121 |       {
122 |         int point2Id = adjacency[point1Id][point2It];
123 |         Eigen::Vector3f point2Normal = cloud->at(point2Id).getNormalVector3fMap();
124 |         
125 |         float weight = (1.0 + point1Normal.dot(point2Normal)) / 2;
126 |         utl::graph::setEdgeWeight(adjacency, binaryWeights, point1Id, point2Id, weight);
127 |       }
128 |     }
129 |     
130 |     //----------------------------------------------------------------------------
131 |     // Segment
132 |     //----------------------------------------------------------------------------
133 |     
134 |     std::vector<int> fgPoints, bgPoints;
135 |     alg::mincut(fgWeights, bgWeights, adjacency, binaryWeights, bgPoints, fgPoints);
136 |     plane_points = fgPoints;
137 |     
138 | //     //----------------------------------------------------------------------------
139 | //     // Debug visualization
140 | //     //----------------------------------------------------------------------------
141 | //     
142 | //     PclVis visualizer = pcl::createVisualizer();
143 | //     visualizer.setCameraPosition (  0.0, 0.0, 0.0,   // camera position
144 | //                                     0.0, 0.0, 1.0,   // viewpoint
145 | //                                     0.0, -1.0, 0.0,   // normal
146 | //                                     0.0);            // viewport  
147 | //   
148 | //     utl::Colormap colormap;
149 | //     pcl::showPointCloudWithData<PointT,float> (visualizer, cloud, fgWeights, colormap);
150 | //     visualizer.addText("FG weights", 0, 50, 12, 1.0, 1.0, 1.0);
151 | //     visualizer.spin();  
152 | //   
153 | //     visualizer.removeAllPointClouds();
154 | //     visualizer.removeAllShapes();
155 | //     pcl::showPointCloudWithData<PointT,float> (visualizer, cloud, bgWeights, colormap);
156 | //     visualizer.addText("BG weights", 0, 50, 12, 1.0, 1.0, 1.0);
157 | //     visualizer.spin();  
158 | //     
159 | //     visualizer.removeAllPointClouds();
160 | //     pcl::showFGsegmentation(visualizer, *cloud, plane_points);
161 | //     visualizer.spin();  
162 |     
163 |     return true;
164 |   }
165 | }
166 | 
167 | #endif    // MIN_CUT_PLANE_SEGMENTATION_HPP


--------------------------------------------------------------------------------
/include/namaris/utilities/pcl_visualization/vtk_colormaps_lut.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef VTK_COLORMAPS_LUT_HPP
  2 | #define VTK_COLORMAPS_LUT_HPP
  3 | 
  4 | namespace utl
  5 | {
  6 |   const double JET_LUT[256][3] =
  7 |   {
  8 |     {0,0,5.156250e-01},
  9 |     {0,0,5.312500e-01},
 10 |     {0,0,5.468750e-01},
 11 |     {0,0,5.625000e-01},
 12 |     {0,0,5.781250e-01},
 13 |     {0,0,5.937500e-01},
 14 |     {0,0,6.093750e-01},
 15 |     {0,0,6.250000e-01},
 16 |     {0,0,6.406250e-01},
 17 |     {0,0,6.562500e-01},
 18 |     {0,0,6.718750e-01},
 19 |     {0,0,6.875000e-01},
 20 |     {0,0,7.031250e-01},
 21 |     {0,0,7.187500e-01},
 22 |     {0,0,7.343750e-01},
 23 |     {0,0,7.500000e-01},
 24 |     {0,0,7.656250e-01},
 25 |     {0,0,7.812500e-01},
 26 |     {0,0,7.968750e-01},
 27 |     {0,0,8.125000e-01},
 28 |     {0,0,8.281250e-01},
 29 |     {0,0,8.437500e-01},
 30 |     {0,0,8.593750e-01},
 31 |     {0,0,8.750000e-01},
 32 |     {0,0,8.906250e-01},
 33 |     {0,0,9.062500e-01},
 34 |     {0,0,9.218750e-01},
 35 |     {0,0,9.375000e-01},
 36 |     {0,0,9.531250e-01},
 37 |     {0,0,9.687500e-01},
 38 |     {0,0,9.843750e-01},
 39 |     {0,0,1},
 40 |     {0,1.562500e-02,1},
 41 |     {0,3.125000e-02,1},
 42 |     {0,4.687500e-02,1},
 43 |     {0,6.250000e-02,1},
 44 |     {0,7.812500e-02,1},
 45 |     {0,9.375000e-02,1},
 46 |     {0,1.093750e-01,1},
 47 |     {0,1.250000e-01,1},
 48 |     {0,1.406250e-01,1},
 49 |     {0,1.562500e-01,1},
 50 |     {0,1.718750e-01,1},
 51 |     {0,1.875000e-01,1},
 52 |     {0,2.031250e-01,1},
 53 |     {0,2.187500e-01,1},
 54 |     {0,2.343750e-01,1},
 55 |     {0,2.500000e-01,1},
 56 |     {0,2.656250e-01,1},
 57 |     {0,2.812500e-01,1},
 58 |     {0,2.968750e-01,1},
 59 |     {0,3.125000e-01,1},
 60 |     {0,3.281250e-01,1},
 61 |     {0,3.437500e-01,1},
 62 |     {0,3.593750e-01,1},
 63 |     {0,3.750000e-01,1},
 64 |     {0,3.906250e-01,1},
 65 |     {0,4.062500e-01,1},
 66 |     {0,4.218750e-01,1},
 67 |     {0,4.375000e-01,1},
 68 |     {0,4.531250e-01,1},
 69 |     {0,4.687500e-01,1},
 70 |     {0,4.843750e-01,1},
 71 |     {0,5.000000e-01,1},
 72 |     {0,5.156250e-01,1},
 73 |     {0,5.312500e-01,1},
 74 |     {0,5.468750e-01,1},
 75 |     {0,5.625000e-01,1},
 76 |     {0,5.781250e-01,1},
 77 |     {0,5.937500e-01,1},
 78 |     {0,6.093750e-01,1},
 79 |     {0,6.250000e-01,1},
 80 |     {0,6.406250e-01,1},
 81 |     {0,6.562500e-01,1},
 82 |     {0,6.718750e-01,1},
 83 |     {0,6.875000e-01,1},
 84 |     {0,7.031250e-01,1},
 85 |     {0,7.187500e-01,1},
 86 |     {0,7.343750e-01,1},
 87 |     {0,7.500000e-01,1},
 88 |     {0,7.656250e-01,1},
 89 |     {0,7.812500e-01,1},
 90 |     {0,7.968750e-01,1},
 91 |     {0,8.125000e-01,1},
 92 |     {0,8.281250e-01,1},
 93 |     {0,8.437500e-01,1},
 94 |     {0,8.593750e-01,1},
 95 |     {0,8.750000e-01,1},
 96 |     {0,8.906250e-01,1},
 97 |     {0,9.062500e-01,1},
 98 |     {0,9.218750e-01,1},
 99 |     {0,9.375000e-01,1},
100 |     {0,9.531250e-01,1},
101 |     {0,9.687500e-01,1},
102 |     {0,9.843750e-01,1},
103 |     {0,1,1},
104 |     {1.562500e-02,1,9.843750e-01},
105 |     {3.125000e-02,1,9.687500e-01},
106 |     {4.687500e-02,1,9.531250e-01},
107 |     {6.250000e-02,1,9.375000e-01},
108 |     {7.812500e-02,1,9.218750e-01},
109 |     {9.375000e-02,1,9.062500e-01},
110 |     {1.093750e-01,1,8.906250e-01},
111 |     {1.250000e-01,1,8.750000e-01},
112 |     {1.406250e-01,1,8.593750e-01},
113 |     {1.562500e-01,1,8.437500e-01},
114 |     {1.718750e-01,1,8.281250e-01},
115 |     {1.875000e-01,1,8.125000e-01},
116 |     {2.031250e-01,1,7.968750e-01},
117 |     {2.187500e-01,1,7.812500e-01},
118 |     {2.343750e-01,1,7.656250e-01},
119 |     {2.500000e-01,1,7.500000e-01},
120 |     {2.656250e-01,1,7.343750e-01},
121 |     {2.812500e-01,1,7.187500e-01},
122 |     {2.968750e-01,1,7.031250e-01},
123 |     {3.125000e-01,1,6.875000e-01},
124 |     {3.281250e-01,1,6.718750e-01},
125 |     {3.437500e-01,1,6.562500e-01},
126 |     {3.593750e-01,1,6.406250e-01},
127 |     {3.750000e-01,1,6.250000e-01},
128 |     {3.906250e-01,1,6.093750e-01},
129 |     {4.062500e-01,1,5.937500e-01},
130 |     {4.218750e-01,1,5.781250e-01},
131 |     {4.375000e-01,1,5.625000e-01},
132 |     {4.531250e-01,1,5.468750e-01},
133 |     {4.687500e-01,1,5.312500e-01},
134 |     {4.843750e-01,1,5.156250e-01},
135 |     {5.000000e-01,1,5.000000e-01},
136 |     {5.156250e-01,1,4.843750e-01},
137 |     {5.312500e-01,1,4.687500e-01},
138 |     {5.468750e-01,1,4.531250e-01},
139 |     {5.625000e-01,1,4.375000e-01},
140 |     {5.781250e-01,1,4.218750e-01},
141 |     {5.937500e-01,1,4.062500e-01},
142 |     {6.093750e-01,1,3.906250e-01},
143 |     {6.250000e-01,1,3.750000e-01},
144 |     {6.406250e-01,1,3.593750e-01},
145 |     {6.562500e-01,1,3.437500e-01},
146 |     {6.718750e-01,1,3.281250e-01},
147 |     {6.875000e-01,1,3.125000e-01},
148 |     {7.031250e-01,1,2.968750e-01},
149 |     {7.187500e-01,1,2.812500e-01},
150 |     {7.343750e-01,1,2.656250e-01},
151 |     {7.500000e-01,1,2.500000e-01},
152 |     {7.656250e-01,1,2.343750e-01},
153 |     {7.812500e-01,1,2.187500e-01},
154 |     {7.968750e-01,1,2.031250e-01},
155 |     {8.125000e-01,1,1.875000e-01},
156 |     {8.281250e-01,1,1.718750e-01},
157 |     {8.437500e-01,1,1.562500e-01},
158 |     {8.593750e-01,1,1.406250e-01},
159 |     {8.750000e-01,1,1.250000e-01},
160 |     {8.906250e-01,1,1.093750e-01},
161 |     {9.062500e-01,1,9.375000e-02},
162 |     {9.218750e-01,1,7.812500e-02},
163 |     {9.375000e-01,1,6.250000e-02},
164 |     {9.531250e-01,1,4.687500e-02},
165 |     {9.687500e-01,1,3.125000e-02},
166 |     {9.843750e-01,1,1.562500e-02},
167 |     {1,1,0},
168 |     {1,9.843750e-01,0},
169 |     {1,9.687500e-01,0},
170 |     {1,9.531250e-01,0},
171 |     {1,9.375000e-01,0},
172 |     {1,9.218750e-01,0},
173 |     {1,9.062500e-01,0},
174 |     {1,8.906250e-01,0},
175 |     {1,8.750000e-01,0},
176 |     {1,8.593750e-01,0},
177 |     {1,8.437500e-01,0},
178 |     {1,8.281250e-01,0},
179 |     {1,8.125000e-01,0},
180 |     {1,7.968750e-01,0},
181 |     {1,7.812500e-01,0},
182 |     {1,7.656250e-01,0},
183 |     {1,7.500000e-01,0},
184 |     {1,7.343750e-01,0},
185 |     {1,7.187500e-01,0},
186 |     {1,7.031250e-01,0},
187 |     {1,6.875000e-01,0},
188 |     {1,6.718750e-01,0},
189 |     {1,6.562500e-01,0},
190 |     {1,6.406250e-01,0},
191 |     {1,6.250000e-01,0},
192 |     {1,6.093750e-01,0},
193 |     {1,5.937500e-01,0},
194 |     {1,5.781250e-01,0},
195 |     {1,5.625000e-01,0},
196 |     {1,5.468750e-01,0},
197 |     {1,5.312500e-01,0},
198 |     {1,5.156250e-01,0},
199 |     {1,5.000000e-01,0},
200 |     {1,4.843750e-01,0},
201 |     {1,4.687500e-01,0},
202 |     {1,4.531250e-01,0},
203 |     {1,4.375000e-01,0},
204 |     {1,4.218750e-01,0},
205 |     {1,4.062500e-01,0},
206 |     {1,3.906250e-01,0},
207 |     {1,3.750000e-01,0},
208 |     {1,3.593750e-01,0},
209 |     {1,3.437500e-01,0},
210 |     {1,3.281250e-01,0},
211 |     {1,3.125000e-01,0},
212 |     {1,2.968750e-01,0},
213 |     {1,2.812500e-01,0},
214 |     {1,2.656250e-01,0},
215 |     {1,2.500000e-01,0},
216 |     {1,2.343750e-01,0},
217 |     {1,2.187500e-01,0},
218 |     {1,2.031250e-01,0},
219 |     {1,1.875000e-01,0},
220 |     {1,1.718750e-01,0},
221 |     {1,1.562500e-01,0},
222 |     {1,1.406250e-01,0},
223 |     {1,1.250000e-01,0},
224 |     {1,1.093750e-01,0},
225 |     {1,9.375000e-02,0},
226 |     {1,7.812500e-02,0},
227 |     {1,6.250000e-02,0},
228 |     {1,4.687500e-02,0},
229 |     {1,3.125000e-02,0},
230 |     {1,1.562500e-02,0},
231 |     {1,0,0},
232 |     {9.843750e-01,0,0},
233 |     {9.687500e-01,0,0},
234 |     {9.531250e-01,0,0},
235 |     {9.375000e-01,0,0},
236 |     {9.218750e-01,0,0},
237 |     {9.062500e-01,0,0},
238 |     {8.906250e-01,0,0},
239 |     {8.750000e-01,0,0},
240 |     {8.593750e-01,0,0},
241 |     {8.437500e-01,0,0},
242 |     {8.281250e-01,0,0},
243 |     {8.125000e-01,0,0},
244 |     {7.968750e-01,0,0},
245 |     {7.812500e-01,0,0},
246 |     {7.656250e-01,0,0},
247 |     {7.500000e-01,0,0},
248 |     {7.343750e-01,0,0},
249 |     {7.187500e-01,0,0},
250 |     {7.031250e-01,0,0},
251 |     {6.875000e-01,0,0},
252 |     {6.718750e-01,0,0},
253 |     {6.562500e-01,0,0},
254 |     {6.406250e-01,0,0},
255 |     {6.250000e-01,0,0},
256 |     {6.093750e-01,0,0},
257 |     {5.937500e-01,0,0},
258 |     {5.781250e-01,0,0},
259 |     {5.625000e-01,0,0},
260 |     {5.468750e-01,0,0},
261 |     {5.312500e-01,0,0},
262 |     {5.156250e-01,0,0},
263 |     {5.000000e-01,0,0}
264 |   };
265 | }
266 | #endif  // VTK_COLORMAPS_LUT_HPP


--------------------------------------------------------------------------------
/include/namaris/3rd_party/meanshift/EuclideanGeometry.h:
--------------------------------------------------------------------------------
  1 | /****************************************************************
  2 | 
  3 |                  Euclidean Geometry Library
  4 | 	=============================================
  5 | 
  6 | Implementation of manifold operations for Euclidean Space
  7 | 
  8 | Implemented by Raghav Subbarao
  9 | ****************************************************************/
 10 | 
 11 | #ifndef _C_EUCLIDEAN_GEOMETRY_H_
 12 | #define _C_EUCLIDEAN_GEOMETRY_H_
 13 | 
 14 | //include needed libraries
 15 | #include <math.h>
 16 | #include <assert.h>
 17 | #include <memory.h>
 18 | 
 19 | #include "Geometry.h"
 20 | 
 21 | template<typename T> class CEuclideanGeometry : public CGeometry<T>{
 22 | 
 23 | public:
 24 | 
 25 | 	/*/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\*/
 26 | 	/* Class Constructor and Destructor */
 27 | 	/*\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/*/
 28 | 
 29 | 	// Default Constructor
 30 | 	// scale = dimension of Euclidean space.
 31 | 	CEuclideanGeometry(int d);
 32 | 
 33 | 	// Virtual Destructor
 34 | 	virtual ~CEuclideanGeometry(){};
 35 | 
 36 | 	/*\\||//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||\\--/
 37 | 	//<---------------------------------------------------->|//
 38 | 	//|														|//
 39 | 	//|	Method Name:	g									|//
 40 | 	//|   ============										|//
 41 | 	//|														|//
 42 | 	//<---------------------------------------------------->|//
 43 | 	//|														|//
 44 | 	//|	Description:										|//
 45 | 	//|	============										|//
 46 | 	//|														|//
 47 | 	//|   Computes the dot product of tangents.				|//
 48 | 	//|   It always	returns a double value.					|//
 49 | 	//|														|//
 50 | 	//|   The following arguments must be provided:			|//
 51 | 	//|														|//
 52 | 	//|   <const T* gamma>									|//
 53 | 	//|   An array of type T containing the tangent.		|//
 54 | 	//|														|//
 55 | 	//|   <const T* delta>									|//
 56 | 	//|   An array of type T containing the tangent.		|//
 57 | 	//|														|//
 58 | 	//<---------------------------------------------------->|//
 59 | 	//|														|//
 60 | 	//|	Usage:												|//
 61 | 	//|   ======											|//
 62 | 	//|		g(delta, gamma)									|//
 63 | 	//|														|//
 64 | 	//<---------------------------------------------------->|//
 65 | 	//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||\*/
 66 | 	inline double g(const T *gamma, const T *delta);
 67 | 	
 68 | 
 69 | 
 70 | 	/*\\||//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||\\--/
 71 | 	//<---------------------------------------------------->|//
 72 | 	//|														|//
 73 | 	//|	Method Name:	expm								|//
 74 | 	//|   ============										|//
 75 | 	//|														|//
 76 | 	//<---------------------------------------------------->|//
 77 | 	//|														|//
 78 | 	//|	Description:										|//
 79 | 	//|	============										|//
 80 | 	//|														|//
 81 | 	//|   Compute the sum of x and delta. The memory for y	|//
 82 | 	//|   is assumed to have been allocated already.		|//
 83 | 	//|														|//
 84 | 	//|   The following arguments must be provided:			|//
 85 | 	//|														|//
 86 | 	//|   <T* x>											|//
 87 | 	//|   An array of type T containing the point x.		|//
 88 | 	//|														|//
 89 | 	//|   <const T* delta>									|//
 90 | 	//|   An array of type T containing the tangent vector.	|//
 91 | 	//|														|//
 92 | 	//|   <T* y>											|//
 93 | 	//|   The result is returned in y.						|//
 94 | 	//|														|//
 95 | 	//<---------------------------------------------------->|//
 96 | 	//|														|//
 97 | 	//|	Usage:												|//
 98 | 	//|   ======											|//
 99 | 	//|		expm(delta, y)									|//
100 | 	//|														|//
101 | 	//<---------------------------------------------------->|//
102 | 	//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||\*/
103 | 	inline void expm(const T *x, const T *delta, T *y);
104 | 
105 | 
106 | 
107 | 	/*\\||//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||\\--/
108 | 	//<---------------------------------------------------->|//
109 | 	//|														|//
110 | 	//|	Method Name:	logm								|//
111 | 	//|   ============										|//
112 | 	//|														|//
113 | 	//<---------------------------------------------------->|//
114 | 	//|														|//
115 | 	//|	Description:										|//
116 | 	//|	============										|//
117 | 	//|														|//
118 | 	//|   Computes the difference of x and y. The memory for|//
119 | 	//|   delta is assumed to have been allocated already.	|//
120 | 	//|														|//
121 | 	//|   The following arguments must be provided:			|//
122 | 	//|														|//
123 | 	//|   <T* x>											|//
124 | 	//|   An array of type T containing the point x.		|//
125 | 	//|														|//
126 | 	//|   <const T* y>										|//
127 | 	//|   An array of type T containing the point.			|//
128 | 	//|														|//
129 | 	//|   <T* delta>										|//
130 | 	//|   The tangent is returned in delta.					|//
131 | 	//|														|//
132 | 	//<---------------------------------------------------->|//
133 | 	//|														|//
134 | 	//|	Usage:												|//
135 | 	//|   ======											|//
136 | 	//|		logm(y, delta)									|//
137 | 	//|														|//
138 | 	//<---------------------------------------------------->|//
139 | 	//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||//--\\||\*/
140 | 	inline void logm(const T *x, const T *y, T *delta);
141 | 
142 | };
143 | 
144 | 
145 | 
146 | /********************************************************/
147 | //Class Constructor										//
148 | //*******************************************************/
149 | //Post:													//
150 | //      The EuclideanGeometry class is initialized.		//
151 | /********************************************************/
152 | template<typename T> CEuclideanGeometry<T>::CEuclideanGeometry(int d):
153 | CGeometry<T>(d, d)
154 | {}
155 | 
156 | /********************************************************/
157 | //Inner Product											//
158 | //******************************************************//
159 | //Computes the dot product of tangents.					//
160 | /********************************************************/
161 | //Pre:													//
162 | //      - gamma is a tangent of size m_d.				//
163 | //      - delta is a tangent of size m_d.				//
164 | //Post:													//
165 | //      - the product is returned as a double value.	//
166 | //*******************************************************/
167 | template<typename T> double CEuclideanGeometry<T>::g(const T *gamma, const T *delta){
168 | 
169 | 	double d = 0;
170 | 	for(int i = 0; i < this->m_d; i++)
171 | 		d += gamma[i] * delta[i];
172 | 	return d;
173 | 
174 | }
175 | 
176 | 
177 | 
178 | /********************************************************/
179 | //Manifold Exponential									//
180 | //******************************************************//
181 | //Computes the exponential by adding delta to x.		//
182 | /********************************************************/
183 | //Pre:													//
184 | //      - x is a vector of size m_d.					//
185 | //      - delta is a tangent of size m_d.				//
186 | //Post:													//
187 | //      - the returned vector is y = x + delta			//
188 | //*******************************************************/
189 | template<typename T> void CEuclideanGeometry<T>::expm(const T* x, const T *delta, T *y){
190 | 
191 | 	for(int i = 0; i < this->m_d; i++)
192 | 		y[i] = x[i] + delta[i];
193 | 	
194 | }
195 | 
196 | 
197 | 
198 | /********************************************************/
199 | //Manifold Logarithm									//
200 | //******************************************************//
201 | //Computes the logarithm as the difference of x and y.	//
202 | /********************************************************/
203 | //Pre:													//
204 | //      - x is a vector of size m_d.					//
205 | //      - y is a vector of size m_d.					//
206 | //Post:													//
207 | //      - the returned tangent is delta = y - x			//
208 | //*******************************************************/
209 | template<typename T> void CEuclideanGeometry<T>::logm(const T* x, const T *y, T *delta){
210 | 
211 | 	for(int i = 0; i < this->m_d; i++)
212 | 		delta[i] = y[i] - x[i];
213 | 	
214 | }
215 | 
216 | 
217 | #endif
218 | 


--------------------------------------------------------------------------------
/include/namaris/utilities/filesystem.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef FILESYSTEM_UTILITIES_HPP
  2 | #define FILESYSTEM_UTILITIES_HPP
  3 | 
  4 | // STD includes
  5 | #include <iostream>
  6 | 
  7 | // BOOST
  8 | #include <boost/filesystem.hpp>
  9 | #include <boost/range/algorithm.hpp>
 10 | #include <boost/regex.hpp>
 11 | 
 12 | // Other
 13 | #include <namaris/utilities/string.hpp>
 14 | #include <namaris/utilities/alphanum/alphanum.hpp>
 15 | 
 16 | namespace utl
 17 | {
 18 |   namespace fs
 19 |   {
 20 |     /** \brief An analogue of the MATLAB dir function
 21 |       * \param[in] dir_path input directory
 22 |       * \param[out] content_list a vector of strings representing names of files/folders in the directory
 23 |       */
 24 |     inline
 25 |     void dir(const std::string &dir_path, std::vector<std::string> &content_list)
 26 |     {
 27 |       std::string dirPath (dir_path);
 28 |       
 29 |       // First normalize the beginning of the path
 30 |       // If directory path does not start with '/', './' or '../' append './'
 31 |       if  (!(((dirPath.length() >= 1) && (dirPath.substr(0,1) == "/")) || 
 32 |           ((dirPath.length() >= 2) && (dirPath.substr(0,2) == "./")) ||
 33 |           ((dirPath.length() >= 3) && (dirPath.substr(0,3) == "../"))))
 34 |       {
 35 |         dirPath.insert(0, "./");
 36 |       }
 37 |         
 38 |       // Find the rightmost '/' delimeter and split along it
 39 |       std::string left, right;
 40 |       size_t found = dirPath.find_last_of("/");
 41 |       if (found == std::string::npos)
 42 |       {
 43 |         std::cout << "[dir] something wrong\n";
 44 |         exit(EXIT_FAILURE);
 45 |       }
 46 |       else
 47 |       {
 48 |         left = dirPath.substr(0, found+1);
 49 |         right = dirPath.substr(found+1);
 50 |       }
 51 |       
 52 |       // Now analyze the right part
 53 |       std::string fileFormatStr;
 54 |       
 55 |       // If right part is empty list everything in left
 56 |       if (right.empty())
 57 |       {
 58 |         fileFormatStr = "*";
 59 |         dirPath = left;
 60 |       }
 61 |       
 62 |       // If right side containts a wildcard or an extension list everything in left that matches
 63 |       else if ((right.find("*") != std::string::npos) || (boost::filesystem::path(right).has_extension()))
 64 |       {
 65 |         fileFormatStr = right;
 66 |         dirPath = left;
 67 |       }
 68 |       
 69 |       // If right is a directory append it to left and list everything
 70 |       else if (!boost::filesystem::path(right).has_extension())
 71 |       {
 72 |         fileFormatStr = "*";
 73 |         dirPath = left + right;
 74 |       }
 75 | 
 76 |       // Generate regex expression
 77 |       std::string regexExpression (fileFormatStr);
 78 |       utl::str::replaceSubstring(regexExpression, ".", "[.]");      // Replace all occurences of '.' with '[.]'
 79 |       utl::str::replaceSubstring(regexExpression, "*", ".*");       // Replace all occurences of '*' with '.*'
 80 |       boost::regex filenameFormatRgx(regexExpression);  
 81 |       
 82 |       // List
 83 |       content_list.resize(0);
 84 |       for (boost::filesystem::directory_iterator file_it(dirPath), end; file_it != end; ++file_it)
 85 |       {
 86 |         std::string curFileName = file_it->path().leaf().string();                          // Get current file name
 87 |         
 88 |         if (boost::regex_match(curFileName, filenameFormatRgx))                               // Check if it matches the filename format   
 89 |           content_list.push_back(curFileName);
 90 |       }
 91 |       
 92 |       // Sort the list
 93 |       std::sort(content_list.begin(), content_list.end(), doj::alphanum_less<std::string>());    
 94 |     }
 95 | 
 96 |     /** \brief Add trailing slash to pathname if it is missing
 97 |       * \param[in] pathname file path
 98 |       * \return path with trailing slash
 99 |       */
100 |     inline
101 |     std::string addTrailingSlash (const std::string &pathname)
102 |     {
103 |       if (boost::filesystem::path(pathname).has_extension())
104 |         return pathname;
105 |       if (!pathname.empty() && pathname.back() != '/')
106 |         return pathname + '/';
107 |       else
108 |         return pathname;
109 |     }
110 |     
111 |     /** \brief Add trailing slash to pathname if it is missing
112 |       * \param[in] pathname file path
113 |       * \return path with trailing slash
114 |       */
115 |     inline
116 |     std::string removeTrailingSlash (const std::string &pathname)
117 |     {
118 |       if (!pathname.empty() && pathname.back() == '/')
119 |         return pathname.substr(0, pathname.size()-1);
120 |       else
121 |         return pathname;
122 |     }
123 |     
124 |     /** \brief Find filename extension of a path
125 |       * \param[in] path path
126 |       * \return extension
127 |       */
128 |     inline
129 |     std::string getExtension (const std::string &pathname)
130 |     {
131 |       return boost::filesystem::extension(boost::filesystem::path(pathname));
132 |     }
133 | 
134 |     
135 |     /** \brief Find the parent directory of a path
136 |       * \param[in] path path
137 |       * \return parent directory
138 |       */
139 |     inline
140 |     std::string getParentDir (const std::string &path)
141 |     {
142 |       std::string parentDir = boost::filesystem::path(removeTrailingSlash(path)).parent_path().string();
143 |       if (parentDir == "")
144 |         parentDir = "./";
145 |       
146 |       return parentDir;
147 |     }
148 |     
149 |     /** \brief If input is a filename - return filename without preceeding path. 
150 |      *  If input is path - return last directory in the path
151 |       * \param[in] path path
152 |       * \return filename
153 |       */
154 |     inline
155 |     std::string getBasename (const std::string &path)
156 |     {
157 |       if (boost::filesystem::path(path).has_extension())
158 |         return boost::filesystem::path(path).filename().string();
159 |       else if (!path.empty() && path.back() != '/')
160 |         return boost::filesystem::path(path).stem().string();
161 |       else
162 |         return boost::filesystem::path(path.substr(0, path.length()-1)).stem().string();
163 |     }
164 | 
165 |     /** \brief Get filename without extension from the path
166 |       * \param[in] path path
167 |       * \return filename without extension
168 |       */
169 |     inline
170 |     std::string getBasenameNoExtension (const std::string &path)
171 |     {
172 |       return boost::filesystem::path(path).stem().string();
173 |     }
174 |     
175 |     /** \brief Check if a path exists or not
176 |       * \return true if path exists
177 |       */
178 |     inline
179 |     bool exists (const std::string &path)
180 |     {
181 |       return boost::filesystem::exists(path);
182 |     }
183 | 
184 |     /** \brief Check if a path is an existing file
185 |       * \return true if path is a filename
186 |       */
187 |     inline
188 |     bool isFile (const std::string &path)
189 |     {
190 |       return (boost::filesystem::is_regular_file(boost::filesystem::path(path)));    
191 |     }
192 |     
193 |     /** \brief Check if a path is a directory that exists
194 |       * \return true if path is a directory
195 |       */
196 |     inline
197 |     bool isDirectory (const std::string &path)
198 |     {
199 |       return (boost::filesystem::is_directory(boost::filesystem::path(path)));
200 |     }  
201 |     
202 |     /** \brief Delete a directory and all of its contents
203 |       * \param[in] path path
204 |       * \return false if directory does not exist or could not delete directory
205 |       */
206 |     inline
207 |     bool deleteDir (const std::string &path)
208 |     {
209 |       if (!boost::filesystem::remove_all(path))
210 |       {
211 |         std::cout << "[utl::deleteDir] could not delete directory '" << path << "'\n";
212 |         return false;
213 |       }
214 |       
215 |       return true;
216 |     }
217 |     
218 |     /** \brief Create a directory and all of its contents
219 |       * \param[in] path path
220 |       * \return false if directory already exists or could not create directory
221 |       */
222 |     inline
223 |     bool createDir (const std::string &path)
224 |     {    
225 |       if (!boost::filesystem::create_directory(boost::filesystem::path(path)))
226 |       {
227 |         std::cout << "[utl::createDir] Could not create directory '" << path << "'\n";
228 |         return false;
229 |       }
230 |       
231 |       return true;
232 |     }
233 |     
234 |     /** \brief Join to paths to generate new path. Inspired by MATLAB's fullfile
235 |       * \param[in] path1 first path
236 |       * \param[in] path2 second path
237 |       * \return joined path
238 |       */
239 |     inline
240 |     std::string fullfile (const std::string &path1, const std::string &path2)
241 |     {
242 |       boost::filesystem::path joinedPath = boost::filesystem::path(path1) / boost::filesystem::path(path2);
243 |       return joinedPath.string();
244 |     }        
245 |   }
246 | }
247 | 
248 | #endif    // FILESYSTEM_UTILITIES_HPP


--------------------------------------------------------------------------------
/include/namaris/algorithms/region_growing_plane_segmentation.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef REGION_GROWING_PLANE_SEGMENTATION_HPP
  2 | #define REGION_GROWING_PLANE_SEGMENTATION_HPP
  3 | 
  4 | // PCL includes
  5 | #include <pcl/common/angles.h>
  6 | 
  7 | // Utilities
  8 | #include <namaris/utilities/geometry.hpp>
  9 | #include <namaris/utilities/pointcloud.hpp>
 10 | 
 11 | // Algorithms
 12 | #include <namaris/algorithms/region_growing_normal_variation/region_growing_normal_variation.hpp>
 13 | 
 14 | namespace alg
 15 | {
 16 |   /** \brief Segment out all planar surfaces in a cloud. 
 17 |     * \param[in] cloud input cloud
 18 |     * \param[in] plane_normal plane normal
 19 |     * \param[in] plane_point plane point
 20 |     * \param[out] plane_points segmented plane points
 21 |     * \note Cloud must contain normal data.
 22 |     */
 23 |   template <typename PointT>
 24 |   bool regionGrowingPlaneSegmentation ( const typename pcl::PointCloud<PointT>::ConstPtr  &cloud,
 25 |                                         std::vector<std::vector<int> >                    &plane_points,
 26 |                                         const int                                         min_seg_size
 27 |                                       )
 28 |   {
 29 |     alg::RegionGrowingNormalVariation<PointT> rg;
 30 |     rg.setInputCloud(cloud);
 31 |     rg.setConsistentNormals(true);
 32 |     rg.setNumberOfNeighbors(5);
 33 |     rg.setSearchRadius(0.005);
 34 |     rg.setNormalVariationThreshold(pcl::deg2rad(20.0) * 100);
 35 |     rg.setMinSegmentSize(min_seg_size);
 36 |     rg.segment(plane_points);
 37 |     
 38 |     return true;
 39 |     
 40 |     
 41 | //     //----------------------------------------------------------------------------
 42 | //     // Check input and convert plane parameters
 43 | //     //----------------------------------------------------------------------------
 44 | //     
 45 | //     if (plane_coefficients.size() != 4)
 46 | //     {
 47 | //       std::cout << "[alg::minCutPlaneSegmentation] plane coefficients must be length 4, instead they are length " << plane_coefficients.size() << "." << std::endl;
 48 | //       return false;
 49 | //     }
 50 | //     
 51 | //     Eigen::Vector3f planeNormal (plane_coefficients[0], plane_coefficients[1], plane_coefficients[2]);
 52 | //     Eigen::Vector3f planePoint  ( - plane_coefficients[3] / plane_coefficients[0], 0.0f, 0.0f);
 53 | //     
 54 | //     planePoint = utl::geom::projectPointToPlane<float>(cloud->at(0).getVector3fMap(), planePoint, planeNormal);
 55 | //     
 56 | //     //----------------------------------------------------------------------------
 57 | //     // Calculate unary potentials
 58 | //     //----------------------------------------------------------------------------
 59 | //     
 60 | //     Eigen::Vector2f fgNormalAngleThresh (pcl::deg2rad(0.0),   pcl::deg2rad(10.0));
 61 | //     Eigen::Vector2f bgNormalAngleThresh (pcl::deg2rad(30.0),  pcl::deg2rad(45.0));
 62 | //     
 63 | //     Eigen::Vector2f fgDistanceThresh (0.00f, 0.03f);
 64 | //     Eigen::Vector2f bgDistanceThresh (0.05f, 0.2f);
 65 | // 
 66 | //     
 67 | //     
 68 | //     std::vector<float> fgWeights (cloud->size(), 0);
 69 | //     std::vector<float> bgWeights (cloud->size(), 0);
 70 | //     
 71 | //     for (size_t pointId = 0; pointId < cloud->size(); pointId++)
 72 | //     {
 73 | //       // Get normal weight
 74 | //       float curFgWeightNormal = 0;
 75 | //       float curBgWeightNormal = 0;
 76 | //       
 77 | //       float dotProd = utl::math::clampValue(cloud->at(pointId).getNormalVector3fMap().dot(planeNormal), -1.0f, 1.0f);
 78 | //       float normalAngleDiff = std::acos(dotProd);
 79 | //       curFgWeightNormal = 1 - (utl::math::clampValue(normalAngleDiff, fgNormalAngleThresh[0], fgNormalAngleThresh[1]) - fgNormalAngleThresh[0]) / (fgNormalAngleThresh[1] - fgNormalAngleThresh[0]);
 80 | //       curBgWeightNormal =     (utl::math::clampValue(normalAngleDiff, bgNormalAngleThresh[0], bgNormalAngleThresh[1]) - bgNormalAngleThresh[0]) / (bgNormalAngleThresh[1] - bgNormalAngleThresh[0]);
 81 | //       
 82 | //       // Get distance weight
 83 | //       float curFgWeightDistance = 0;
 84 | //       float curBgWeightDistance = 0;
 85 | //       
 86 | //       float distance = std::abs(utl::geom::pointToPlaneDistance<float>(cloud->at(pointId).getVector3fMap(), planePoint, planeNormal));
 87 | //       curFgWeightDistance = 1 - (utl::math::clampValue(distance, fgDistanceThresh[0], fgDistanceThresh[1]) - fgDistanceThresh[0]) / (fgDistanceThresh[1] - fgDistanceThresh[0]);
 88 | //       curBgWeightDistance =     (utl::math::clampValue(distance, bgDistanceThresh[0], bgDistanceThresh[1]) - bgDistanceThresh[0]) / (bgDistanceThresh[1] - bgDistanceThresh[0]);
 89 | //       
 90 | // 
 91 | //       // Combine weights
 92 | //       fgWeights[pointId] = curFgWeightNormal * curFgWeightDistance;
 93 | //       bgWeights[pointId] = std::max(curBgWeightNormal, curBgWeightDistance);
 94 | //       
 95 | // //       if ((fgWeights[pointId] < 0) || (bgWeights[pointId] < 0) || std::isnan(fgWeights[pointId]) || std::isnan(bgWeights[pointId]))
 96 | // //       {
 97 | // //         cout << pointId << ", " << fgWeights[pointId] << ", " << bgWeights[pointId] << ", " << curFgWeightNormal << ", " << curBgWeightNormal << ", " << normalAngleDiff << ", " << distance << endl;
 98 | // //         
 99 | // //       }
100 | //     }
101 | //     
102 | //     //----------------------------------------------------------------------------
103 | //     // Find connectivity of the graph
104 | //     //----------------------------------------------------------------------------
105 | // 
106 | //     int numNeighbours = 6;
107 | //     
108 | //     pcl::search::KdTree<PointT> searchTree;
109 | //     searchTree.setInputCloud(cloud);  
110 | //     utl::graph::Graph adjacency (cloud->size());
111 | //     
112 | //     for (size_t point1Id = 0; point1Id < cloud->size(); point1Id++)
113 | //     {
114 | //       // Find nearest neighbour
115 | //       std::vector<int> neighbours (numNeighbours);
116 | //       std::vector<float> distance (numNeighbours);    
117 | //       searchTree.nearestKSearch(point1Id, numNeighbours, neighbours, distance);
118 | //       
119 | //       for (size_t nbrIdIt = 1; nbrIdIt < neighbours.size(); nbrIdIt++)
120 | //         utl::graph::addEdge(point1Id, neighbours[nbrIdIt], adjacency);
121 | //     }
122 | //       
123 | //     //----------------------------------------------------------------------------
124 | //     // Calcualte binary weights
125 | //     //----------------------------------------------------------------------------
126 | // 
127 | //     utl::graph::GraphWeights binaryWeights = utl::graph::createGraphWeights(adjacency);
128 | //     
129 | //     for (size_t point1Id = 0; point1Id < cloud->size(); point1Id++)
130 | //     {
131 | //       Eigen::Vector3f point1Normal = cloud->at(point1Id).getNormalVector3fMap();
132 | //       
133 | //       for (size_t point2It = 0; point2It < adjacency[point1Id].size(); point2It++)
134 | //       {
135 | //         int point2Id = adjacency[point1Id][point2It];
136 | //         Eigen::Vector3f point2Normal = cloud->at(point2Id).getNormalVector3fMap();
137 | //         
138 | //         float weight = (1.0 + point1Normal.dot(point2Normal)) / 2;
139 | //         utl::graph::setEdgeWeight(adjacency, binaryWeights, point1Id, point2Id, weight);
140 | //       }
141 | //     }
142 | //     
143 | //     //----------------------------------------------------------------------------
144 | //     // Segment
145 | //     //----------------------------------------------------------------------------
146 | //     
147 | //     std::vector<int> fgPoints, bgPoints;
148 | //     alg::mincut(fgWeights, bgWeights, adjacency, binaryWeights, bgPoints, fgPoints);
149 | //     plane_points = fgPoints;
150 | //     
151 | // //     //----------------------------------------------------------------------------
152 | // //     // Debug visualization
153 | // //     //----------------------------------------------------------------------------
154 | // //     
155 | // //     PclVis visualizer = pcl::createVisualizer();
156 | // //     visualizer.setCameraPosition (  0.0, 0.0, 0.0,   // camera position
157 | // //                                     0.0, 0.0, 1.0,   // viewpoint
158 | // //                                     0.0, -1.0, 0.0,   // normal
159 | // //                                     0.0);            // viewport  
160 | // //   
161 | // //     utl::Colormap colormap;
162 | // //     pcl::showPointCloudWithData<PointT,float> (visualizer, cloud, fgWeights, colormap);
163 | // //     visualizer.addText("FG weights", 0, 50, 12, 1.0, 1.0, 1.0);
164 | // //     visualizer.spin();  
165 | // //   
166 | // //     visualizer.removeAllPointClouds();
167 | // //     visualizer.removeAllShapes();
168 | // //     pcl::showPointCloudWithData<PointT,float> (visualizer, cloud, bgWeights, colormap);
169 | // //     visualizer.addText("BG weights", 0, 50, 12, 1.0, 1.0, 1.0);
170 | // //     visualizer.spin();  
171 | // //     
172 | // //     visualizer.removeAllPointClouds();
173 | // //     pcl::showFGsegmentation(visualizer, *cloud, plane_points);
174 | // //     visualizer.spin();  
175 |     
176 |     return true;
177 |   }
178 | }
179 | 
180 | #endif    // REGION_GROWING_PLANE_SEGMENTATION_HPP


--------------------------------------------------------------------------------