├── .idea ├── ForestMetrics.iml ├── dictionaries │ └── she397.xml ├── misc.xml ├── modules.xml ├── vcs.xml └── workspace.xml ├── CMakeLists.txt ├── Doxyfile.in ├── LICENSE ├── README.md ├── Step_1.PNG ├── Step_2.PNG ├── Step_3.PNG ├── Step_4.PNG ├── cmake └── Modules │ └── UseDoxygen.cmake ├── data ├── subset1.pcd ├── subset2.pcd ├── subset3.pcd ├── subset4.pcd ├── subset5.pcd ├── subset6.pcd ├── subset7.pcd └── subset8.pcd ├── gui ├── CMakeLists.txt ├── delineation │ ├── CMakeLists.txt │ ├── cluster_list_model.cpp │ ├── cluster_list_model.h │ ├── config.cpp │ ├── config.h │ ├── delineation.pro │ ├── graph_building_form.cpp │ ├── graph_building_form.h │ ├── graph_building_form.ui │ ├── main.cpp │ ├── main_window.cpp │ ├── main_window.h │ ├── main_window.ui │ ├── min_max_widget.cpp │ ├── min_max_widget.h │ ├── min_max_widget.ui │ ├── pipeline.h │ ├── preprocessing_form.cpp │ ├── preprocessing_form.h │ ├── preprocessing_form.ui │ ├── seed_selection.cpp │ ├── seed_selection.h │ ├── seed_selection_form.cpp │ ├── seed_selection_form.h │ ├── seed_selection_form.ui │ ├── segmentation_form.cpp │ ├── segmentation_form.h │ ├── segmentation_form.ui │ ├── status_bar.h │ ├── trunk_detection_form.cpp │ ├── trunk_detection_form.h │ ├── trunk_detection_form.ui │ └── types.h └── rws │ ├── CMakeLists.txt │ ├── main.cpp │ ├── main_window.cpp │ ├── main_window.h │ ├── main_window.ui │ ├── rws.pro │ ├── rws.pro.user │ ├── seed_selection.cpp │ └── seed_selection.h ├── include ├── as_range.h ├── conversions.h ├── factory │ ├── edge_weight_computer_factory.h │ ├── factory.h │ ├── graph_builder_factory.h │ └── graph_factory.h ├── graph │ ├── common.h │ ├── edge_weight_computer.h │ ├── edge_weight_computer_terms.h │ ├── graph_builder.h │ ├── impl │ │ ├── common.hpp │ │ ├── edge_weight_computer.hpp │ │ ├── nearest_neighbors_graph_builder.hpp │ │ └── voxel_grid_graph_builder.hpp │ ├── nearest_neighbors_graph_builder.h │ ├── point_cloud_graph.h │ ├── point_cloud_graph_concept.h │ ├── utils.h │ └── voxel_grid_graph_builder.h ├── graph_visualizer.h ├── hungarian │ └── hungarian.h ├── impl │ ├── io.hpp │ ├── octree_pointcloud_adjacency3.hpp │ ├── octree_pointcloud_adjacency_container2.hpp │ ├── random_walker.hpp │ └── random_walker_segmentation.hpp ├── io.h ├── kde.h ├── label_utils.h ├── measure_runtime.h ├── mesh_grid.h ├── random_walker.h ├── random_walker_segmentation.h ├── scaler.h ├── seed_utils.h ├── tree_top_detector.h └── typedefs.h ├── src ├── create_seeds.cpp ├── hungarian │ ├── CMakeLists.txt │ └── hungarian.cpp ├── io.cpp ├── kde.cpp ├── mesh_grid.cpp ├── random_walker_segmentation.cpp ├── random_walker_segmentation_app.cpp ├── segmentation_evaluation.cpp ├── slice.cpp ├── tree_top_detector.cpp └── view_point_cloud_graph.cpp └── third-party ├── hungarian-v2.0 ├── Assignment.cpp ├── Assignment.h ├── BipartiteGraph.cpp ├── BipartiteGraph.h ├── CmdParser.h ├── Hungarian.cpp ├── Hungarian.h ├── Log ├── Makefile ├── Matrix.h ├── PlotGraph.cpp ├── PlotGraph.h ├── README ├── example.demo ├── main.cpp └── plot │ ├── Cgnuplot.cpp │ ├── Cgnuplot.h │ ├── Makefile │ ├── README │ ├── graph_hungarian.cpp │ └── graph_hungarian.h └── tviewer └── CMakeLists.txt /.idea/ForestMetrics.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | -------------------------------------------------------------------------------- /.idea/dictionaries/she397.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | passthrough 5 | 6 | 7 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6 FATAL_ERROR) 2 | project(delineation) 3 | find_package(PCL 1.7.2 REQUIRED) 4 | set(CMAKE_BUILD_TYPE "Release") 5 | 6 | option(WITH_QT_GUI "Build Qt GUI application." ON) 7 | 8 | set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODULE_PATH}) 9 | 10 | # set the default path for built executables to the "bin" directory 11 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 12 | # set the default path for built libraries to the "lib" directory 13 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 14 | # add include folder to the list include directories 15 | include_directories(${PROJECT_SOURCE_DIR}/include) 16 | 17 | add_definitions(-std=c++11) 18 | 19 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 20 | add_definitions(-fcolor-diagnostics) 21 | endif () 22 | 23 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 24 | 25 | #...: enable all but certain warnings 26 | add_definitions(-Wall) 27 | add_definitions(-Wno-unknown-pragmas) 28 | add_definitions(-Wno-deprecated) 29 | add_definitions(-fpermissive) 30 | 31 | # PCL 32 | include_directories(${PCL_INCLUDE_DIRS}) 33 | link_directories(${PCL_LIBRARY_DIRS}) 34 | add_definitions(${PCL_DEFINITIONS}) 35 | 36 | # TViewer 37 | add_subdirectory(third-party/tviewer) 38 | include_directories(${TVIEWER_INCLUDE_DIR}) 39 | add_definitions(${TVIEWER_DEFINITIONS}) 40 | 41 | # Hungarian 42 | add_subdirectory(src/hungarian) 43 | 44 | add_library(random_walker_segmentation 45 | src/random_walker_segmentation.cpp 46 | ) 47 | target_link_libraries(random_walker_segmentation 48 | ${PCL_LIBRARIES} 49 | ) 50 | set_target_properties(random_walker_segmentation 51 | PROPERTIES COMPILE_DEFINITIONS "PCL_ONLY_CORE_POINT_TYPES" 52 | ) 53 | 54 | add_library(io 55 | src/io.cpp 56 | ) 57 | target_link_libraries(io 58 | ${PCL_LIBRARIES} 59 | ) 60 | 61 | add_library(kde 62 | src/kde.cpp 63 | ) 64 | 65 | add_executable(app_random_walker_segmentation 66 | src/random_walker_segmentation_app.cpp 67 | ) 68 | target_link_libraries(app_random_walker_segmentation 69 | ${PCL_LIBRARIES} 70 | ${TVIEWER_LIBRARIES} 71 | random_walker_segmentation 72 | io 73 | ) 74 | add_dependencies(app_random_walker_segmentation 75 | tviewer 76 | ) 77 | 78 | add_executable(view_point_cloud_graph 79 | src/view_point_cloud_graph.cpp 80 | ) 81 | target_link_libraries(view_point_cloud_graph 82 | ${PCL_LIBRARIES} 83 | ${TVIEWER_LIBRARIES} 84 | io 85 | ) 86 | add_dependencies(view_point_cloud_graph 87 | tviewer 88 | ) 89 | 90 | add_executable(segmentation_evaluation 91 | src/segmentation_evaluation.cpp 92 | ) 93 | target_link_libraries(segmentation_evaluation 94 | ${PCL_LIBRARIES} 95 | ${TVIEWER_LIBRARIES} 96 | hungarian 97 | ) 98 | add_dependencies(segmentation_evaluation 99 | tviewer 100 | ) 101 | 102 | add_executable(create_seeds 103 | src/create_seeds.cpp 104 | ) 105 | target_link_libraries(create_seeds 106 | ${PCL_LIBRARIES} 107 | ) 108 | 109 | add_executable(slice 110 | src/slice.cpp 111 | ) 112 | target_link_libraries(slice 113 | ${PCL_LIBRARIES} 114 | ${TVIEWER_LIBRARIES} 115 | ) 116 | add_dependencies(slice 117 | tviewer 118 | ) 119 | 120 | if(WITH_QT_GUI) 121 | add_subdirectory(gui) 122 | endif() 123 | -------------------------------------------------------------------------------- /Step_1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yurithefury/ForestMetrics/0e5caf54fc02b3264dc46e493a21c9fa45d45f53/Step_1.PNG -------------------------------------------------------------------------------- /Step_2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yurithefury/ForestMetrics/0e5caf54fc02b3264dc46e493a21c9fa45d45f53/Step_2.PNG -------------------------------------------------------------------------------- /Step_3.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yurithefury/ForestMetrics/0e5caf54fc02b3264dc46e493a21c9fa45d45f53/Step_3.PNG -------------------------------------------------------------------------------- /Step_4.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yurithefury/ForestMetrics/0e5caf54fc02b3264dc46e493a21c9fa45d45f53/Step_4.PNG -------------------------------------------------------------------------------- /cmake/Modules/UseDoxygen.cmake: -------------------------------------------------------------------------------- 1 | # - Run Doxygen 2 | # 3 | # Adds a doxygen target that runs doxygen to generate the html 4 | # and optionally the LaTeX API documentation. 5 | # The doxygen target is added to the doc target as a dependency. 6 | # i.e.: the API documentation is built with: 7 | # make doc 8 | # 9 | # USAGE: GLOBAL INSTALL 10 | # 11 | # Install it with: 12 | # cmake ./ && sudo make install 13 | # Add the following to the CMakeLists.txt of your project: 14 | # include(UseDoxygen OPTIONAL) 15 | # Optionally copy Doxyfile.in in the directory of CMakeLists.txt and edit it. 16 | # 17 | # USAGE: INCLUDE IN PROJECT 18 | # 19 | # set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}) 20 | # include(UseDoxygen) 21 | # Add the Doxyfile.in and UseDoxygen.cmake files to the projects source directory. 22 | # 23 | # 24 | # Variables you may define are: 25 | # DOXYFILE_SOURCE_DIR - Path where the Doxygen input files are. 26 | # Defaults to the current source and binary directory. 27 | # DOXYFILE_OUTPUT_DIR - Path where the Doxygen output is stored. Defaults to "doc". 28 | # 29 | # DOXYFILE_LATEX - Set to "NO" if you do not want the LaTeX documentation 30 | # to be built. 31 | # DOXYFILE_LATEX_DIR - Directory relative to DOXYFILE_OUTPUT_DIR where 32 | # the Doxygen LaTeX output is stored. Defaults to "latex". 33 | # 34 | # DOXYFILE_HTML_DIR - Directory relative to DOXYFILE_OUTPUT_DIR where 35 | # the Doxygen html output is stored. Defaults to "html". 36 | # 37 | 38 | # 39 | # Copyright (c) 2009, 2010 Tobias Rautenkranz 40 | # 41 | # Redistribution and use is allowed according to the terms of the New 42 | # BSD license. 43 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 44 | # 45 | 46 | macro(usedoxygen_set_default name value) 47 | if(NOT DEFINED "${name}") 48 | set("${name}" "${value}") 49 | endif() 50 | endmacro() 51 | 52 | find_package(Doxygen) 53 | 54 | if(DOXYGEN_FOUND) 55 | find_file(DOXYFILE_IN "Doxyfile.in" 56 | PATHS "${CMAKE_CURRENT_SOURCE_DIR}" "${CMAKE_ROOT}/Modules/" 57 | NO_DEFAULT_PATH) 58 | set(DOXYFILE "${CMAKE_CURRENT_BINARY_DIR}/Doxyfile") 59 | include(FindPackageHandleStandardArgs) 60 | find_package_handle_standard_args(DOXYFILE_IN DEFAULT_MSG "DOXYFILE_IN") 61 | endif() 62 | 63 | if(DOXYGEN_FOUND AND DOXYFILE_IN_FOUND) 64 | usedoxygen_set_default(DOXYFILE_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/doc") 65 | usedoxygen_set_default(DOXYFILE_HTML_DIR "html") 66 | usedoxygen_set_default(DOXYFILE_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 67 | 68 | set_property(DIRECTORY APPEND PROPERTY 69 | ADDITIONAL_MAKE_CLEAN_FILES 70 | "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_HTML_DIR}") 71 | 72 | add_custom_target(tocsdoxygen 73 | COMMAND ${DOXYGEN_EXECUTABLE} 74 | ${DOXYFILE} 75 | COMMENT "Writing documentation to ${DOXYFILE_OUTPUT_DIR}..." 76 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) 77 | 78 | ## LaTeX 79 | set(DOXYFILE_PDFLATEX "NO") 80 | set(DOXYFILE_DOT "NO") 81 | 82 | find_package(LATEX) 83 | find_program(MAKE_PROGRAM make) 84 | if(LATEX_COMPILER AND MAKEINDEX_COMPILER AND MAKE_PROGRAM AND 85 | (NOT DEFINED DOXYFILE_LATEX OR DOXYFILE_LATEX STREQUAL "YES")) 86 | set(DOXYFILE_LATEX "YES") 87 | usedoxygen_set_default(DOXYFILE_LATEX_DIR "latex") 88 | 89 | set_property(DIRECTORY APPEND PROPERTY 90 | ADDITIONAL_MAKE_CLEAN_FILES 91 | "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}") 92 | 93 | if(PDFLATEX_COMPILER) 94 | set(DOXYFILE_PDFLATEX "YES") 95 | endif() 96 | if(DOXYGEN_DOT_EXECUTABLE) 97 | set(DOXYFILE_DOT "YES") 98 | endif() 99 | 100 | add_custom_command(TARGET tocsdoxygen 101 | POST_BUILD 102 | COMMAND ${MAKE_PROGRAM} 103 | COMMENT "Running LaTeX for Doxygen documentation in ${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}..." 104 | WORKING_DIRECTORY "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}") 105 | else() 106 | set(DOXYGEN_LATEX "NO") 107 | endif() 108 | 109 | 110 | configure_file(${DOXYFILE_IN} Doxyfile IMMEDIATE @ONLY) 111 | 112 | get_target_property(DOC_TARGET doc TYPE) 113 | if(NOT DOC_TARGET) 114 | add_custom_target(doc) 115 | endif() 116 | 117 | add_dependencies(doc tocsdoxygen) 118 | endif() 119 | -------------------------------------------------------------------------------- /gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(VTK_VERSION VERSION_LESS "6.0") 2 | message(STATUS "VTK version is 5 or below, therefore using Qt4") 3 | find_package(Qt4 REQUIRED) 4 | include(${QT_USE_FILE}) 5 | include(${VTK_USE_FILE}) 6 | macro(qt_wrap_ui) 7 | qt4_wrap_ui(${ARGN}) 8 | endmacro() 9 | list(APPEND VTK_LIBRARIES QVTK) 10 | else() 11 | message(STATUS "VTK version is 6 or above, therefore using Qt5") 12 | find_package(Qt5Widgets REQUIRED) 13 | macro(qt_wrap_ui) 14 | qt5_wrap_ui(${ARGN}) 15 | endmacro() 16 | set(QT_LIBRARIES ${Qt5Widgets_LIBRARIES}) 17 | endif() 18 | 19 | # Find includes in corresponding build directories 20 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 21 | # Instruct CMake to run moc automatically when needed. 22 | set(CMAKE_AUTOMOC ON) 23 | add_definitions(${QT_DEFINITIONS} "-DQT_NO_KEYWORDS") 24 | 25 | macro(ADD_QT_FORM _prefix _name) 26 | list(APPEND ${_prefix}_SOURCES ${_name}.cpp) 27 | list(APPEND ${_prefix}_HEADERS ${_name}.h) 28 | list(APPEND ${_prefix}_FORMS ${_name}.ui) 29 | endmacro(ADD_QT_FORM) 30 | 31 | add_subdirectory(rws) 32 | add_subdirectory(delineation) 33 | -------------------------------------------------------------------------------- /gui/delineation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_QT_FORM(GUI_DELINEATION main_window) 2 | ADD_QT_FORM(GUI_DELINEATION preprocessing_form) 3 | ADD_QT_FORM(GUI_DELINEATION trunk_detection_form) 4 | ADD_QT_FORM(GUI_DELINEATION graph_building_form) 5 | ADD_QT_FORM(GUI_DELINEATION seed_selection_form) 6 | ADD_QT_FORM(GUI_DELINEATION segmentation_form) 7 | ADD_QT_FORM(GUI_DELINEATION min_max_widget) 8 | 9 | list(APPEND GUI_DELINEATION_SOURCES main.cpp 10 | seed_selection.cpp 11 | cluster_list_model.cpp) 12 | list(APPEND GUI_DELINEATION_HEADERS seed_selection.h 13 | cluster_list_model.h) 14 | 15 | qt_wrap_ui(GUI_DELINEATION_FORMS_HEADERS ${GUI_DELINEATION_FORMS}) 16 | 17 | add_executable(gui_delineation 18 | config.cpp 19 | ${PROJECT_SOURCE_DIR}/src/mesh_grid.cpp 20 | ${PROJECT_SOURCE_DIR}/src/tree_top_detector.cpp 21 | ${GUI_DELINEATION_SOURCES} 22 | ${GUI_DELINEATION_FORMS_HEADERS} 23 | ) 24 | target_link_libraries(gui_delineation 25 | io 26 | kde 27 | random_walker_segmentation 28 | ${QT_LIBRARIES} 29 | ${PCL_LIBRARIES} 30 | ${VTK_LIBRARIES} 31 | ${TVIEWER_LIBRARIES} 32 | ) 33 | add_dependencies(gui_delineation 34 | tviewer 35 | ) 36 | -------------------------------------------------------------------------------- /gui/delineation/cluster_list_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "cluster_list_model.h" 10 | 11 | ClusterListModel::ClusterListModel (QObject* parent) 12 | : QAbstractListModel (parent) 13 | , cloud_ (new PointCloud) 14 | { 15 | } 16 | 17 | ClusterListModel::~ClusterListModel () 18 | { 19 | } 20 | 21 | int 22 | ClusterListModel::rowCount (const QModelIndex&) const 23 | { 24 | return cluster_indices_.size (); 25 | } 26 | 27 | QVariant 28 | ClusterListModel::data (const QModelIndex& index, int role) const 29 | { 30 | if (role == Qt::DisplayRole) 31 | { 32 | return QString ("%1").arg (cluster_indices_[index.row ()].indices.size ()); 33 | } 34 | if (role == Qt::BackgroundRole) 35 | { 36 | auto c = pcl::GlasbeyLUT::at (index.row () % pcl::GlasbeyLUT::size ()); 37 | return QBrush (QColor (c.r, c.g, c.b)); 38 | } 39 | return QVariant (); 40 | } 41 | 42 | QModelIndex 43 | ClusterListModel::getIndexFromPointIndex (int index) 44 | { 45 | for (size_t i = 0; i < cluster_indices_.size (); ++i) 46 | { 47 | index -= static_cast (cluster_indices_[i].indices.size ()); 48 | if (index < 0) 49 | return createIndex (i, 0); 50 | } 51 | return QModelIndex (); 52 | } 53 | 54 | void 55 | ClusterListModel::setClusterList (const PointCloud::ConstPtr& cloud, 56 | const std::vector& cluster_indices) 57 | { 58 | auto old_size = cluster_indices_.size (); 59 | auto new_size = cluster_indices.size (); 60 | if (new_size > old_size) 61 | { 62 | beginInsertRows (QModelIndex (), old_size, new_size - 1); 63 | endInsertRows (); 64 | } 65 | else if (new_size < old_size) 66 | { 67 | beginRemoveRows (QModelIndex (), new_size, old_size - 1); 68 | endRemoveRows (); 69 | } 70 | cloud_ = cloud; 71 | cluster_indices_ = cluster_indices; 72 | } 73 | 74 | pcl::PointCloud::Ptr 75 | ClusterListModel::getPointCloudForVisualization () 76 | { 77 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 78 | for (size_t i = 0; i < cluster_indices_.size (); ++i) 79 | { 80 | uint8_t alpha = 0xFF; 81 | if (current_selection_.size () != 0 && current_selection_.count (i + 1) == 0) 82 | alpha = 0x3F; 83 | for (const auto& index : cluster_indices_[i].indices) 84 | { 85 | pcl::PointXYZRGBA p; 86 | pcl::copyPoint (cloud_->at (index), p); 87 | p.rgba = pcl::GlasbeyLUT::at (i % pcl::GlasbeyLUT::size ()).rgba; 88 | p.a = alpha; 89 | cloud->push_back (p); 90 | } 91 | } 92 | return cloud; 93 | } 94 | 95 | void 96 | ClusterListModel::currentChanged (const QItemSelection& current, const QItemSelection& previous) 97 | { 98 | for (size_t i = 0; i < current.indexes ().size (); ++i) 99 | current_selection_.insert (current.indexes ().at (i).row () + 1); 100 | for (size_t i = 0; i < previous.indexes ().size (); ++i) 101 | current_selection_.erase (previous.indexes ().at (i).row () + 1); 102 | selectionChanged (); 103 | } 104 | 105 | -------------------------------------------------------------------------------- /gui/delineation/cluster_list_model.h: -------------------------------------------------------------------------------- 1 | #ifndef CLUSTER_LIST_MODEL_H 2 | #define CLUSTER_LIST_MODEL_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #ifndef Q_MOC_RUN 13 | #include "types.h" 14 | #endif 15 | 16 | class ClusterListModel : public QAbstractListModel 17 | { 18 | 19 | Q_OBJECT 20 | 21 | public: 22 | 23 | typedef boost::shared_ptr Ptr; 24 | 25 | ClusterListModel (QObject* parent = 0); 26 | 27 | ~ClusterListModel (); 28 | 29 | virtual int 30 | rowCount (const QModelIndex&) const override; 31 | 32 | virtual QVariant 33 | data (const QModelIndex& index, int role) const override; 34 | 35 | QModelIndex 36 | getIndexFromPointIndex (int index); 37 | 38 | void 39 | setClusterList (const PointCloud::ConstPtr& cloud, 40 | const std::vector& cluster_indices); 41 | 42 | pcl::PointCloud::Ptr 43 | getPointCloudForVisualization (); 44 | 45 | public Q_SLOTS: 46 | 47 | void 48 | currentChanged (const QItemSelection& current, const QItemSelection& previous); 49 | 50 | Q_SIGNALS: 51 | 52 | //void 53 | //pointPicked (); 54 | 55 | void 56 | selectionChanged (); 57 | 58 | private: 59 | 60 | PointCloud::ConstPtr cloud_; 61 | 62 | std::vector cluster_indices_; 63 | 64 | std::set current_selection_; 65 | 66 | }; 67 | 68 | #endif /* CLUSTER_LIST_MODEL_H */ 69 | 70 | -------------------------------------------------------------------------------- /gui/delineation/config.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "config.h" 10 | #include "pipeline.h" 11 | #include "min_max_widget.h" 12 | 13 | Config::Config () 14 | { 15 | } 16 | 17 | Config::Config (const std::string& filename) 18 | { 19 | boost::property_tree::read_json (filename, pt_); 20 | } 21 | 22 | void 23 | Config::save (const std::string& filename) 24 | { 25 | boost::property_tree::write_json (filename, pt_); 26 | } 27 | 28 | void 29 | Config::put (const std::string& name, pipeline::Stage* stage) 30 | { 31 | section_ = name + "."; 32 | stage->saveConfig (*this); 33 | section_ = ""; 34 | } 35 | 36 | void 37 | Config::get (const std::string& name, pipeline::Stage* stage) 38 | { 39 | section_ = name + "."; 40 | stage->loadConfig (*this); 41 | section_ = ""; 42 | } 43 | 44 | template <> void 45 | Config::put (const std::string& name, QTabWidget* obj) 46 | { 47 | pt_.put (section_ + name, obj->currentIndex ()); 48 | } 49 | 50 | template <> void 51 | Config::get (const std::string& name, QTabWidget* obj, int dflt) 52 | { 53 | obj->setCurrentIndex (pt_.get (section_ + name, dflt)); 54 | } 55 | 56 | template <> void 57 | Config::put (const std::string& name, QComboBox* obj) 58 | { 59 | pt_.put (section_ + name, obj->currentIndex ()); 60 | } 61 | 62 | template <> void 63 | Config::get (const std::string& name, QComboBox* obj, int dflt) 64 | { 65 | obj->setCurrentIndex (pt_.get (section_ + name, dflt)); 66 | } 67 | 68 | template <> void 69 | Config::put (const std::string& name, QDoubleSpinBox* obj) 70 | { 71 | pt_.put (section_ + name, obj->value ()); 72 | } 73 | 74 | template <> void 75 | Config::get (const std::string& name, QDoubleSpinBox* obj, double dflt) 76 | { 77 | obj->setValue (pt_.get (section_ + name, dflt)); 78 | } 79 | 80 | template <> void 81 | Config::put (const std::string& name, QSpinBox* obj) 82 | { 83 | pt_.put (section_ + name, obj->value ()); 84 | } 85 | 86 | template <> void 87 | Config::get (const std::string& name, QSpinBox* obj, int dflt) 88 | { 89 | obj->setValue (pt_.get (section_ + name, dflt)); 90 | } 91 | 92 | template <> void 93 | Config::put (const std::string& name, QCheckBox* obj) 94 | { 95 | pt_.put (section_ + name, obj->checkState ()); 96 | } 97 | 98 | template <> void 99 | Config::get (const std::string& name, QCheckBox* obj, int dflt) 100 | { 101 | obj->setCheckState (Qt::CheckState (pt_.get (section_ + name, dflt))); 102 | } 103 | 104 | template <> void 105 | Config::put (const std::string& name, MinMaxWidget* obj) 106 | { 107 | pt_.put (section_ + name + ".Min", obj->value ().first); 108 | pt_.put (section_ + name + ".Max", obj->value ().second); 109 | pt_.put (section_ + name + ".RangeMin", obj->range ().first); 110 | pt_.put (section_ + name + ".RangeMax", obj->range ().second); 111 | } 112 | 113 | template <> void 114 | Config::get (const std::string& name, MinMaxWidget* obj) 115 | { 116 | obj->setRange (pt_.get (section_ + name + ".Min", 0.000), 117 | pt_.get (section_ + name + ".Max", 0.000)); 118 | obj->setValue (pt_.get (section_ + name + ".RangeMin", 0.000), 119 | pt_.get (section_ + name + ".RangeMax", 0.000)); 120 | } 121 | 122 | -------------------------------------------------------------------------------- /gui/delineation/config.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_CONFIG_H 2 | #define GUI_DELINEATION_CONFIG_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace pipeline 9 | { 10 | class Stage; 11 | } 12 | 13 | class Config 14 | { 15 | 16 | public: 17 | 18 | /// Constructor that creates an empty config. 19 | Config (); 20 | 21 | /// Constructor that loads existing config. 22 | Config (const std::string& filename); 23 | 24 | void 25 | save (const std::string& filename); 26 | 27 | void 28 | put (const std::string& name, pipeline::Stage* obj); 29 | 30 | void 31 | get (const std::string& name, pipeline::Stage* obj); 32 | 33 | template void 34 | put (const std::string& name, T* obj); 35 | 36 | template void 37 | get (const std::string& name, T* obj); 38 | 39 | template void 40 | get (const std::string& name, T* obj, M dflt); 41 | 42 | private: 43 | 44 | boost::property_tree::ptree pt_; 45 | std::string section_; 46 | 47 | }; 48 | 49 | #endif /* GUI_DELINEATION_CONFIG_H */ 50 | 51 | -------------------------------------------------------------------------------- /gui/delineation/delineation.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # Project created by QtCreator 2014-08-21T10:59:23 4 | # 5 | #------------------------------------------------- 6 | 7 | QT += core gui 8 | 9 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets 10 | 11 | TARGET = gui 12 | TEMPLATE = app 13 | 14 | SOURCES += main.cpp\ 15 | mainwindow.cpp \ 16 | seed_selection.cpp \ 17 | main_window.cpp \ 18 | preprocessing_form.cpp \ 19 | trunk_detection_form.cpp \ 20 | graph_building_form.cpp \ 21 | seed_selection_form.cpp \ 22 | segmentation_form.cpp \ 23 | min_max_widget.cpp 24 | 25 | HEADERS += mainwindow.h \ 26 | seed_selection.h \ 27 | main_window.h \ 28 | preprocessing_form.h \ 29 | trunk_detection_form.h \ 30 | graph_building_form.h \ 31 | seed_selection_form.h \ 32 | segmentation_form.h \ 33 | min_max_widget.h 34 | 35 | FORMS += main_window.ui \ 36 | preprocessing_form.ui \ 37 | trunk_detection_form.ui \ 38 | graph_building_form.ui \ 39 | seed_selection_form.ui \ 40 | segmentation_form.ui \ 41 | min_max_widget.ui 42 | -------------------------------------------------------------------------------- /gui/delineation/graph_building_form.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_GRAPH_BUILDING_FORM_H 2 | #define GUI_DELINEATION_GRAPH_BUILDING_FORM_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #ifndef Q_MOC_RUN 10 | #include "types.h" 11 | #include "pipeline.h" 12 | #endif 13 | 14 | namespace Ui 15 | { 16 | class GraphBuildingForm; 17 | } 18 | 19 | class GraphBuildingForm : public QWidget, public pipeline::Stage 20 | { 21 | 22 | Q_OBJECT 23 | 24 | public: 25 | 26 | explicit GraphBuildingForm (QWidget* parent = 0); 27 | 28 | ~GraphBuildingForm (); 29 | 30 | virtual void 31 | loadConfig (Config& config) override; 32 | 33 | virtual void 34 | saveConfig (Config& config) const override; 35 | 36 | virtual void 37 | enter () override; 38 | 39 | virtual void 40 | leave () override; 41 | 42 | void 43 | execute (); 44 | 45 | public Q_SLOTS: 46 | 47 | void 48 | onGraphBuilderEditingFinished (); 49 | 50 | void 51 | onEdgeWeightsEditingFinished (); 52 | 53 | void 54 | onUpdateButtonClicked (); 55 | 56 | private: 57 | 58 | void 59 | buildGraph (); 60 | 61 | void 62 | computeEdgeWeights (); 63 | 64 | pipeline::Input input_cloud_ = "LoadedCloud"; 65 | pipeline::Input> input_indices_ = "FilteredIndices"; 66 | pipeline::Output output_graph_ = "Graph"; 67 | 68 | Ui::GraphBuildingForm* ui_; 69 | 70 | GraphPtr graph_; 71 | 72 | PointCloud::Ptr vertices_; 73 | 74 | vtkSmartPointer edges_; 75 | 76 | }; 77 | 78 | #endif // GUI_DELINEATION_GRAPH_BUILDING_FORM_H 79 | -------------------------------------------------------------------------------- /gui/delineation/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "main_window.h" 8 | 9 | int 10 | main (int argc, char** argv) 11 | { 12 | if (argc < 2 || pcl::console::find_switch (argc, argv, "--help")) 13 | { 14 | pcl::console::print_error ("Usage: %s [seeds.pcd]" 15 | , argv[0]); 16 | return (1); 17 | } 18 | 19 | QApplication app (argc, argv); 20 | MainWindow w (argv[1]); 21 | 22 | if (argc > 2) 23 | w.loadSeeds (argv[2]); 24 | 25 | w.show (); 26 | return app.exec (); 27 | } 28 | 29 | -------------------------------------------------------------------------------- /gui/delineation/main_window.h: -------------------------------------------------------------------------------- 1 | #ifndef MAIN_WINDOW_H 2 | #define MAIN_WINDOW_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #ifndef Q_MOC_RUN 14 | #include "types.h" 15 | #include "pipeline.h" 16 | #include "random_walker_segmentation.h" 17 | #endif 18 | 19 | namespace Ui 20 | { 21 | class MainWindow; 22 | } 23 | 24 | class PipelineStage; 25 | class Visualizer; 26 | class StatusBar; 27 | 28 | class MainWindow : public QMainWindow 29 | { 30 | Q_OBJECT 31 | 32 | public: 33 | 34 | MainWindow (const std::string& filename, QWidget* parent = 0); 35 | 36 | ~MainWindow (); 37 | 38 | public Q_SLOTS: 39 | 40 | void 41 | onCurrentToolboxPageChanged (int index); 42 | 43 | void 44 | onButtonUpdateGraphClicked (); 45 | 46 | void 47 | buttonNewLabelClicked (); 48 | 49 | void 50 | buttonDeleteLabelClicked (); 51 | 52 | void 53 | buttonSegmentClicked (); 54 | 55 | void 56 | checkboxDisplayStateChanged (int state) 57 | { 58 | displayGraphVertices (); 59 | displayGraphEdges (); 60 | } 61 | 62 | void 63 | onActionExitTriggered () 64 | { 65 | this->close (); 66 | } 67 | 68 | inline void 69 | onActionLoadSeedsTriggered () 70 | { 71 | loadSeeds (QFileDialog::getOpenFileName ().toStdString ()); 72 | } 73 | 74 | void 75 | onActionSaveSeedsTriggered (); 76 | 77 | void 78 | onActionSaveSegmentationTriggered (); 79 | 80 | void 81 | seedsChanged (); 82 | 83 | void 84 | onKeyUp (); 85 | 86 | void 87 | onKeyDown (); 88 | 89 | bool 90 | loadData (const std::string& filename); 91 | 92 | bool 93 | loadSeeds (const std::string& filename); 94 | 95 | private: 96 | 97 | void 98 | displayGraphVertices (); 99 | 100 | void 101 | displayGraphEdges (uint32_t color = 0); 102 | 103 | void 104 | displaySeeds (); 105 | 106 | void 107 | saveConfig (); 108 | 109 | void 110 | loadConfig (); 111 | 112 | void 113 | activateStage (const std::string& name); 114 | 115 | void 116 | onDataChanged (const std::string& hub_name); 117 | 118 | Ui::MainWindow* ui_; 119 | 120 | std::shared_ptr status_bar_; 121 | 122 | PointCloud::Ptr cloud_; 123 | GraphPtr graph_; 124 | std::map colormap_; 125 | 126 | pipeline::Output loaded_cloud_ = "LoadedCloud"; 127 | pipeline::Output loaded_seeds_ = "Seeds"; 128 | 129 | }; 130 | 131 | #endif // MAIN_WINDOW_H 132 | -------------------------------------------------------------------------------- /gui/delineation/min_max_widget.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "min_max_widget.h" 4 | #include "ui_min_max_widget.h" 5 | 6 | MinMaxWidget::MinMaxWidget (QWidget* parent) 7 | : QWidget (parent) 8 | , ui_ (new Ui::MinMaxWidget) 9 | , updating_widgets_ (false) 10 | { 11 | ui_->setupUi (this); 12 | slider_range_ = ui_->slider_min->maximum () - ui_->slider_min->minimum (); 13 | ratio_ = (range_max_ - range_min_) / slider_range_; 14 | updateWidgets (); 15 | } 16 | 17 | MinMaxWidget::~MinMaxWidget () 18 | { 19 | delete ui_; 20 | } 21 | 22 | void 23 | MinMaxWidget::setRange (double min, double max) 24 | { 25 | Q_ASSERT (min <= max); 26 | if (min != range_min_ || max != range_max_) 27 | { 28 | range_min_ = min; 29 | range_max_ = max; 30 | ratio_ = (range_max_ - range_min_) / slider_range_; 31 | if (min_ < range_min_ || min_ > range_max_) 32 | min_ = range_min_; 33 | if (max_ > range_max_ || max_ < range_min_) 34 | max_ = range_max_; 35 | valueChanged (min_, max_); 36 | updateWidgets (); 37 | } 38 | } 39 | 40 | void 41 | MinMaxWidget::setValue (double min, double max) 42 | { 43 | Q_ASSERT (min <= max); 44 | if (min < range_min_ || min > range_max_) 45 | min = range_min_; 46 | if (max > range_max_ || max < range_min_) 47 | max = range_max_; 48 | min_ = min; 49 | max_ = max; 50 | valueChanged (min_, max_); 51 | updateWidgets (); 52 | } 53 | 54 | void 55 | MinMaxWidget::setSingleStep (double step) 56 | { 57 | Q_ASSERT (step >= 0); 58 | ui_->spinbox_min->setSingleStep (step); 59 | ui_->spinbox_max->setSingleStep (step); 60 | } 61 | 62 | std::pair 63 | MinMaxWidget::value () const 64 | { 65 | return {min_, max_}; 66 | } 67 | 68 | std::pair 69 | MinMaxWidget::range () const 70 | { 71 | return {range_min_, range_max_}; 72 | } 73 | 74 | double 75 | MinMaxWidget::singleStep () const 76 | { 77 | return ui_->spinbox_min->singleStep (); 78 | } 79 | 80 | void 81 | MinMaxWidget::onSliderMinValueChanged (int value) 82 | { 83 | if (!updating_widgets_) 84 | { 85 | min_ = toRealValue (value); 86 | if (min_ > max_) 87 | { 88 | max_ = min_; 89 | } 90 | valueChanged (min_, max_); 91 | updateWidgets (); 92 | } 93 | } 94 | 95 | void 96 | MinMaxWidget::onSpinboxMinValueChanged (double value) 97 | { 98 | if (!updating_widgets_) 99 | { 100 | min_ = value; 101 | if (min_ > max_) 102 | { 103 | max_ = min_; 104 | 105 | } 106 | valueChanged (min_, max_); 107 | updateWidgets (); 108 | } 109 | } 110 | 111 | void 112 | MinMaxWidget::onSliderMaxValueChanged (int value) 113 | { 114 | if (!updating_widgets_) 115 | { 116 | max_ = toRealValue (value); 117 | if (max_ < min_) 118 | { 119 | min_ = max_; 120 | } 121 | valueChanged (min_, max_); 122 | updateWidgets (); 123 | } 124 | } 125 | 126 | void 127 | MinMaxWidget::onSpinboxMaxValueChanged (double value) 128 | { 129 | if (!updating_widgets_) 130 | { 131 | max_ = value; 132 | if (max_ < min_) 133 | { 134 | min_ = max_; 135 | } 136 | valueChanged (min_, max_); 137 | updateWidgets (); 138 | } 139 | } 140 | 141 | void 142 | MinMaxWidget::updateWidgets () 143 | { 144 | updating_widgets_ = true; 145 | ui_->spinbox_min->setRange (range_min_, range_max_); 146 | ui_->spinbox_max->setRange (range_min_, range_max_); 147 | ui_->spinbox_min->setValue (min_); 148 | ui_->spinbox_max->setValue (max_); 149 | ui_->slider_min->setValue (toSliderValue (min_)); 150 | ui_->slider_max->setValue (toSliderValue (max_)); 151 | updating_widgets_ = false; 152 | } 153 | 154 | double 155 | MinMaxWidget::toRealValue (int value) 156 | { 157 | return ratio_ * value - ratio_ * ui_->slider_min->minimum () + range_min_; 158 | } 159 | 160 | int 161 | MinMaxWidget::toSliderValue (double value) 162 | { 163 | double v = ui_->slider_min->minimum () + (value - range_min_) / ratio_; 164 | if (v > ui_->slider_min->maximum ()) 165 | return ui_->slider_min->maximum (); 166 | if (v < ui_->slider_min->minimum ()) 167 | return ui_->slider_min->minimum (); 168 | return v; 169 | } 170 | 171 | -------------------------------------------------------------------------------- /gui/delineation/min_max_widget.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_MIN_MAX_WIDGET_H 2 | #define GUI_DELINEATION_MIN_MAX_WIDGET_H 3 | 4 | #include 5 | 6 | namespace Ui 7 | { 8 | class MinMaxWidget; 9 | } 10 | 11 | class MinMaxWidget : public QWidget 12 | { 13 | 14 | Q_OBJECT 15 | 16 | public: 17 | 18 | explicit MinMaxWidget (QWidget* parent = 0); 19 | 20 | ~MinMaxWidget (); 21 | 22 | void 23 | setRange (double min, double max); 24 | 25 | void 26 | setValue (double min, double max); 27 | 28 | void 29 | setSingleStep (double step); 30 | 31 | std::pair 32 | value () const; 33 | 34 | std::pair 35 | range () const; 36 | 37 | double 38 | singleStep () const; 39 | 40 | public Q_SLOTS: 41 | 42 | void 43 | onSliderMinValueChanged (int value); 44 | 45 | void 46 | onSpinboxMinValueChanged (double value); 47 | 48 | void 49 | onSliderMaxValueChanged (int value); 50 | 51 | void 52 | onSpinboxMaxValueChanged (double value); 53 | 54 | Q_SIGNALS: 55 | 56 | void 57 | valueChanged (double min, double max); 58 | 59 | private: 60 | 61 | /** Update values and ranges in sliders and spinboxes to reflect the 62 | * internal state. */ 63 | void 64 | updateWidgets (); 65 | 66 | /** Map integer slider value to the real value range. */ 67 | double 68 | toRealValue (int value); 69 | 70 | int 71 | toSliderValue (double value); 72 | 73 | Ui::MinMaxWidget* ui_; 74 | 75 | double range_min_ = 0; 76 | double range_max_ = 99; 77 | double min_ = 0; 78 | double max_ = 99; 79 | 80 | double ratio_; 81 | unsigned int slider_range_; 82 | bool updating_widgets_; 83 | 84 | }; 85 | 86 | #endif // GUI_DELINEATION_MIN_MAX_WIDGET_H 87 | -------------------------------------------------------------------------------- /gui/delineation/min_max_widget.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MinMaxWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | QFormLayout::AllNonFixedFieldsGrow 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -2147483647 27 | 28 | 29 | 2147483647 30 | 31 | 32 | Qt::Horizontal 33 | 34 | 35 | 36 | 37 | 38 | 39 | true 40 | 41 | 42 | 43 | 44 | 45 | 46 | -2147483647 47 | 48 | 49 | 2147483647 50 | 51 | 52 | 99 53 | 54 | 55 | Qt::Horizontal 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | slider_min 65 | valueChanged(int) 66 | MinMaxWidget 67 | onSliderMinValueChanged(int) 68 | 69 | 70 | 236 71 | 16 72 | 73 | 74 | 199 75 | 149 76 | 77 | 78 | 79 | 80 | spinbox_min 81 | valueChanged(double) 82 | MinMaxWidget 83 | onSpinboxMinValueChanged(double) 84 | 85 | 86 | 42 87 | 20 88 | 89 | 90 | 199 91 | 149 92 | 93 | 94 | 95 | 96 | slider_max 97 | valueChanged(int) 98 | MinMaxWidget 99 | onSliderMaxValueChanged(int) 100 | 101 | 102 | 236 103 | 45 104 | 105 | 106 | 199 107 | 149 108 | 109 | 110 | 111 | 112 | spinbox_max 113 | valueChanged(double) 114 | MinMaxWidget 115 | onSpinboxMaxValueChanged(double) 116 | 117 | 118 | 42 119 | 49 120 | 121 | 122 | 199 123 | 149 124 | 125 | 126 | 127 | 128 | 129 | onSliderMinValueChanged(int) 130 | onSpinboxMinValueChanged(double) 131 | onSliderMaxValueChanged(int) 132 | onSpinboxMaxValueChanged(double) 133 | 134 | 135 | -------------------------------------------------------------------------------- /gui/delineation/preprocessing_form.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "tviewer/tviewer.h" 6 | #include "tviewer/tviewer_widget/tviewer_widget.h" 7 | 8 | #include "types.h" 9 | #include "preprocessing_form.h" 10 | #include "ui_preprocessing_form.h" 11 | 12 | using namespace tviewer; 13 | 14 | PreprocessingForm::PreprocessingForm (QWidget* parent) 15 | : QWidget (parent) 16 | , ui_ (new Ui::PreprocessingForm) 17 | , display_ (new PointCloud) 18 | { 19 | ui_->setupUi (this); 20 | connect (ui_->minmax_x, 21 | SIGNAL (valueChanged (double, double)), 22 | this, 23 | SLOT (onPassthroughFilterChanged ())); 24 | connect (ui_->minmax_y, 25 | SIGNAL (valueChanged (double, double)), 26 | this, 27 | SLOT (onPassthroughFilterChanged ())); 28 | } 29 | 30 | PreprocessingForm::~PreprocessingForm () 31 | { 32 | delete ui_; 33 | } 34 | 35 | void 36 | PreprocessingForm::enter () 37 | { 38 | if (input_cloud_.changed ()) 39 | { 40 | pcl::copyPointCloud (input_cloud_, *display_); 41 | Point min; 42 | Point max; 43 | pcl::getMinMax3D (*display_, min, max); 44 | ui_->minmax_x->setRange (min.x, max.x); 45 | ui_->minmax_y->setRange (min.y, max.y); 46 | onPassthroughFilterChanged (); 47 | } 48 | 49 | viewer_->add 50 | ( CreatePointCloudObject ("points", "p") 51 | . description ("Input point cloud") 52 | . data (display_) 53 | . pointSize (2) 54 | , true 55 | ); 56 | } 57 | 58 | void 59 | PreprocessingForm::onPassthroughFilterChanged () 60 | { 61 | uint32_t IN = 0xFFF000; 62 | uint32_t OUT = 0x1F1F1F; 63 | auto x = ui_->minmax_x->value (); 64 | auto y = ui_->minmax_y->value (); 65 | for (auto& point : *display_) 66 | { 67 | if (point.x < x.first || point.x > x.second || 68 | point.y < y.first || point.y > y.second) 69 | point.rgba = OUT; 70 | else 71 | point.rgba = IN; 72 | } 73 | viewer_->update ("points"); 74 | } 75 | 76 | void 77 | PreprocessingForm::onButtonApplyClicked () 78 | { 79 | boost::shared_ptr> indices (new std::vector); 80 | auto x = ui_->minmax_x->value (); 81 | auto y = ui_->minmax_y->value (); 82 | auto predicate = boost::make_shared> 83 | ([=] (const Eigen::Vector3f& p) 84 | { return p[0] < x.first || p[0] > x.second || p[1] < y.first || p[1] > y.second; }); 85 | for (size_t i = 0; i < display_->size (); ++i) 86 | { 87 | if (predicate->operator() (display_->at (i).getVector3fMap ())) 88 | continue; 89 | indices->push_back (i); 90 | } 91 | output_indices_ = indices; 92 | output_predicate_ = predicate; 93 | } 94 | 95 | void 96 | PreprocessingForm::onButtonResetClicked () 97 | { 98 | auto x = ui_->minmax_x->range (); 99 | ui_->minmax_x->setValue (x.first, x.second); 100 | auto y = ui_->minmax_y->range (); 101 | ui_->minmax_y->setValue (y.first, y.second); 102 | output_indices_ = boost::none; 103 | output_predicate_ = boost::none; 104 | } 105 | 106 | void 107 | PreprocessingForm::loadConfig (Config& config) 108 | { 109 | config.get ("Passthrough.X", ui_->minmax_x); 110 | config.get ("Passthrough.Y", ui_->minmax_y); 111 | } 112 | 113 | void 114 | PreprocessingForm::saveConfig (Config& config) const 115 | { 116 | config.put ("Passthrough.X", ui_->minmax_x); 117 | config.put ("Passthrough.Y", ui_->minmax_y); 118 | } 119 | 120 | -------------------------------------------------------------------------------- /gui/delineation/preprocessing_form.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_PREPROCESSING_FORM_H 2 | #define GUI_DELINEATION_PREPROCESSING_FORM_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #ifndef Q_MOC_RUN 10 | #include "types.h" 11 | #include "pipeline.h" 12 | #endif 13 | 14 | namespace Ui 15 | { 16 | class PreprocessingForm; 17 | } 18 | 19 | class PreprocessingForm : public QWidget, public pipeline::Stage 20 | { 21 | 22 | Q_OBJECT 23 | 24 | public: 25 | 26 | explicit PreprocessingForm (QWidget* parent = 0); 27 | 28 | virtual ~PreprocessingForm (); 29 | 30 | virtual void 31 | enter () override; 32 | 33 | virtual void 34 | loadConfig (Config& config) override; 35 | 36 | virtual void 37 | saveConfig (Config& config) const override; 38 | 39 | public Q_SLOTS: 40 | 41 | void 42 | onPassthroughFilterChanged (); 43 | 44 | void 45 | onButtonApplyClicked (); 46 | 47 | void 48 | onButtonResetClicked (); 49 | 50 | private: 51 | 52 | pipeline::Input input_cloud_ = "LoadedCloud"; 53 | pipeline::Output> output_indices_ = "FilteredIndices"; 54 | pipeline::Output> output_predicate_ = "FilterPredicate"; 55 | 56 | Ui::PreprocessingForm* ui_; 57 | 58 | PointCloud::Ptr display_; 59 | 60 | }; 61 | 62 | #endif // GUI_DELINEATION_PREPROCESSING_FORM_H 63 | -------------------------------------------------------------------------------- /gui/delineation/preprocessing_form.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | PreprocessingForm 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | Passthrough filter 21 | 22 | 23 | 24 | 25 | 26 | Y 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | X 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | Reset 53 | 54 | 55 | 56 | 57 | 58 | 59 | Apply 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | Qt::Vertical 70 | 71 | 72 | 73 | 20 74 | 40 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | MinMaxWidget 84 | QWidget 85 |
min_max_widget.h
86 | 1 87 | 88 | 89 | 90 | 91 | 92 | button_apply 93 | clicked() 94 | PreprocessingForm 95 | onButtonApplyClicked() 96 | 97 | 98 | 296 99 | 378 100 | 101 | 102 | 199 103 | 199 104 | 105 | 106 | 107 | 108 | button_reset 109 | clicked() 110 | PreprocessingForm 111 | onButtonResetClicked() 112 | 113 | 114 | 103 115 | 378 116 | 117 | 118 | 199 119 | 199 120 | 121 | 122 | 123 | 124 | 125 | onPassthroughFilterChanged() 126 | onButtonApplyClicked() 127 | onButtonResetClicked() 128 | 129 | 130 | -------------------------------------------------------------------------------- /gui/delineation/seed_selection.cpp: -------------------------------------------------------------------------------- 1 | #include "seed_selection.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | SeedSelection::SeedSelection (QObject* parent) 9 | : QAbstractListModel (parent) 10 | , seeds_cloud_ (new LabeledPointCloud) 11 | , num_labels_ (0) 12 | { 13 | current_labels_.insert (0); 14 | } 15 | 16 | SeedSelection::~SeedSelection () 17 | { 18 | } 19 | 20 | int 21 | SeedSelection::rowCount (const QModelIndex&) const 22 | { 23 | return num_labels_; 24 | } 25 | 26 | QVariant 27 | SeedSelection::data (const QModelIndex& index, int role) const 28 | { 29 | if (role == Qt::DisplayRole) 30 | { 31 | uint32_t label = index.row () + 1; 32 | return QString ("%1 %2 seeds").arg (label).arg (countSeedsWithLabel (label)); 33 | } 34 | return QVariant (); 35 | } 36 | 37 | void 38 | SeedSelection::pickPoint (const pcl::PointXYZ& p) 39 | { 40 | using namespace pcl::utils; 41 | 42 | for (size_t i = 0; i < seeds_cloud_->size (); ++i) 43 | { 44 | pcl::PointXYZL& pt = seeds_cloud_->points[i]; 45 | if (equal (p.x, pt.x) && equal (p.y, pt.y) && equal (p.z, pt.z)) 46 | { 47 | if (current_labels_.count (pt.label)) 48 | { 49 | LabeledPointCloudPtr new_seeds (new LabeledPointCloud); 50 | for (size_t j = 0; j < seeds_cloud_->size (); ++j) 51 | if (j != i) 52 | new_seeds->push_back (seeds_cloud_->at (j)); 53 | seeds_cloud_.swap (new_seeds); 54 | seedsChanged (); 55 | dataChanged (QModelIndex (), QModelIndex ()); 56 | return; 57 | } 58 | else 59 | { 60 | // When multiple seed labels are selected do nothing 61 | if (current_labels_.size () == 1) 62 | { 63 | pt.label = *current_labels_.begin (); 64 | seedsChanged (); 65 | dataChanged (QModelIndex (), QModelIndex ()); 66 | } 67 | return; 68 | } 69 | } 70 | } 71 | 72 | // Picked a point which is not a seed. Make it a seed, but only in 73 | // single seed label mode 74 | if (current_labels_.size () == 1) 75 | { 76 | pcl::PointXYZL pt; 77 | pt.x = p.x, pt.y = p.y, pt.z = p.z, pt.label = *current_labels_.begin (); 78 | seeds_cloud_->push_back (pt); 79 | seedsChanged (); 80 | dataChanged (QModelIndex (), QModelIndex ()); 81 | } 82 | } 83 | 84 | QModelIndex 85 | SeedSelection::addNewLabel () 86 | { 87 | ++num_labels_; 88 | current_labels_.clear (); 89 | current_labels_.insert (num_labels_); 90 | dataChanged (QModelIndex (), QModelIndex ()); 91 | return index (num_labels_ - 1, 0); 92 | } 93 | 94 | void 95 | SeedSelection::deleteLabel () 96 | { 97 | // TODO: forbid deletion of multiple labels because it requires more complicated 98 | // label reassignment algorithm (to preserve sequential linear ordering). 99 | if (current_labels_.size () > 1) 100 | return; 101 | 102 | if (num_labels_ == current_labels_.size ()) 103 | return; 104 | 105 | LabeledPointCloudPtr new_seeds (new LabeledPointCloud); 106 | for (size_t i = 0; i < seeds_cloud_->size (); ++i) 107 | if (!current_labels_.count (seeds_cloud_->at (i).label)) 108 | { 109 | new_seeds->push_back (seeds_cloud_->at (i)); 110 | if (seeds_cloud_->at (i).label > *current_labels_.begin ()) 111 | --new_seeds->back ().label; 112 | } 113 | 114 | --num_labels_; 115 | seeds_cloud_.swap (new_seeds); 116 | dataChanged (QModelIndex (), QModelIndex ()); 117 | seedsChanged (); 118 | } 119 | 120 | void 121 | SeedSelection::setSeeds (const LabeledPointCloud& seeds) 122 | { 123 | pcl::copyPointCloud (seeds, *seeds_cloud_); 124 | num_labels_ = 0; 125 | for (size_t i = 0; i < seeds.size (); ++i) 126 | if (seeds[i].label > num_labels_) 127 | num_labels_ = seeds[i].label; 128 | current_labels_.clear (); 129 | current_labels_.insert (num_labels_); 130 | dataChanged (QModelIndex (), QModelIndex ()); 131 | seedsChanged (); 132 | } 133 | 134 | SeedSelection::ColoredPointCloudPtr 135 | SeedSelection::getPointCloudForVisualization () 136 | { 137 | ColoredPointCloudPtr cloud (new ColoredPointCloud); 138 | pcl::copyPointCloud (*seeds_cloud_, *cloud); 139 | for (size_t i = 0; i < cloud->size (); ++i) 140 | cloud->at (i).rgb = pcl::GlasbeyLUT::at (seeds_cloud_->at (i).label % pcl::GlasbeyLUT::size ()).rgb; 141 | return cloud; 142 | } 143 | 144 | void 145 | SeedSelection::currentChanged (const QItemSelection& current, const QItemSelection& previous) 146 | { 147 | for (size_t i = 0; i < current.indexes ().size (); ++i) 148 | current_labels_.insert (current.indexes ().at (i).row () + 1); 149 | for (size_t i = 0; i < previous.indexes ().size (); ++i) 150 | current_labels_.erase (previous.indexes ().at (i).row () + 1); 151 | seedsChanged (); 152 | } 153 | 154 | size_t 155 | SeedSelection::countSeedsWithLabel (uint32_t label) const 156 | { 157 | size_t count = 0; 158 | for (size_t i = 0; i < seeds_cloud_->size (); ++i) 159 | if (seeds_cloud_->points[i].label == label) 160 | ++count; 161 | return count; 162 | } 163 | 164 | -------------------------------------------------------------------------------- /gui/delineation/seed_selection.h: -------------------------------------------------------------------------------- 1 | #ifndef SEED_SELECTION_H 2 | #define SEED_SELECTION_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | class SeedSelection : public QAbstractListModel 13 | { 14 | 15 | Q_OBJECT 16 | 17 | public: 18 | 19 | typedef boost::shared_ptr Ptr; 20 | 21 | typedef pcl::PointCloud ColoredPointCloud; 22 | typedef ColoredPointCloud::Ptr ColoredPointCloudPtr; 23 | 24 | typedef pcl::PointCloud LabeledPointCloud; 25 | typedef LabeledPointCloud::Ptr LabeledPointCloudPtr; 26 | 27 | SeedSelection (QObject* parent = 0); 28 | 29 | ~SeedSelection (); 30 | 31 | virtual int 32 | rowCount (const QModelIndex&) const; 33 | 34 | virtual QVariant 35 | data (const QModelIndex& index, int role) const; 36 | 37 | void 38 | pickPoint (const pcl::PointXYZ& p); 39 | 40 | QModelIndex 41 | addNewLabel (); 42 | 43 | void 44 | deleteLabel (); 45 | 46 | LabeledPointCloudPtr 47 | getSelectedSeeds () 48 | { 49 | return seeds_cloud_; 50 | } 51 | 52 | void 53 | setSeeds (const LabeledPointCloud& seeds); 54 | 55 | ColoredPointCloudPtr 56 | getPointCloudForVisualization (); 57 | 58 | uint32_t getFirstCurrentLabel () const 59 | { 60 | return current_labels_.size () ? *current_labels_.cbegin () : 0; 61 | } 62 | 63 | uint32_t getLastCurrentLabel () const 64 | { 65 | return current_labels_.size () ? *current_labels_.crbegin () : num_labels_; 66 | } 67 | 68 | const std::set getCurrentLabels () const 69 | { 70 | return current_labels_; 71 | } 72 | 73 | size_t getNumLabels () const 74 | { 75 | return num_labels_; 76 | } 77 | 78 | public Q_SLOTS: 79 | 80 | void 81 | currentChanged (const QItemSelection& current, const QItemSelection& previous); 82 | 83 | Q_SIGNALS: 84 | 85 | /** This signal is triggered when the seeds cloud is changed. 86 | * 87 | * This includes addition/deletion of labels/seeds, as well as changes of 88 | * visualization colors when the user selects a label. */ 89 | void 90 | seedsChanged (); 91 | 92 | private: 93 | 94 | size_t 95 | countSeedsWithLabel (uint32_t label) const; 96 | 97 | LabeledPointCloudPtr seeds_cloud_; 98 | 99 | size_t num_labels_; 100 | 101 | std::set current_labels_; 102 | 103 | }; 104 | 105 | #endif /* SEED_SELECTION_H */ 106 | 107 | -------------------------------------------------------------------------------- /gui/delineation/seed_selection_form.cpp: -------------------------------------------------------------------------------- 1 | #include "tviewer/tviewer.h" 2 | #include "tviewer/tviewer_widget/tviewer_widget.h" 3 | 4 | #include "seed_selection.h" 5 | #include "seed_selection_form.h" 6 | #include "ui_seed_selection_form.h" 7 | 8 | using namespace tviewer; 9 | 10 | SeedSelectionForm::SeedSelectionForm (QWidget* parent) 11 | : QWidget (parent) 12 | , ui_ (new Ui::SeedSelectionForm) 13 | , seed_selection_ (new SeedSelection) 14 | , vertices_ (new PointCloud) 15 | , seeds_ (new PointCloudLabel) 16 | { 17 | ui_->setupUi (this); 18 | ui_->list_labels->setModel (seed_selection_.get ()); 19 | } 20 | 21 | SeedSelectionForm::~SeedSelectionForm () 22 | { 23 | delete ui_; 24 | } 25 | 26 | void 27 | SeedSelectionForm::enter () 28 | { 29 | if (input_graph_.changed () && input_graph_) 30 | { 31 | GraphPtr graph = input_graph_; 32 | pcl::copyPointCloud (*pcl::graph::point_cloud (*graph), *vertices_); 33 | for (size_t i = 0; i < vertices_->size (); ++i) 34 | vertices_->at (i).rgba = 0xFFF000; 35 | } 36 | if (input_seeds_.changed () || filter_predicate_.changed ()) 37 | { 38 | if (filter_predicate_) 39 | { 40 | std::function predicate = filter_predicate_; 41 | PointCloudLabel in = input_seeds_; 42 | std::vector indices; 43 | for (size_t i = 0; i < in.size (); ++i) 44 | { 45 | if (predicate (in.at (i).getVector3fMap ())) 46 | continue; 47 | indices.push_back (i); 48 | } 49 | pcl::copyPointCloud (in, indices, *seeds_); 50 | } 51 | else 52 | { 53 | pcl::copyPointCloud (input_seeds_, *seeds_); 54 | } 55 | seed_selection_->setSeeds (*seeds_); 56 | output_seeds_ = seeds_; 57 | } 58 | 59 | viewer_->add 60 | ( CreatePointCloudObject ("points", "p") 61 | . description ("Input point cloud") 62 | . data (vertices_) 63 | . pointSize (2) 64 | , true 65 | ); 66 | 67 | viewer_->add 68 | ( CreatePointCloudObject ("seeds", "s") 69 | . description ("Seeds") 70 | . pointSize (7) 71 | . onUpdate ([&] 72 | { 73 | return seed_selection_->getPointCloudForVisualization (); 74 | }) 75 | , true 76 | , true 77 | ); 78 | } 79 | 80 | void 81 | SeedSelectionForm::leave () 82 | { 83 | viewer_->remove ("seeds"); 84 | } 85 | 86 | void 87 | SeedSelectionForm::onButtonNewLabelClicked () 88 | { 89 | } 90 | 91 | -------------------------------------------------------------------------------- /gui/delineation/seed_selection_form.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_SEED_SELECTION_FORM_H 2 | #define GUI_DELINEATION_SEED_SELECTION_FORM_H 3 | 4 | #include 5 | 6 | #ifndef Q_MOC_RUN 7 | #include "types.h" 8 | #include "pipeline.h" 9 | #endif 10 | 11 | namespace Ui 12 | { 13 | class SeedSelectionForm; 14 | } 15 | 16 | class SeedSelection; 17 | 18 | class SeedSelectionForm : public QWidget, public pipeline::Stage 19 | { 20 | 21 | Q_OBJECT 22 | 23 | public: 24 | 25 | explicit SeedSelectionForm (QWidget* parent = 0); 26 | 27 | ~SeedSelectionForm (); 28 | 29 | virtual void 30 | enter () override; 31 | 32 | virtual void 33 | leave () override; 34 | 35 | public Q_SLOTS: 36 | 37 | void 38 | onButtonNewLabelClicked (); 39 | 40 | private: 41 | 42 | pipeline::Input input_graph_ = "Graph"; 43 | pipeline::Input input_seeds_ = "LoadedSeeds"; 44 | pipeline::Input> filter_predicate_ = "FilterPredicate"; 45 | pipeline::Output output_seeds_ = "SelectedSeeds"; 46 | 47 | Ui::SeedSelectionForm *ui_; 48 | 49 | std::unique_ptr seed_selection_; 50 | 51 | PointCloud::Ptr vertices_; 52 | PointCloudLabel::Ptr seeds_; 53 | 54 | }; 55 | 56 | #endif // GUI_DELINEATION_SEED_SELECTION_FORM_H 57 | -------------------------------------------------------------------------------- /gui/delineation/seed_selection_form.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | SeedSelectionForm 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 0 25 | 0 26 | 27 | 28 | 29 | 30 | 100 31 | 0 32 | 33 | 34 | 35 | QAbstractItemView::ExtendedSelection 36 | 37 | 38 | 39 | 40 | 41 | 42 | false 43 | 44 | 45 | New label 46 | 47 | 48 | 49 | 50 | 51 | 52 | false 53 | 54 | 55 | Delete label 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /gui/delineation/segmentation_form.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "tviewer/tviewer.h" 6 | #include "tviewer/tviewer_widget/tviewer_widget.h" 7 | 8 | #include "status_bar.h" 9 | #include "segmentation_form.h" 10 | #include "ui_segmentation_form.h" 11 | 12 | #include "random_walker_segmentation.h" 13 | 14 | using namespace tviewer; 15 | 16 | SegmentationForm::SegmentationForm (QWidget* parent) 17 | : QWidget (parent) 18 | , ui_ (new Ui::SegmentationForm) 19 | , seeds_ (new PointCloudLabel) 20 | , cluster_list_ (new ClusterListModel) 21 | { 22 | ui_->setupUi (this); 23 | ui_->list_clusters->setModel (cluster_list_.get ()); 24 | 25 | connect (ui_->list_clusters->selectionModel (), 26 | SIGNAL (selectionChanged (QItemSelection, QItemSelection)), 27 | cluster_list_.get (), 28 | SLOT (currentChanged (QItemSelection, QItemSelection))); 29 | connect (cluster_list_.get (), 30 | SIGNAL (selectionChanged ()), 31 | this, 32 | SLOT (onClusterSelectionChanged ())); 33 | } 34 | 35 | SegmentationForm::~SegmentationForm () 36 | { 37 | delete ui_; 38 | } 39 | 40 | void 41 | SegmentationForm::enter () 42 | { 43 | if (input_seeds_.changed () && input_seeds_) 44 | pcl::copyPointCloud (input_seeds_, *seeds_); 45 | 46 | viewer_->add 47 | ( CreatePointCloudObject ("seeds", "s") 48 | . description ("Seeds") 49 | . pointSize (7) 50 | . data (seeds_) 51 | , true 52 | ); 53 | 54 | viewer_->add 55 | ( CreatePointCloudObject ("trees", "t") 56 | . description ("Segmented trees") 57 | . pointSize (2) 58 | . onUpdate ([&] 59 | { 60 | return cluster_list_->getPointCloudForVisualization (); 61 | }) 62 | ); 63 | 64 | connect (viewer_, 65 | SIGNAL (pointPicked (QString, int)), 66 | this, 67 | SLOT (onPointPicked (QString, int))); 68 | } 69 | 70 | void 71 | SegmentationForm::leave () 72 | { 73 | viewer_->remove ("trees"); 74 | viewer_->hide ("seeds"); 75 | viewer_->disconnect (this); 76 | } 77 | 78 | void 79 | SegmentationForm::onButtonSegmentClicked () 80 | { 81 | if (!input_graph_) 82 | { 83 | QMessageBox::warning (this, "Missing graph", "A graph is required to perform segmentation.\n" 84 | "Visit \"Graph building\" form to create it."); 85 | return; 86 | } 87 | if (!input_seeds_) 88 | { 89 | QMessageBox::warning (this, "Missing seeds", "Seeds are required to perform segmentation.\n" 90 | "Load existing seeds using \"File\" menu, or visit \"Trunk detection\" form to create new seeds from the data."); 91 | return; 92 | } 93 | 94 | status_bar_->showMessage ("Segmenting graph..."); 95 | 96 | rws_.setInputGraph (input_graph_); 97 | rws_.setSeeds (input_seeds_); 98 | 99 | rws_.setClustersWithNoSeedsLabeling (ui_->checkbox_unique_labels->checkState () ? rws_.DIFFERENT_GENERATED_LABELS : rws_.SAME_GENERATED_LABEL); 100 | std::vector clusters; 101 | rws_.segment (clusters); 102 | 103 | PointCloud::Ptr vertices (new PointCloud); 104 | pcl::copyPointCloud (*pcl::graph::point_cloud (*rws_.getGraph ()), *vertices); 105 | cluster_list_->setClusterList (vertices, clusters); 106 | 107 | PointCloudLabel::Ptr labeled_vertices (new PointCloudLabel); 108 | pcl::copyPointCloud (*pcl::graph::point_cloud (*rws_.getGraph ()), *labeled_vertices); 109 | for (size_t i = 0; i < labeled_vertices->size (); ++i) 110 | labeled_vertices->at (i).label = boost::get (boost::vertex_color, *rws_.getGraph(), i); 111 | output_segmentation_ = labeled_vertices; 112 | 113 | viewer_->update ("trees"); 114 | viewer_->show ("trees"); 115 | 116 | status_bar_->showMessage ("Segmented graph into %i clusters", clusters.size () - 1); 117 | } 118 | 119 | void 120 | SegmentationForm::onClusterSelectionChanged () 121 | { 122 | viewer_->update ("trees"); 123 | } 124 | 125 | void 126 | SegmentationForm::onPointPicked (QString name, int index) 127 | { 128 | if (name == "trees") 129 | ui_->list_clusters->setCurrentIndex (cluster_list_->getIndexFromPointIndex (index)); 130 | } 131 | 132 | void 133 | SegmentationForm::loadConfig (Config& config) 134 | { 135 | config.get ("UniqueLabels", ui_->checkbox_unique_labels, 2); 136 | } 137 | 138 | void 139 | SegmentationForm::saveConfig (Config& config) const 140 | { 141 | config.put ("UniqueLabels", ui_->checkbox_unique_labels); 142 | } 143 | 144 | -------------------------------------------------------------------------------- /gui/delineation/segmentation_form.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_SEGMENTATION_FORM_H 2 | #define GUI_DELINEATION_SEGMENTATION_FORM_H 3 | 4 | #include 5 | 6 | #ifndef Q_MOC_RUN 7 | #include "types.h" 8 | #include "pipeline.h" 9 | #endif 10 | 11 | #include "cluster_list_model.h" 12 | 13 | namespace Ui 14 | { 15 | class SegmentationForm; 16 | } 17 | 18 | class SegmentationForm : public QWidget, public pipeline::Stage 19 | { 20 | 21 | Q_OBJECT 22 | 23 | public: 24 | 25 | explicit SegmentationForm (QWidget* parent = 0); 26 | 27 | ~SegmentationForm (); 28 | 29 | virtual void 30 | enter () override; 31 | 32 | virtual void 33 | leave () override; 34 | 35 | virtual void 36 | loadConfig (Config& config) override; 37 | 38 | virtual void 39 | saveConfig (Config& config) const override; 40 | 41 | public Q_SLOTS: 42 | 43 | void 44 | onButtonSegmentClicked (); 45 | 46 | void 47 | onClusterSelectionChanged (); 48 | 49 | void 50 | onPointPicked (QString name, int index); 51 | 52 | private: 53 | 54 | pipeline::Input input_graph_ = "Graph"; 55 | pipeline::Input input_seeds_ = "Seeds"; 56 | pipeline::Output output_segmentation_ = "Segmentation"; 57 | 58 | Ui::SegmentationForm *ui_; 59 | 60 | RandomWalkerSegmentation rws_; 61 | 62 | PointCloudLabel::Ptr seeds_; 63 | 64 | ClusterListModel::Ptr cluster_list_; 65 | 66 | }; 67 | 68 | #endif // GUI_DELINEATION_SEGMENTATION_FORM_H 69 | -------------------------------------------------------------------------------- /gui/delineation/segmentation_form.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | SegmentationForm 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 300 11 | 12 | 13 | 14 | Form 15 | 16 | 17 | 18 | 19 | 20 | Unique labels for "no-seeds" clusters 21 | 22 | 23 | true 24 | 25 | 26 | 27 | 28 | 29 | 30 | Segment 31 | 32 | 33 | 34 | 35 | 36 | 37 | 5 38 | 39 | 40 | QListView::IconMode 41 | 42 | 43 | 44 | 45 | 46 | 47 | Qt::Vertical 48 | 49 | 50 | 51 | 20 52 | 40 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | button_segment 63 | clicked() 64 | SegmentationForm 65 | onButtonSegmentClicked() 66 | 67 | 68 | 199 69 | 149 70 | 71 | 72 | 199 73 | 149 74 | 75 | 76 | 77 | 78 | 79 | onButtonSegmentClicked() 80 | 81 | 82 | -------------------------------------------------------------------------------- /gui/delineation/status_bar.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_STATUS_BAR_H 2 | #define GUI_DELINEATION_STATUS_BAR_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | /** Wrapper around Qt Status Bar with easy formatting support. */ 9 | class StatusBar 10 | { 11 | 12 | public: 13 | 14 | using Ptr = std::shared_ptr; 15 | 16 | StatusBar (QStatusBar* status_bar) 17 | : status_bar_ (status_bar) 18 | { 19 | } 20 | 21 | void 22 | showMessage (boost::format& message) 23 | { 24 | status_bar_->showMessage (message.str ().c_str ()); 25 | } 26 | 27 | template void 28 | showMessage (boost::format& message, TValue arg, TArgs... args) 29 | { 30 | message % arg; 31 | showMessage (message, args...); 32 | } 33 | 34 | template void 35 | showMessage (const std::string& fmt, TArgs... args) 36 | { 37 | boost::format message (fmt); 38 | showMessage (message, args...); 39 | } 40 | 41 | private: 42 | 43 | QStatusBar* status_bar_; 44 | 45 | }; 46 | 47 | 48 | #endif /* GUI_DELINEATION_STATUS_BAR_H */ 49 | 50 | -------------------------------------------------------------------------------- /gui/delineation/trunk_detection_form.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_TRUNK_DETECTION_FORM_H 2 | #define GUI_DELINEATION_TRUNK_DETECTION_FORM_H 3 | 4 | #include 5 | 6 | #ifndef Q_MOC_RUN 7 | #include "types.h" 8 | #include "pipeline.h" 9 | #endif 10 | 11 | #include "cluster_list_model.h" 12 | 13 | namespace Ui 14 | { 15 | class TrunkDetectionForm; 16 | } 17 | 18 | class TrunkDetectionForm : public QWidget, public pipeline::Stage 19 | { 20 | 21 | Q_OBJECT 22 | 23 | public: 24 | 25 | explicit TrunkDetectionForm (QWidget *parent = 0); 26 | 27 | ~TrunkDetectionForm (); 28 | 29 | virtual void 30 | loadConfig (Config& config) override; 31 | 32 | virtual void 33 | saveConfig (Config& config) const override; 34 | 35 | virtual void 36 | enter () override; 37 | 38 | virtual void 39 | leave () override; 40 | 41 | public Q_SLOTS: 42 | 43 | void 44 | onHeightFilterChanged (); 45 | 46 | void 47 | onButtonDetectClicked (); 48 | 49 | void 50 | onButtonFitClicked (); 51 | 52 | void 53 | onButtonEnrichClicked (); 54 | 55 | void 56 | onButtonUseAsSeedsClicked (); 57 | 58 | void 59 | onClusterSelectionChanged (); 60 | 61 | private: 62 | 63 | bool 64 | euclideanClusteringCondition (const Point& a, const Point& b, float dist); 65 | 66 | void 67 | updateTreeSeeds (); 68 | 69 | void 70 | performBottomUpTreeDetection (); 71 | 72 | void 73 | performTopDownTreeDetection (); 74 | 75 | pipeline::Input input_cloud_ = "LoadedCloud"; 76 | pipeline::Input> input_indices_ = "FilteredIndices"; 77 | pipeline::Output output_seeds_ = "Seeds"; 78 | 79 | Ui::TrunkDetectionForm *ui_; 80 | 81 | PointCloud::Ptr input_; 82 | PointCloudLabel::Ptr tree_seeds_; 83 | std::vector tree_cluster_indices_; 84 | std::vector tree_line_coefficients_; 85 | 86 | pcl::IndicesPtr filtered_indices_; 87 | 88 | ClusterListModel::Ptr cluster_list_; 89 | 90 | }; 91 | 92 | #endif // GUI_DELINEATION_TRUNK_DETECTION_FORM_H 93 | -------------------------------------------------------------------------------- /gui/delineation/types.h: -------------------------------------------------------------------------------- 1 | #ifndef GUI_DELINEATION_TYPES_H 2 | #define GUI_DELINEATION_TYPES_H 3 | 4 | #include 5 | #include 6 | 7 | #include "random_walker_segmentation.h" 8 | 9 | using Point = pcl::PointXYZRGB; 10 | using PointLabel = pcl::PointXYZL; 11 | using PointWithNormal = pcl::PointXYZRGBNormal; 12 | using PointCloud = pcl::PointCloud; 13 | using PointCloudLabel = pcl::PointCloud; 14 | using PointCloudWithNormal = pcl::PointCloud; 15 | 16 | using RandomWalkerSegmentation = pcl::segmentation::RandomWalkerSegmentation; 17 | using Graph = RandomWalkerSegmentation::Graph; 18 | using GraphPtr = RandomWalkerSegmentation::GraphPtr; 19 | using GraphConstPtr = RandomWalkerSegmentation::GraphConstPtr; 20 | 21 | #endif /* GUI_DELINEATION_TYPES_H */ 22 | 23 | -------------------------------------------------------------------------------- /gui/rws/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(GUI_RWS_SOURCES main.cpp main_window.cpp seed_selection.cpp) 2 | set(GUI_RWS_HEADERS main_window.h seed_selection.h) 3 | set(GUI_RWS_FORMS main_window.ui) 4 | 5 | qt_wrap_ui(GUI_RWS_FORMS_HEADERS ${GUI_RWS_FORMS}) 6 | 7 | add_executable(gui_random_walker_segmentation 8 | ${GUI_RWS_SOURCES} 9 | ${GUI_RWS_FORMS_HEADERS} 10 | ) 11 | target_link_libraries(gui_random_walker_segmentation 12 | io 13 | random_walker_segmentation 14 | ${QT_LIBRARIES} 15 | ${PCL_LIBRARIES} 16 | ${VTK_LIBRARIES} 17 | ${TVIEWER_LIBRARIES} 18 | ) 19 | add_dependencies(gui_random_walker_segmentation 20 | tviewer 21 | ) 22 | -------------------------------------------------------------------------------- /gui/rws/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "main_window.h" 8 | 9 | int 10 | main (int argc, char** argv) 11 | { 12 | if (argc < 2 || pcl::console::find_switch (argc, argv, "--help")) 13 | { 14 | pcl::console::print_error ("Usage: %s [seeds.pcd]" 15 | , argv[0]); 16 | return (1); 17 | } 18 | 19 | QApplication app (argc, argv); 20 | MainWindow w (argv[1]); 21 | 22 | if (argc > 2) 23 | w.loadSeeds (argv[2]); 24 | 25 | w.show (); 26 | return app.exec (); 27 | } 28 | 29 | -------------------------------------------------------------------------------- /gui/rws/main_window.h: -------------------------------------------------------------------------------- 1 | #ifndef MAIN_WINDOW_H 2 | #define MAIN_WINDOW_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #ifndef Q_MOC_RUN 15 | #include "seed_selection.h" 16 | #include "random_walker_segmentation.h" 17 | #endif 18 | 19 | namespace Ui 20 | { 21 | class MainWindow; 22 | } 23 | 24 | class MainWindow : public QMainWindow 25 | { 26 | Q_OBJECT 27 | 28 | public: 29 | 30 | typedef pcl::PointXYZRGB PointT; 31 | typedef pcl::PointXYZRGBNormal PointWithNormalT; 32 | typedef pcl::PointCloud PointCloudT; 33 | typedef pcl::PointCloud PointCloudWithNormalT; 34 | 35 | typedef pcl::segmentation::RandomWalkerSegmentation RandomWalkerSegmentation; 36 | typedef RandomWalkerSegmentation::Graph Graph; 37 | typedef RandomWalkerSegmentation::GraphPtr GraphPtr; 38 | typedef RandomWalkerSegmentation::GraphConstPtr GraphConstPtr; 39 | 40 | MainWindow (const std::string& filename, QWidget* parent = 0); 41 | 42 | ~MainWindow (); 43 | 44 | public Q_SLOTS: 45 | 46 | void 47 | onButtonUpdateGraphClicked (); 48 | 49 | void 50 | buttonNewLabelClicked (); 51 | 52 | void 53 | buttonDeleteLabelClicked (); 54 | 55 | void 56 | buttonSegmentClicked (); 57 | 58 | void 59 | checkboxDisplayStateChanged (int state) 60 | { 61 | displayGraphVertices (); 62 | displayGraphEdges (); 63 | } 64 | 65 | void 66 | onActionExitTriggered () 67 | { 68 | this->close (); 69 | } 70 | 71 | void 72 | onActionLoadViewpointTriggered () 73 | { 74 | if (boost::filesystem::exists ("viewpoint.cam")) 75 | viewer_->loadCameraParameters ("viewpoint.cam"); 76 | } 77 | 78 | void 79 | onActionSaveViewpointTriggered () 80 | { 81 | viewer_->saveCameraParameters ("viewpoint.cam"); 82 | } 83 | 84 | inline void 85 | onActionLoadSeedsTriggered () 86 | { 87 | loadSeeds (QFileDialog::getOpenFileName ().toStdString ()); 88 | } 89 | 90 | void 91 | onActionSaveSeedsTriggered () 92 | { 93 | pcl::io::savePCDFile ("seeds.pcd", *seed_selection_->getSelectedSeeds ()); 94 | } 95 | 96 | void 97 | onActionViewToggled (bool checked) 98 | { 99 | displayGraphVertices (); 100 | displayGraphEdges (); 101 | displaySeeds (); 102 | } 103 | 104 | void 105 | onActionSaveSegmentationTriggered (); 106 | 107 | void 108 | seedsChanged (); 109 | 110 | void 111 | onKeyUp (); 112 | 113 | void 114 | onKeyDown (); 115 | 116 | void 117 | loadSeeds (const std::string& filename); 118 | 119 | private: 120 | 121 | void 122 | pointPickingCallback (const pcl::visualization::PointPickingEvent& event, void*); 123 | 124 | void 125 | displayGraphVertices (); 126 | 127 | void 128 | displayGraphEdges (uint32_t color = 0); 129 | 130 | void 131 | displaySeeds (); 132 | 133 | void 134 | buildGraph (pcl::graph::GraphBuilder::Ptr graph_builder); 135 | 136 | void 137 | computeEdgeWeights (); 138 | 139 | void 140 | saveConfig (); 141 | 142 | void 143 | loadConfig (); 144 | 145 | enum GlobalState 146 | { 147 | GS_NOT_SEGMENTED, 148 | GS_SEGMENTED, 149 | }; 150 | 151 | void 152 | setGlobalState (GlobalState state); 153 | 154 | Ui::MainWindow* ui_; 155 | 156 | pcl::visualization::PCLVisualizer::Ptr viewer_; 157 | 158 | PointCloudT::Ptr cloud_; 159 | GraphPtr graph_; 160 | SeedSelection::Ptr seed_selection_; 161 | std::map colormap_; 162 | 163 | GlobalState state_; 164 | 165 | }; 166 | 167 | #endif // MAIN_WINDOW_H 168 | -------------------------------------------------------------------------------- /gui/rws/rws.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # Project created by QtCreator 2014-08-21T10:59:23 4 | # 5 | #------------------------------------------------- 6 | 7 | QT += core gui 8 | 9 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets 10 | 11 | TARGET = gui 12 | TEMPLATE = app 13 | 14 | SOURCES += main.cpp\ 15 | mainwindow.cpp \ 16 | seed_selection.cpp \ 17 | main_window.cpp 18 | 19 | HEADERS += mainwindow.h \ 20 | seed_selection.h \ 21 | main_window.h 22 | 23 | FORMS += main_window.ui 24 | -------------------------------------------------------------------------------- /gui/rws/seed_selection.cpp: -------------------------------------------------------------------------------- 1 | #include "seed_selection.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | SeedSelection::SeedSelection (QObject* parent) 8 | : QAbstractListModel (parent) 9 | , seeds_cloud_ (new LabeledPointCloud) 10 | , num_labels_ (0) 11 | { 12 | current_labels_.insert (0); 13 | } 14 | 15 | SeedSelection::~SeedSelection () 16 | { 17 | } 18 | 19 | int 20 | SeedSelection::rowCount (const QModelIndex&) const 21 | { 22 | return num_labels_; 23 | } 24 | 25 | QVariant 26 | SeedSelection::data (const QModelIndex& index, int role) const 27 | { 28 | if (role == Qt::DisplayRole) 29 | { 30 | uint32_t label = index.row () + 1; 31 | return QString ("%1 %2 seeds").arg (label).arg (countSeedsWithLabel (label)); 32 | } 33 | return QVariant (); 34 | } 35 | 36 | void 37 | SeedSelection::pickPoint (const pcl::PointXYZ& p) 38 | { 39 | using namespace pcl::utils; 40 | 41 | for (size_t i = 0; i < seeds_cloud_->size (); ++i) 42 | { 43 | pcl::PointXYZL& pt = seeds_cloud_->points[i]; 44 | if (equal (p.x, pt.x) && equal (p.y, pt.y) && equal (p.z, pt.z)) 45 | { 46 | if (current_labels_.count (pt.label)) 47 | { 48 | LabeledPointCloudPtr new_seeds (new LabeledPointCloud); 49 | for (size_t j = 0; j < seeds_cloud_->size (); ++j) 50 | if (j != i) 51 | new_seeds->push_back (seeds_cloud_->at (j)); 52 | seeds_cloud_.swap (new_seeds); 53 | seedsChanged (); 54 | dataChanged (QModelIndex (), QModelIndex ()); 55 | return; 56 | } 57 | else 58 | { 59 | // When multiple seed labels are selected do nothing 60 | if (current_labels_.size () == 1) 61 | { 62 | pt.label = *current_labels_.begin (); 63 | seedsChanged (); 64 | dataChanged (QModelIndex (), QModelIndex ()); 65 | } 66 | return; 67 | } 68 | } 69 | } 70 | 71 | // Picked a point which is not a seed. Make it a seed, but only in 72 | // single seed label mode 73 | if (current_labels_.size () == 1) 74 | { 75 | pcl::PointXYZL pt; 76 | pt.x = p.x, pt.y = p.y, pt.z = p.z, pt.label = *current_labels_.begin (); 77 | seeds_cloud_->push_back (pt); 78 | seedsChanged (); 79 | dataChanged (QModelIndex (), QModelIndex ()); 80 | } 81 | } 82 | 83 | QModelIndex 84 | SeedSelection::addNewLabel () 85 | { 86 | ++num_labels_; 87 | current_labels_.clear (); 88 | current_labels_.insert (num_labels_); 89 | dataChanged (QModelIndex (), QModelIndex ()); 90 | return index (num_labels_ - 1, 0); 91 | } 92 | 93 | void 94 | SeedSelection::deleteLabel () 95 | { 96 | // TODO: forbid deletion of multiple labels because it requires more complicated 97 | // label reassignment algorithm (to preserve sequential linear ordering). 98 | if (current_labels_.size () > 1) 99 | return; 100 | 101 | if (num_labels_ == current_labels_.size ()) 102 | return; 103 | 104 | LabeledPointCloudPtr new_seeds (new LabeledPointCloud); 105 | for (size_t i = 0; i < seeds_cloud_->size (); ++i) 106 | if (!current_labels_.count (seeds_cloud_->at (i).label)) 107 | { 108 | new_seeds->push_back (seeds_cloud_->at (i)); 109 | if (seeds_cloud_->at (i).label > *current_labels_.begin ()) 110 | --new_seeds->back ().label; 111 | } 112 | 113 | --num_labels_; 114 | seeds_cloud_.swap (new_seeds); 115 | dataChanged (QModelIndex (), QModelIndex ()); 116 | seedsChanged (); 117 | } 118 | 119 | void 120 | SeedSelection::setSeeds (const LabeledPointCloud& seeds) 121 | { 122 | pcl::copyPointCloud (seeds, *seeds_cloud_); 123 | num_labels_ = 0; 124 | for (size_t i = 0; i < seeds.size (); ++i) 125 | if (seeds[i].label > num_labels_) 126 | num_labels_ = seeds[i].label; 127 | current_labels_.clear (); 128 | current_labels_.insert (num_labels_); 129 | dataChanged (QModelIndex (), QModelIndex ()); 130 | seedsChanged (); 131 | } 132 | 133 | SeedSelection::ColoredPointCloudPtr 134 | SeedSelection::getPointCloudForVisualization () 135 | { 136 | ColoredPointCloudPtr cloud (new ColoredPointCloud); 137 | pcl::copyPointCloud (*seeds_cloud_, *cloud); 138 | for (size_t i = 0; i < cloud->size (); ++i) 139 | if (current_labels_.count (seeds_cloud_->at (i).label)) 140 | if (current_labels_.size () > 1) 141 | cloud->at (i).rgb = 0xFF0FFF; 142 | else 143 | cloud->at (i).rgb = 0xFFFFFF; 144 | else 145 | cloud->at (i).rgb = 0xFF0000; 146 | return cloud; 147 | } 148 | 149 | void 150 | SeedSelection::currentChanged (const QItemSelection& current, const QItemSelection& previous) 151 | { 152 | for (size_t i = 0; i < current.indexes ().size (); ++i) 153 | current_labels_.insert (current.indexes ().at (i).row () + 1); 154 | for (size_t i = 0; i < previous.indexes ().size (); ++i) 155 | current_labels_.erase (previous.indexes ().at (i).row () + 1); 156 | seedsChanged (); 157 | } 158 | 159 | size_t 160 | SeedSelection::countSeedsWithLabel (uint32_t label) const 161 | { 162 | size_t count = 0; 163 | for (size_t i = 0; i < seeds_cloud_->size (); ++i) 164 | if (seeds_cloud_->points[i].label == label) 165 | ++count; 166 | return count; 167 | } 168 | 169 | -------------------------------------------------------------------------------- /gui/rws/seed_selection.h: -------------------------------------------------------------------------------- 1 | #ifndef SEED_SELECTION_H 2 | #define SEED_SELECTION_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | class SeedSelection : public QAbstractListModel 13 | { 14 | 15 | Q_OBJECT 16 | 17 | public: 18 | 19 | typedef boost::shared_ptr Ptr; 20 | 21 | typedef pcl::PointCloud ColoredPointCloud; 22 | typedef ColoredPointCloud::Ptr ColoredPointCloudPtr; 23 | 24 | typedef pcl::PointCloud LabeledPointCloud; 25 | typedef LabeledPointCloud::Ptr LabeledPointCloudPtr; 26 | 27 | SeedSelection (QObject* parent = 0); 28 | 29 | ~SeedSelection (); 30 | 31 | virtual int 32 | rowCount (const QModelIndex&) const; 33 | 34 | virtual QVariant 35 | data (const QModelIndex& index, int role) const; 36 | 37 | void 38 | pickPoint (const pcl::PointXYZ& p); 39 | 40 | QModelIndex 41 | addNewLabel (); 42 | 43 | void 44 | deleteLabel (); 45 | 46 | LabeledPointCloudPtr 47 | getSelectedSeeds () 48 | { 49 | return seeds_cloud_; 50 | } 51 | 52 | void 53 | setSeeds (const LabeledPointCloud& seeds); 54 | 55 | ColoredPointCloudPtr 56 | getPointCloudForVisualization (); 57 | 58 | uint32_t getFirstCurrentLabel () const 59 | { 60 | return current_labels_.size () ? *current_labels_.cbegin () : 0; 61 | } 62 | 63 | uint32_t getLastCurrentLabel () const 64 | { 65 | return current_labels_.size () ? *current_labels_.crbegin () : num_labels_; 66 | } 67 | 68 | const std::set getCurrentLabels () const 69 | { 70 | return current_labels_; 71 | } 72 | 73 | size_t getNumLabels () const 74 | { 75 | return num_labels_; 76 | } 77 | 78 | public Q_SLOTS: 79 | 80 | void 81 | currentChanged (const QItemSelection& current, const QItemSelection& previous); 82 | 83 | Q_SIGNALS: 84 | 85 | /** This signal is triggered when the seeds cloud is changed. 86 | * 87 | * This includes addition/deletion of labels/seeds, as well as changes of 88 | * visualization colors when the user selects a label. */ 89 | void 90 | seedsChanged (); 91 | 92 | private: 93 | 94 | size_t 95 | countSeedsWithLabel (uint32_t label) const; 96 | 97 | LabeledPointCloudPtr seeds_cloud_; 98 | 99 | size_t num_labels_; 100 | 101 | std::set current_labels_; 102 | 103 | }; 104 | 105 | #endif /* SEED_SELECTION_H */ 106 | 107 | -------------------------------------------------------------------------------- /include/as_range.h: -------------------------------------------------------------------------------- 1 | #ifndef AS_RANGE_H 2 | #define AS_RANGE_H 3 | 4 | // Credits: http://stackoverflow.com/a/6175873/1525865 5 | 6 | template 7 | struct iterator_pair_range : std::pair 8 | { 9 | iterator_pair_range (std::pair const& x) 10 | : std::pair (x) 11 | { } 12 | Iterator begin () const { return this->first; } 13 | Iterator end () const { return this->second; } 14 | }; 15 | 16 | template 17 | inline iterator_pair_range as_range (std::pair const& x) 18 | { 19 | return iterator_pair_range (x); 20 | } 21 | 22 | #endif /* AS_RANGE_H */ 23 | 24 | -------------------------------------------------------------------------------- /include/conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef CONVERSIONS_H 2 | #define CONVERSIONS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "graph/point_cloud_graph.h" 12 | 13 | // Based on geom_utils.h from object discovery source code. 14 | 15 | template void 16 | meshToPointsAndNormals (const pcl::PolygonMesh::ConstPtr& mesh, 17 | typename pcl::PointCloud::Ptr& cloud, 18 | pcl::PointCloud::Ptr& normals) 19 | { 20 | // Unfold mesh, which is stored as ROS message 21 | pcl::fromPCLPointCloud2 (mesh->cloud, *cloud); 22 | std::vector counts (cloud->points.size (), 0); 23 | normals->points.resize (cloud->points.size ()); 24 | 25 | for (size_t i = 0; i < mesh->polygons.size (); i++) 26 | { 27 | const pcl::Vertices& vv = mesh->polygons[i]; 28 | 29 | // Get the 3 points 30 | int i1 = vv.vertices[0]; 31 | int i2 = vv.vertices[1]; 32 | int i3 = vv.vertices[2]; 33 | PointT& p1 = cloud->points[i1]; 34 | PointT& p2 = cloud->points[i2]; 35 | PointT& p3 = cloud->points[i3]; 36 | 37 | // Convert to eigen points 38 | Eigen::Vector3d pe1 (p1.x, p1.y, p1.z); 39 | Eigen::Vector3d pe2 (p2.x, p2.y, p2.z); 40 | Eigen::Vector3d pe3 (p3.x, p3.y, p3.z); 41 | 42 | // Find normal 43 | Eigen::Vector3d normal = (pe2 - pe1).cross (pe3 - pe1); 44 | normal = normal / normal.norm (); 45 | pcl::Normal pnormal (normal[0], normal[1], normal[2]); 46 | 47 | // Smoothly blend with the old normal estimate at this point. 48 | // Basically each face votes for the normal of the verteces around it. 49 | float v; 50 | pcl::Normal a; 51 | a = normals->points[i1]; 52 | v = 1.0 / (counts[i1] + 1.0); 53 | normals->points[i1] = pcl::Normal (v * pnormal.normal_x + (1.0 - v) * a.normal_x, 54 | v * pnormal.normal_y + (1.0 - v) * a.normal_y, 55 | v * pnormal.normal_z + (1.0 - v) * a.normal_z); 56 | a = normals->points[i2]; 57 | v = 1.0 / (counts[i2] + 1.0); 58 | normals->points[i2] = pcl::Normal (v * pnormal.normal_x + (1.0 - v) * a.normal_x, 59 | v * pnormal.normal_y + (1.0 - v) * a.normal_y, 60 | v * pnormal.normal_z + (1.0 - v) * a.normal_z); 61 | a = normals->points[i3]; 62 | v= 1.0 / (counts[i3] + 1.0); 63 | normals->points[i3] = pcl::Normal (v * pnormal.normal_x + (1.0 - v) * a.normal_x, 64 | v * pnormal.normal_y + (1.0 - v) * a.normal_y, 65 | v * pnormal.normal_z + (1.0 - v) * a.normal_z); 66 | counts[i1]++; 67 | counts[i2]++; 68 | counts[i3]++; 69 | } 70 | } 71 | 72 | template void 73 | mesh2graph (const std::string& filename, Graph& graph) 74 | { 75 | typedef typename boost::graph_traits::vertex_descriptor VertexId; 76 | typedef typename boost::graph_traits::edge_iterator EdgeIterator; 77 | 78 | vtkSmartPointer poly_data = vtkSmartPointer::New (); 79 | vtkSmartPointer ply_reader = vtkSmartPointer::New (); 80 | ply_reader->SetFileName (filename.c_str ()); 81 | ply_reader->Update (); 82 | poly_data = ply_reader->GetOutput (); 83 | 84 | size_t num_points = poly_data->GetNumberOfPoints (); 85 | 86 | // First get the xyz information 87 | graph = Graph (num_points); 88 | for (VertexId i = 0; i < num_points; i++) 89 | { 90 | double point_xyz[3]; 91 | poly_data->GetPoint (i, &point_xyz[0]); 92 | graph[i].getVector3fMap () = Eigen::Vector3f (point_xyz[0], point_xyz[1], point_xyz[2]); 93 | } 94 | 95 | // Then the color information, if any 96 | vtkUnsignedCharArray* poly_colors = NULL; 97 | if (poly_data->GetPointData() != NULL) 98 | poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB")); 99 | if (poly_colors && (poly_colors->GetNumberOfComponents () == 3)) 100 | { 101 | for (VertexId i = 0; i < num_points; i++) 102 | { 103 | uint8_t color[3]; 104 | poly_colors->GetTupleValue (i, color); 105 | graph[i].getBGRVector3cMap () << color[2], color[1], color[0]; 106 | } 107 | } 108 | 109 | // Now handle the polygons 110 | vtkIdType* cell_points; 111 | vtkIdType num_cell_points; 112 | vtkCellArray* mesh_polygons = poly_data->GetPolys (); 113 | mesh_polygons->InitTraversal (); 114 | int id_poly = 0; 115 | while (mesh_polygons->GetNextCell (num_cell_points, cell_points)) 116 | { 117 | VertexId i1 = cell_points[0]; 118 | VertexId i2 = cell_points[1]; 119 | VertexId i3 = cell_points[2]; 120 | const Eigen::Vector3f& p1 = graph[i1].getVector3fMap (); 121 | const Eigen::Vector3f& p2 = graph[i2].getVector3fMap (); 122 | const Eigen::Vector3f& p3 = graph[i3].getVector3fMap (); 123 | 124 | Eigen::Vector3f normal = (p2 - p1).cross (p3 - p1); 125 | normal.normalize (); 126 | 127 | graph[i1].getNormalVector3fMap () += normal; 128 | graph[i2].getNormalVector3fMap () += normal; 129 | graph[i3].getNormalVector3fMap () += normal; 130 | 131 | if (!boost::edge (i1, i2, graph).second) 132 | boost::add_edge (i1, i2, graph); 133 | if (!boost::edge (i2, i3, graph).second) 134 | boost::add_edge (i2, i3, graph); 135 | if (!boost::edge (i3, i1, graph).second) 136 | boost::add_edge (i3, i1, graph); 137 | ++id_poly; 138 | } 139 | 140 | for (const auto& vertex : as_range (boost::vertices (graph))) 141 | graph[vertex].getNormalVector3fMap ().normalize (); 142 | 143 | EdgeIterator ei, ee; 144 | for (boost::tie (ei, ee) = boost::edges (graph); ei != ee; ++ei) 145 | { 146 | const VertexId& i1 = boost::source (*ei, graph); 147 | const VertexId& i2 = boost::target (*ei, graph); 148 | const Eigen::Vector3f& p = graph[i1].getVector3fMap (); 149 | const Eigen::Vector3f& q = graph[i2].getVector3fMap (); 150 | const Eigen::Vector3f& np = graph[i1].getNormalVector3fMap (); 151 | const Eigen::Vector3f& nq = graph[i2].getNormalVector3fMap (); 152 | Eigen::Vector3f x = p - q; 153 | float d1 = nq.dot (x); 154 | float d2 = np.dot (-x); 155 | graph[i1].curvature += d1; 156 | graph[i2].curvature += d2; 157 | } 158 | 159 | for (const auto& vertex : as_range (boost::vertices (graph))) 160 | graph[vertex].curvature /= boost::out_degree (vertex, graph) * 0.001; 161 | } 162 | 163 | #endif /* CONVERSIONS_H */ 164 | 165 | -------------------------------------------------------------------------------- /include/factory/edge_weight_computer_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FACTORY_EDGE_WEIGHT_COMPUTER_FACTORY_H 2 | #define FACTORY_EDGE_WEIGHT_COMPUTER_FACTORY_H 3 | 4 | #include "factory.h" 5 | #include "graph/edge_weight_computer.h" 6 | 7 | namespace factory 8 | { 9 | 10 | template 11 | class EdgeWeightComputerFactory : public Factory 12 | { 13 | 14 | public: 15 | 16 | typedef pcl::graph::EdgeWeightComputer EdgeWeightComputer; 17 | 18 | EdgeWeightComputerFactory () 19 | : Factory ("Edge Weight Computer") 20 | , xyz_influence_ ("xyz influence", "-z", 3.0f) 21 | , normal_influence_ ("normal influence", "-n", 0.01f) 22 | , curvature_influence_ ("curvature influence", "-b", 0.0001f) 23 | , color_influence_ ("color influence", "-c", 3.0f) 24 | , verticality_influence_ ("verticality influence", "-V", 1.0f) 25 | , weight_threshold_ ("weight threshold", "--weight-threshold", 0.00001f) 26 | , convex_multiplier_ ("convex multiplier", "--convex-multiplier", 0.01f) 27 | { 28 | add (&xyz_influence_); 29 | add (&normal_influence_); 30 | add (&curvature_influence_); 31 | add (&color_influence_); 32 | add (&verticality_influence_); 33 | add (&weight_threshold_); 34 | add (&convex_multiplier_); 35 | } 36 | 37 | typename EdgeWeightComputer::Ptr 38 | instantiate (int argc, char** argv) 39 | { 40 | using namespace pcl::graph::terms; 41 | parse (argc, argv); 42 | typename EdgeWeightComputer::Ptr ewc (new EdgeWeightComputer); 43 | ewc->template addTerm (xyz_influence_, EdgeWeightComputer::NORMALIZATION_LOCAL); 44 | ewc->template addTerm (normal_influence_, convex_multiplier_); 45 | ewc->template addTerm (curvature_influence_, convex_multiplier_); 46 | ewc->template addTerm (color_influence_, EdgeWeightComputer::NORMALIZATION_GLOBAL); 47 | ewc->template addTerm (verticality_influence_); 48 | ewc->setSmallWeightThreshold (weight_threshold_); 49 | ewc->setSmallWeightPolicy (EdgeWeightComputer::SMALL_WEIGHT_COERCE_TO_THRESHOLD); 50 | return ewc; 51 | } 52 | 53 | private: 54 | 55 | NumericOption xyz_influence_; 56 | NumericOption normal_influence_; 57 | NumericOption curvature_influence_; 58 | NumericOption color_influence_; 59 | NumericOption verticality_influence_; 60 | NumericOption weight_threshold_; 61 | NumericOption convex_multiplier_; 62 | 63 | }; 64 | 65 | } 66 | 67 | #endif /* FACTORY_EDGE_WEIGHT_COMPUTER_FACTORY_H */ 68 | 69 | -------------------------------------------------------------------------------- /include/factory/factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FACTORY_FACTORY_H 2 | #define FACTORY_FACTORY_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace factory 9 | { 10 | 11 | namespace printer 12 | { 13 | 14 | void 15 | startSection (const std::string& name = "") 16 | { 17 | using namespace pcl::console; 18 | change_text_color (stdout, TT_RESET, TT_MAGENTA); 19 | std::cout << (boost::format ("%=44s\n") % name); 20 | reset_text_color (stdout); 21 | std::cout << std::setfill ('-') << std::setw (44) << "" << std::endl; 22 | } 23 | 24 | void 25 | endSection () 26 | { 27 | std::cout << std::setfill ('-') << std::setw (44) << "" << std::endl; 28 | } 29 | 30 | template void 31 | printValue (const std::string& name, const T& value) 32 | { 33 | pcl::console::print_info ("%21s : ", name.c_str ()); 34 | pcl::console::print_value ("%s\n", boost::lexical_cast (value).c_str ()); 35 | } 36 | 37 | } 38 | 39 | struct Option 40 | { 41 | 42 | typedef std::pair ValueInfo; 43 | 44 | Option (const std::string& desc, const std::string& k) 45 | : description (desc) 46 | , key (k) 47 | { 48 | } 49 | 50 | virtual void 51 | parse (int argc, char** argv) = 0; 52 | 53 | virtual std::vector 54 | getValueInfo () = 0; 55 | 56 | virtual std::string 57 | getDescription () { return description; } 58 | 59 | virtual std::string 60 | getKey () { return key; } 61 | 62 | std::string description; 63 | std::string key; 64 | 65 | }; 66 | 67 | template 68 | struct NumericOption : Option 69 | { 70 | 71 | typedef T value_t; 72 | 73 | NumericOption (const std::string& desc, const std::string& key, value_t default_value) : Option (desc, key), value (default_value) { } 74 | 75 | operator value_t () { return value; } 76 | 77 | virtual void parse (int argc, char** argv) { pcl::console::parse (argc, argv, key.c_str (), value); } 78 | 79 | virtual std::vector getValueInfo () { return { std::make_pair (description, boost::lexical_cast (value)) }; } 80 | 81 | value_t value; 82 | 83 | }; 84 | 85 | struct BoolOption : Option 86 | { 87 | 88 | typedef bool value_t; 89 | 90 | BoolOption (const std::string& desc, const std::string& key) : Option (desc, key), value (false) { } 91 | 92 | operator value_t () { return value; } 93 | 94 | virtual void parse (int argc, char** argv) { value = pcl::console::find_switch (argc, argv, key.c_str ()); } 95 | 96 | virtual std::vector getValueInfo () { return { std::make_pair (description, value ? "ON" : "OFF") }; } 97 | 98 | value_t value; 99 | 100 | }; 101 | 102 | struct EnumOption : Option 103 | { 104 | 105 | typedef std::string value_t; 106 | typedef std::vector> list_t; 107 | 108 | EnumOption (const std::string& desc, const std::string& key, const list_t& vars) 109 | : Option (desc, key) 110 | , variants (vars) 111 | , value (vars[0].first) 112 | { 113 | } 114 | 115 | operator value_t () 116 | { 117 | return value; 118 | } 119 | 120 | virtual void 121 | parse (int argc, char** argv) 122 | { 123 | pcl::console::parse (argc, argv, key.c_str (), value); 124 | for (const auto& p : variants) 125 | if (p.first == value) 126 | return; 127 | pcl::console::print_warn ("Invalid value for %s option: \"%s\". Defaulting to \"%s\"\n", key.c_str (), value.c_str (), variants[0].first.c_str ()); 128 | } 129 | 130 | virtual std::vector 131 | getValueInfo () 132 | { 133 | for (const auto& p : variants) 134 | if (p.first == value) 135 | return { std::make_pair (description, p.second) }; 136 | return { std::make_pair (description, "UNKNOWN") }; 137 | } 138 | 139 | virtual std::string 140 | getDescription () 141 | { 142 | std::vector names; 143 | for (const auto& p : variants) 144 | names.push_back (p.first); 145 | return description + " [" + boost::algorithm::join (names, "|") + "]"; 146 | } 147 | 148 | list_t variants; 149 | value_t value; 150 | 151 | }; 152 | 153 | 154 | class Factory 155 | { 156 | 157 | public: 158 | 159 | Factory (const std::string& name) : name_ (name) { }; 160 | 161 | virtual const std::string 162 | getUsage () 163 | { 164 | std::stringstream usage; 165 | usage << name_ << " options: "; 166 | boost::format fmt ("\n %s <%s>"); 167 | for (const auto& option : options_) 168 | usage << boost::str (fmt % option->getKey () % option->getDescription ()); 169 | return usage.str (); 170 | } 171 | 172 | virtual void 173 | printValues () 174 | { 175 | printer::startSection (name_); 176 | for (const auto& option : options_) 177 | for (const auto& value : option->getValueInfo ()) 178 | printer::printValue (value.first, value.second); 179 | printer::endSection (); 180 | } 181 | 182 | protected: 183 | 184 | inline void 185 | add (Option* option) 186 | { 187 | options_.push_back (option); 188 | } 189 | 190 | inline void 191 | parse (int argc, char** argv) 192 | { 193 | for (const auto& option : options_) 194 | option->parse (argc, argv); 195 | } 196 | 197 | private: 198 | 199 | std::vector options_; 200 | std::string name_; 201 | 202 | }; 203 | 204 | } 205 | 206 | #endif /* FACTORY_FACTORY_H */ 207 | 208 | -------------------------------------------------------------------------------- /include/factory/graph_builder_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FACTORY_GRAPH_BUILDER_FACTORY_H 2 | #define FACTORY_GRAPH_BUILDER_FACTORY_H 3 | 4 | #include "factory.h" 5 | #include "graph/graph_builder.h" 6 | #include "graph/nearest_neighbors_graph_builder.h" 7 | #include "graph/voxel_grid_graph_builder.h" 8 | 9 | namespace factory 10 | { 11 | 12 | template 13 | class GraphBuilderFactory : public Factory 14 | { 15 | 16 | public: 17 | 18 | typedef pcl::graph::GraphBuilder GraphBuilderT; 19 | 20 | GraphBuilderFactory () 21 | : Factory ("Graph Builder") 22 | , builder_ ("builder type", "--builder", { { "vg", "VOXEL GRID" } 23 | , { "nnk", "NEAREST NEIGHBORS KNN" } 24 | , { "nnr", "NEAREST NEIGHBORS RADIUS"} }) 25 | , voxel_resolution_ ("voxel resolution", "-v", 0.006f) 26 | , number_of_neighbors_ ("number of neighbors", "--nn", 14) 27 | , radius_ ("sphere radius", "--radius", 0.006f) 28 | , no_transform_ ("no transform", "-nt") 29 | { 30 | add (&builder_); 31 | add (&voxel_resolution_); 32 | add (&number_of_neighbors_); 33 | add (&radius_); 34 | add (&no_transform_); 35 | } 36 | 37 | typename GraphBuilderT::Ptr 38 | instantiate (int argc, char** argv) 39 | { 40 | parse (argc, argv); 41 | typename GraphBuilderT::Ptr gb; 42 | if (builder_.value == "vg") 43 | { 44 | gb.reset (new pcl::graph::VoxelGridGraphBuilder (voxel_resolution_)); 45 | } 46 | else if (builder_.value == "nnk") 47 | { 48 | auto nngb = new pcl::graph::NearestNeighborsGraphBuilder; 49 | nngb->setNumberOfNeighbors (number_of_neighbors_); 50 | nngb->useNearestKSearch (); 51 | gb.reset (nngb); 52 | } 53 | else 54 | { 55 | auto nngb = new pcl::graph::NearestNeighborsGraphBuilder; 56 | nngb->setNumberOfNeighbors (number_of_neighbors_); 57 | nngb->setRadius (radius_); 58 | nngb->useRadiusSearch (); 59 | gb.reset (nngb); 60 | } 61 | return gb; 62 | } 63 | 64 | private: 65 | 66 | EnumOption builder_; 67 | NumericOption voxel_resolution_; 68 | NumericOption number_of_neighbors_; 69 | NumericOption radius_; 70 | BoolOption no_transform_; 71 | 72 | }; 73 | 74 | } 75 | 76 | #endif /* FACTORY_GRAPH_BUILDER_FACTORY_H */ 77 | 78 | -------------------------------------------------------------------------------- /include/factory/graph_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FACTORY_GRAPH_FACTORY_H 2 | #define FACTORY_GRAPH_FACTORY_H 3 | 4 | #include 5 | 6 | #include "factory.h" 7 | #include "graph_builder_factory.h" 8 | #include "edge_weight_computer_factory.h" 9 | 10 | #include "graph/common.h" 11 | 12 | #include "measure_runtime.h" 13 | 14 | namespace factory 15 | { 16 | 17 | template 18 | class GraphFactory : public Factory 19 | { 20 | 21 | public: 22 | 23 | typedef boost::shared_ptr GraphPtr; 24 | typedef boost::reference_wrapper GraphRef; 25 | typedef std::vector GraphRefVector; 26 | typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr; 27 | typedef typename pcl::graph::GraphBuilder::Ptr GraphBuilderPtr; 28 | 29 | GraphFactory () 30 | : Factory ("Graph") 31 | , component_ ("connected component", "--component", -1) 32 | , smoothing_spatial_ ("smoothing spatial", "--smoothing-spatial", 0.012) 33 | , smoothing_influence_ ("smoothing influence", "--smoothing-influence", 0.0012) 34 | { 35 | add (&component_); 36 | add (&smoothing_spatial_); 37 | add (&smoothing_influence_); 38 | } 39 | 40 | virtual const std::string 41 | getUsage () 42 | { 43 | std::stringstream usage; 44 | usage << Factory::getUsage () << std::endl; 45 | usage << gb_factory_.getUsage () << std::endl; 46 | usage << wc_factory_.getUsage (); 47 | return usage.str (); 48 | } 49 | 50 | virtual void 51 | printValues () 52 | { 53 | Factory::printValues (); 54 | gb_factory_.printValues (); 55 | wc_factory_.printValues (); 56 | if (produced_graph_) 57 | { 58 | pcl::console::print_info ("Total graph size: %zu / %zu\n", 59 | boost::num_vertices (*produced_graph_), 60 | boost::num_edges (*produced_graph_)); 61 | } 62 | } 63 | 64 | GraphRef 65 | instantiate (const PointCloudConstPtr& cloud, int argc, char** argv) 66 | { 67 | parse (argc, argv); 68 | gb_ = gb_factory_.instantiate (argc, argv); 69 | auto wc = wc_factory_.instantiate (argc, argv); 70 | // Build graph 71 | produced_graph_.reset (new Graph); 72 | gb_->setInputCloud (cloud); 73 | MEASURE_RUNTIME ("Building graph... ", 74 | gb_->compute (*produced_graph_)); 75 | MEASURE_RUNTIME ("Computing normals... ", 76 | pcl::graph::computeNormalsAndCurvatures (*produced_graph_)); 77 | MEASURE_RUNTIME ("Smoothening graph... ", 78 | pcl::graph::smoothen (*produced_graph_, smoothing_spatial_, smoothing_influence_)); 79 | MEASURE_RUNTIME ("Re-computing normals... ", 80 | pcl::graph::computeNormalsAndCurvatures (*produced_graph_)); 81 | MEASURE_RUNTIME ("Computing curvature signs... ", 82 | pcl::graph::computeSignedCurvatures (*produced_graph_)); 83 | MEASURE_RUNTIME ("Computing edge weights... ", 84 | wc->compute (*produced_graph_)); 85 | MEASURE_RUNTIME ("Computing connected components... ", 86 | pcl::graph::createSubgraphsFromConnectedComponents (*produced_graph_, components_)); 87 | if (component_ != -1 && component_ < static_cast (components_.size ())) 88 | return components_[component_]; 89 | return GraphRef (*produced_graph_); 90 | } 91 | 92 | GraphPtr 93 | getProducedGraph () 94 | { 95 | return produced_graph_; 96 | } 97 | 98 | GraphRefVector& 99 | getProducedGraphComponents () 100 | { 101 | return components_; 102 | } 103 | 104 | GraphBuilderPtr 105 | getGraphBuilder () 106 | { 107 | return gb_; 108 | } 109 | 110 | private: 111 | 112 | NumericOption component_; 113 | NumericOption smoothing_spatial_; 114 | NumericOption smoothing_influence_; 115 | 116 | EdgeWeightComputerFactory wc_factory_; 117 | GraphBuilderFactory gb_factory_; 118 | 119 | GraphBuilderPtr gb_; 120 | 121 | GraphPtr produced_graph_; 122 | GraphRefVector components_; 123 | 124 | }; 125 | 126 | } 127 | 128 | #endif /* FACTORY_GRAPH_FACTORY_H */ 129 | 130 | -------------------------------------------------------------------------------- /include/graph/edge_weight_computer_terms.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_EDGE_WEIGHT_COMPUTER_TERMS_H 39 | #define PCL_GRAPH_EDGE_WEIGHT_COMPUTER_TERMS_H 40 | 41 | #include 42 | 43 | namespace pcl 44 | { 45 | 46 | namespace graph 47 | { 48 | 49 | namespace terms 50 | { 51 | 52 | /** Squared Euclidean distance between points. 53 | * 54 | * \f[ 55 | * d_{xyz}(v_i,v_j) = ||p_i-p_j||^2 56 | * \f] 57 | * 58 | * Requires that the point type has *x*, *y*, and *z* fields. 59 | * 60 | * \ingroup graph 61 | * \author Sergey Alexandrov 62 | */ 63 | struct XYZ 64 | { 65 | typedef pcl::traits::has_xyz is_compatible; 66 | 67 | template float 68 | static compute (const PointT& p1, const PointT& p2) 69 | { 70 | return (p2.getVector3fMap () - p1.getVector3fMap ()).squaredNorm (); 71 | } 72 | }; 73 | 74 | /** Angular distance between normals. 75 | * 76 | * \f[ 77 | * d_{normal}(v_i,v_j) = \frac{||n_i-n_j||^2}{2} 78 | * \f] 79 | * 80 | * Requires that the point type has *normal_x*, *normal_y*, and 81 | * *normal_z* fields. 82 | * 83 | * \ingroup graph 84 | * \author Sergey Alexandrov 85 | */ 86 | struct Normal 87 | { 88 | typedef pcl::traits::has_normal is_compatible; 89 | 90 | template float 91 | static compute (const PointT& p1, const PointT& p2) 92 | { 93 | return (0.5 * (p1.getNormalVector3fMap () - p2.getNormalVector3fMap ()).squaredNorm ()); 94 | } 95 | }; 96 | 97 | /** Product of curvatures. 98 | * 99 | * \f[ 100 | * d_{curvature}(v_i,v_j) = c_i \cdot c_j 101 | * \f] 102 | * 103 | * Requires that the point type has *curvature* field. 104 | * 105 | * \ingroup graph 106 | * \author Sergey Alexandrov 107 | */ 108 | struct Curvature 109 | { 110 | typedef pcl::traits::has_curvature is_compatible; 111 | 112 | template float 113 | static compute (const PointT& p1, const PointT& p2) 114 | { 115 | return (std::fabs (p1.curvature) * std::fabs (p2.curvature)); 116 | } 117 | }; 118 | 119 | /** Squared Euclidean distance in RGB space. 120 | * 121 | * \f[ 122 | * d_{xyz}(v_i,v_j) = ||rgb_i-rgb_j||^2 123 | * \f] 124 | * 125 | * Requires that the point type has *rgb* or *rgba* field. 126 | * 127 | * \ingroup graph 128 | * \author Sergey Alexandrov 129 | */ 130 | struct RGB 131 | { 132 | typedef pcl::traits::has_color is_compatible; 133 | 134 | template float 135 | static compute (const PointT& p1, const PointT& p2) 136 | { 137 | return ((p1.getBGRVector3cMap ().template cast () - 138 | p2.getBGRVector3cMap ().template cast ()).norm () / 255.0f); 139 | } 140 | }; 141 | 142 | struct Verticality 143 | { 144 | typedef pcl::traits::has_xyz is_compatible; 145 | 146 | template float 147 | static compute (const PointT& p1, const PointT& p2) 148 | { 149 | return (1.0f - std::abs((p2.getVector3fMap () - p1.getVector3fMap ()).normalized()[2])); 150 | } 151 | }; 152 | 153 | } 154 | 155 | } 156 | 157 | } 158 | 159 | #endif /* PCL_GRAPH_EDGE_WEIGHT_COMPUTER_TERMS_H */ 160 | 161 | -------------------------------------------------------------------------------- /include/graph/graph_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_GRAPH_BUILDER_H 39 | #define PCL_GRAPH_GRAPH_BUILDER_H 40 | 41 | #include 42 | 43 | #include 44 | 45 | #include "graph/point_cloud_graph.h" 46 | #include "graph/point_cloud_graph_concept.h" 47 | 48 | namespace pcl 49 | { 50 | 51 | namespace graph 52 | { 53 | 54 | /** This is an abstract base class for building a BGL-compatible point cloud 55 | * graph from a point cloud. 56 | * 57 | * Building a graph involves creating vertices and establishing edges 58 | * between them. (A particular algorithm for doing these depends on the 59 | * extending class.) 60 | * 61 | * The two template parameters are the type of points in input cloud and 62 | * the output graph type. The graph type should be a model of 63 | * \ref concepts::PointCloudGraphConcept (i.e. either point_cloud_graph or 64 | * \c boost::subgraph wrapped around the former). The type of input points 65 | * and points bundled in graph vertices do not need to be the same. 66 | * 67 | * \author Sergey Alexandrov 68 | * \ingroup graph */ 69 | template 70 | class PCL_EXPORTS GraphBuilder : public pcl::PCLBase 71 | { 72 | 73 | BOOST_CONCEPT_ASSERT ((pcl::graph::PointCloudGraphConcept)); 74 | 75 | public: 76 | 77 | typedef boost::shared_ptr > Ptr; 78 | 79 | /// Type of points in the input cloud. 80 | typedef PointT PointInT; 81 | /// Type of points in the output graph. 82 | typedef typename point_cloud_graph_traits::point_type PointOutT; 83 | 84 | typedef typename boost::graph_traits::vertex_descriptor VertexId; 85 | 86 | /** Build a graph based on the provided input data. */ 87 | virtual void 88 | compute (GraphT& graph) = 0; 89 | 90 | /** Get a mapping between points in the input cloud and the vertices in 91 | * the output graph. 92 | * 93 | * \warning Some points may have no corresponding vertex. This happens 94 | * e.g. for NaN points, or if the user intentionally excluded some 95 | * points from graph building process by providing indices vector with 96 | * setIndices(). For such points the "nil" vertex id will be assigned, 97 | * which is equal to std::numeric_limits::max (). */ 98 | const std::vector& 99 | getPointToVertexMap () const 100 | { 101 | return (point_to_vertex_map_); 102 | } 103 | 104 | protected: 105 | 106 | std::vector point_to_vertex_map_; 107 | 108 | }; 109 | 110 | } 111 | 112 | } 113 | 114 | #endif /* PCL_GRAPH_GRAPH_BUILDER_H */ 115 | 116 | -------------------------------------------------------------------------------- /include/graph/impl/nearest_neighbors_graph_builder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_IMPL_NEAREST_NEIGHBORS_GRAPH_BUILDER_HPP 39 | #define PCL_GRAPH_IMPL_NEAREST_NEIGHBORS_GRAPH_BUILDER_HPP 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "graph/nearest_neighbors_graph_builder.h" 48 | 49 | template void 50 | pcl::graph::NearestNeighborsGraphBuilder::compute (GraphT& graph) 51 | { 52 | if (!initCompute ()) 53 | { 54 | graph = GraphT (); 55 | deinitCompute (); 56 | return; 57 | } 58 | 59 | size_t k = 0; 60 | for (size_t i = 0; i < indices_->size (); ++i) 61 | { 62 | if (!pcl::isFinite (input_->operator[] (indices_->operator[] (i)))) 63 | fake_indices_ = false; 64 | else 65 | indices_->operator[] (k++) = indices_->operator[] (i); 66 | } 67 | indices_->resize (k); 68 | 69 | // Create a new point cloud which will be the basis for the constructed graph. 70 | // All the fields that are also present in the output point type will be 71 | // copied over from the original point cloud. 72 | typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 73 | pcl::copyPointCloud (*input_, *indices_, *cloud); 74 | graph = GraphT (cloud); 75 | 76 | // In case a search method has not been given, initialize it using defaults 77 | if (!search_) 78 | { 79 | // For organized datasets, use OrganizedNeighbor 80 | if (cloud->isOrganized ()) 81 | search_.reset (new pcl::search::OrganizedNeighbor); 82 | // For unorganized data, use KdTree 83 | else 84 | search_.reset (new pcl::search::KdTree); 85 | } 86 | 87 | // Establish edges with nearest neighbors. 88 | std::vector neighbors (num_neighbors_ + 1); 89 | std::vector distances (num_neighbors_ + 1); 90 | search_->setInputCloud (cloud); 91 | for (size_t i = 0; i < cloud->size (); ++i) 92 | { 93 | switch (search_type_) 94 | { 95 | case KNN: 96 | { 97 | // Search for num_neighbors_ + 1 because the first neighbor output by KdTree 98 | // is always the query point itself. 99 | search_->nearestKSearch (i, num_neighbors_ + 1, neighbors, distances); 100 | break; 101 | } 102 | case RADIUS: 103 | { 104 | search_->radiusSearch (i, radius_, neighbors, distances, num_neighbors_ + 1); 105 | break; 106 | } 107 | } 108 | for (size_t j = 1; j < neighbors.size (); ++j) 109 | if (!boost::edge (i, neighbors[j], graph).second) 110 | boost::add_edge (i, neighbors[j], graph); 111 | } 112 | 113 | // Create point to vertex map 114 | point_to_vertex_map_.resize (input_->size (), std::numeric_limits::max ()); 115 | VertexId v = 0; 116 | for (size_t i = 0; i < indices_->size (); ++i) 117 | point_to_vertex_map_[indices_->operator[] (i)] = v++; 118 | } 119 | 120 | #endif /* PCL_GRAPH_IMPL_NEAREST_NEIGHBORS_GRAPH_BUILDER_HPP */ 121 | 122 | -------------------------------------------------------------------------------- /include/graph/impl/voxel_grid_graph_builder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_IMPL_VOXEL_GRID_GRAPH_BUILDER_HPP 39 | #define PCL_GRAPH_IMPL_VOXEL_GRID_GRAPH_BUILDER_HPP 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "graph/voxel_grid_graph_builder.h" 49 | 50 | /* The function below is required in order to use boost::unordered_map with 51 | * pcl::octree::OctreeKey key type. It simply hashes the x, y, z array of 52 | * indices, because it uniquely defines the key. */ 53 | 54 | namespace pcl 55 | { 56 | 57 | namespace octree 58 | { 59 | 60 | #pragma clang diagnostic push 61 | #pragma clang diagnostic ignored "-Wunused-function" 62 | 63 | static size_t hash_value (const OctreeKey& b) 64 | { 65 | return boost::hash_value (b.key_); 66 | } 67 | 68 | #pragma clang diagnostic pop 69 | 70 | } 71 | 72 | } 73 | 74 | 75 | template void 76 | pcl::graph::VoxelGridGraphBuilder::compute (GraphT& graph) 77 | { 78 | if (!initCompute ()) 79 | { 80 | graph = GraphT (); 81 | deinitCompute (); 82 | return; 83 | } 84 | 85 | typename pcl::PointCloud::Ptr transformed (new pcl::PointCloud); 86 | pcl::copyPointCloud (*input_, *transformed); 87 | for (size_t i = 0; i < transformed->size (); ++i) 88 | { 89 | PointT& p = transformed->points[i]; 90 | p.x /= p.z; 91 | p.y /= p.z; 92 | p.z = std::log (p.z); 93 | } 94 | 95 | Eigen::Vector4f min, max; 96 | pcl::getMinMax3D (*transformed, *indices_, min, max); 97 | 98 | // Create and initialize an Octree that stores point indices 99 | typedef pcl::octree::OctreePointCloud Octree; 100 | Octree octree (voxel_resolution_); 101 | octree.defineBoundingBox (min (0), min (1), min (2), max (0), max (1), max (2)); 102 | octree.setInputCloud (transformed, indices_); 103 | octree.addPointsFromInputCloud (); 104 | 105 | graph = GraphT (octree.getLeafCount ()); 106 | 107 | typedef boost::unordered_map KeyVertexMap; 108 | KeyVertexMap key_to_vertex_map; 109 | 110 | point_to_vertex_map_.clear (); 111 | point_to_vertex_map_.resize (transformed->size (), std::numeric_limits::max ()); 112 | 113 | typename Octree::LeafNodeIterator leaf_itr = octree.leaf_begin (); 114 | for (VertexId v = 0; leaf_itr != octree.leaf_end (); ++leaf_itr, ++v) 115 | { 116 | // Step 1: compute leaf centroid and fill in corresponding elements of the 117 | // point to vertex map. 118 | pcl::CentroidPoint centroid; 119 | std::vector& indices = leaf_itr.getLeafContainer ().getPointIndicesVector (); 120 | for (size_t i = 0; i < indices.size (); ++i) 121 | { 122 | centroid.add (input_->operator[] (indices[i])); 123 | point_to_vertex_map_[indices[i]] = v; 124 | } 125 | centroid.get (graph[v]); 126 | 127 | // Step 2: fill in octree key to vertex map. 128 | octree::OctreeKey key = leaf_itr.getCurrentOctreeKey (); 129 | key_to_vertex_map[key] = v; 130 | 131 | // Step 2: find neighbors and insert edges. 132 | octree::OctreeKey neighbor_key; 133 | for (int dx = (key.x > 0) ? -1 : 0; dx <= 1; ++dx) 134 | { 135 | neighbor_key.x = static_cast (key.x + dx); 136 | for (int dy = (key.y > 0) ? -1 : 0; dy <= 1; ++dy) 137 | { 138 | neighbor_key.y = static_cast (key.y + dy); 139 | for (int dz = (key.z > 0) ? -1 : 0; dz <= 1; ++dz) 140 | { 141 | neighbor_key.z = static_cast (key.z + dz); 142 | typename KeyVertexMap::iterator f = key_to_vertex_map.find (neighbor_key); 143 | if (f != key_to_vertex_map.end () && v != f->second) 144 | boost::add_edge (v, f->second, graph); 145 | } 146 | } 147 | } 148 | } 149 | } 150 | 151 | #endif /* PCL_GRAPH_IMPL_VOXEL_GRID_GRAPH_BUILDER_HPP */ 152 | 153 | -------------------------------------------------------------------------------- /include/graph/point_cloud_graph_concept.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_POINT_CLOUD_GRAPH_CONCEPT_H 39 | #define PCL_GRAPH_POINT_CLOUD_GRAPH_CONCEPT_H 40 | 41 | #include 42 | #include 43 | 44 | namespace pcl 45 | { 46 | 47 | namespace graph 48 | { 49 | 50 | namespace concepts 51 | { 52 | 53 | /** \class pcl::graph::concepts::PointCloudGraphConcept 54 | * 55 | * A PointCloudGraph is a graph that has PCL points bundled in 56 | * vertices and can be viewed as a PCL point cloud (without data-copying). 57 | * 58 | * This concept is a refinement of [PropertyGraph]. Please refer to 59 | * [BGL concepts][GraphConcepts] for a complete list of graph-related 60 | * concepts in boost. 61 | * 62 | * [PropertyGraph]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/PropertyGraph.html 63 | * [GraphConcepts]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/graph_concepts.html 64 | * 65 | * 66 | * ## Notation ## 67 | * 68 | * 69 | * - `G` a type that is a model of PointCloudGraph 70 | * - `g` an object of type `G` 71 | * 72 | * 73 | * ## Associated types ## 74 | * 75 | * 76 | * - `pcl::graph::point_cloud_graph_traits::point_type` 77 | * 78 | * The type of PCL points bundled in vertices. 79 | * 80 | * - `pcl::graph::point_cloud_graph_traits::point_cloud_type` 81 | * 82 | * The type of PCL point cloud this graph can be viewed as. 83 | * 84 | * - `pcl::graph::point_cloud_graph_traits::point_cloud_ptr` 85 | * 86 | * The type of a shared pointer to PCL point cloud this graph can be 87 | * viewed as. 88 | * 89 | * - `pcl::graph::point_cloud_graph_traits::point_cloud_const_ptr` 90 | * 91 | * The type of a shared pointer to const PCL point cloud this graph can 92 | * be viewed as. 93 | * 94 | * 95 | * ## Valid expressions ## 96 | * 97 | * - `G (point_cloud_ptr)` 98 | * - \ref pcl::graph::point_cloud() "pcl::graph::point_cloud (g)" 99 | * - \ref pcl::graph::indices() "pcl::graph::indices (g)" 100 | * 101 | * 102 | * ## Models ## 103 | * 104 | * 105 | * - `pcl::graph::point_cloud_graph` 106 | * - `boost::subgraph` 107 | * 108 | * \author Sergey Alexandrov 109 | * \ingroup graph 110 | */ 111 | 112 | BOOST_concept (PointCloudGraph, (G)) 113 | : boost::concepts::Graph 114 | { 115 | 116 | typedef typename boost::vertex_bundle_type::type vertex_bundled; 117 | typedef typename boost::graph_traits::vertex_descriptor vertex_descriptor; 118 | typedef typename point_cloud_graph_traits::point_type point_type; 119 | typedef typename point_cloud_graph_traits::point_cloud_type point_cloud_type; 120 | typedef typename point_cloud_graph_traits::point_cloud_ptr point_cloud_ptr; 121 | typedef typename point_cloud_graph_traits::point_cloud_const_ptr point_cloud_const_ptr; 122 | 123 | BOOST_STATIC_ASSERT ((boost::mpl::not_ >::value)); 124 | BOOST_STATIC_ASSERT ((boost::is_same::value)); 125 | 126 | BOOST_CONCEPT_USAGE (PointCloudGraph) 127 | { 128 | BOOST_CONCEPT_ASSERT ((boost::concepts::PropertyGraph)); 129 | p = point_cloud (g); 130 | i = indices (g); 131 | G a (p); // require that graph can be constructed from a point cloud pointer 132 | const_constraints (g); 133 | } 134 | 135 | void const_constraints (const G& cg) 136 | { 137 | pc = point_cloud (cg); 138 | i = indices (cg); 139 | } 140 | 141 | G g; 142 | point_cloud_ptr p; 143 | point_cloud_const_ptr pc; 144 | pcl::PointIndices::Ptr i; 145 | 146 | }; 147 | 148 | } // namespace concepts 149 | 150 | using pcl::graph::concepts::PointCloudGraphConcept; 151 | 152 | } // namespace graph 153 | 154 | } // namespace pcl 155 | 156 | #include 157 | 158 | #endif /* PCL_GRAPH_POINT_CLOUD_GRAPH_CONCEPT_H */ 159 | 160 | -------------------------------------------------------------------------------- /include/graph/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_UTILS_H 39 | #define PCL_GRAPH_UTILS_H 40 | 41 | #include 42 | 43 | namespace pcl 44 | { 45 | 46 | namespace graph 47 | { 48 | 49 | namespace detail 50 | { 51 | // Create a meta-function that checks if a type has "graph_type" member 52 | // typedef. This will be used to distinguish between boost::subgraph (does 53 | // have), and normal graph types (do not have). 54 | BOOST_MPL_HAS_XXX_TRAIT_NAMED_DEF(has_root_graph, graph_type, false) 55 | } 56 | 57 | /** remove_edge_if structure is an "extended" version of, 58 | * boost::remove_edge_if that incorporates a workaround to allow edge 59 | * removal from both plain graphs and subgraphs. 60 | * 61 | * Plain graphs should be passed to boost::remove_edge_if as is, whereas 62 | * member field `m_graph` should be used in place of the subgraphs 63 | * (otherwise expect segfaults). */ 64 | template 65 | struct remove_edge_if 66 | { 67 | template void 68 | operator () (const Predicate& predicate, Graph& graph) const 69 | { 70 | boost::remove_edge_if (predicate, graph); 71 | } 72 | }; 73 | 74 | template 75 | struct remove_edge_if >::type> 76 | { 77 | template void 78 | operator () (const Predicate& predicate, Graph& graph) const 79 | { 80 | boost::remove_edge_if (predicate, graph.m_graph); 81 | } 82 | }; 83 | 84 | } 85 | 86 | } 87 | 88 | #endif /* PCL_GRAPH_UTILS_H */ 89 | 90 | -------------------------------------------------------------------------------- /include/graph/voxel_grid_graph_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_GRAPH_VOXEL_GRID_GRAPH_BUILDER_H 39 | #define PCL_GRAPH_VOXEL_GRID_GRAPH_BUILDER_H 40 | 41 | #include "graph/graph_builder.h" 42 | 43 | namespace pcl 44 | { 45 | 46 | namespace graph 47 | { 48 | 49 | /** This class builds a BGL graph representing an input dataset by using 50 | * octree::OctreePointCloud. 51 | * 52 | * For additional information see documentation for \ref GraphBuilder. 53 | * 54 | * \author Sergey Alexandrov 55 | * \ingroup graph */ 56 | template 57 | class PCL_EXPORTS VoxelGridGraphBuilder : public GraphBuilder 58 | { 59 | 60 | using PCLBase::initCompute; 61 | using PCLBase::deinitCompute; 62 | using PCLBase::indices_; 63 | using PCLBase::input_; 64 | using GraphBuilder::point_to_vertex_map_; 65 | 66 | public: 67 | 68 | using typename GraphBuilder::PointInT; 69 | using typename GraphBuilder::PointOutT; 70 | using typename GraphBuilder::VertexId; 71 | 72 | /** Constructor. 73 | * 74 | * \param[in] voxel_resolution resolution of the voxel grid */ 75 | VoxelGridGraphBuilder (float voxel_resolution) 76 | : voxel_resolution_ (voxel_resolution) 77 | { 78 | } 79 | 80 | virtual void 81 | compute (GraphT& graph); 82 | 83 | inline void 84 | setVoxelResolution (float resolution) 85 | { 86 | voxel_resolution_ = resolution; 87 | } 88 | 89 | inline float 90 | getVoxelResolution () const 91 | { 92 | return (voxel_resolution_); 93 | } 94 | 95 | private: 96 | 97 | /// Resolution of the voxel grid. 98 | float voxel_resolution_; 99 | 100 | }; 101 | 102 | } 103 | 104 | } 105 | 106 | #include "graph/impl/voxel_grid_graph_builder.hpp" 107 | 108 | #endif /* PCL_GRAPH_VOXEL_GRID_GRAPH_BUILDER_H */ 109 | 110 | -------------------------------------------------------------------------------- /include/hungarian/hungarian.h: -------------------------------------------------------------------------------- 1 | #ifndef HUNGARIAN_HUNGARIAN_H 2 | #define HUNGARIAN_HUNGARIAN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | std::map 13 | findAssignment (const std::vector>& triples); 14 | 15 | std::map 16 | findAssignment (const Eigen::MatrixXi& weights); 17 | 18 | #endif /* HUNGARIAN_HUNGARIAN_H */ 19 | 20 | -------------------------------------------------------------------------------- /include/impl/io.hpp: -------------------------------------------------------------------------------- 1 | #ifndef IO_HPP 2 | #define IO_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "graph/point_cloud_graph.h" 11 | #include "as_range.h" 12 | 13 | template bool 14 | saveGraph (const std::string& filename, 15 | const Graph& graph) 16 | { 17 | pcl::console::print_highlight ("Saving to file \"%s\"... ", filename.c_str ()); 18 | if (pcl::io::savePCDFile (filename, *pcl::graph::point_cloud (graph))) 19 | { 20 | pcl::console::print_error ("error!\n"); 21 | return false; 22 | } 23 | 24 | std::ofstream file (filename, std::fstream::app); 25 | if (!file.is_open ()) 26 | { 27 | pcl::console::print_error ("error!\n"); 28 | return false; 29 | } 30 | 31 | file << "# Edges\n"; 32 | auto weights = boost::get (boost::edge_weight, graph); 33 | for (const auto& edge : as_range (boost::edges (graph))) 34 | { 35 | auto src = boost::source (edge, graph); 36 | auto tgt = boost::target (edge, graph); 37 | auto wgh = weights[edge]; 38 | file << src << " " << tgt << " " << wgh << "\n"; 39 | } 40 | file.close(); 41 | 42 | pcl::console::print_info ("%zu vertices, %zu edges.\n", 43 | boost::num_vertices (graph), 44 | boost::num_edges (graph)); 45 | 46 | return true; 47 | } 48 | 49 | template bool 50 | loadGraph (const std::string& filename, 51 | Graph& graph) 52 | { 53 | typedef typename pcl::graph::point_cloud_graph_traits::point_type PointT; 54 | typedef pcl::PointCloud PointCloudT; 55 | 56 | typename PointCloudT::Ptr cloud (new PointCloudT); 57 | 58 | pcl::console::print_highlight ("Loading from file \"%s\"... ", filename.c_str ()); 59 | if (pcl::io::loadPCDFile (filename, *cloud)) 60 | { 61 | pcl::console::print_error ("error!\n"); 62 | return false; 63 | } 64 | 65 | std::ifstream stream (filename); 66 | if (!stream) 67 | { 68 | pcl::console::print_error ("error!\n"); 69 | return false; 70 | } 71 | 72 | graph = Graph (cloud); 73 | 74 | std::string line; 75 | bool edges_section = false; 76 | while (getline (stream, line)) 77 | { 78 | if (!edges_section) 79 | { 80 | if (line == "# Edges") 81 | edges_section = true; 82 | } 83 | else 84 | { 85 | if (!line.size () || line[0] == '#') 86 | continue; 87 | int src; 88 | int tgt; 89 | float wgh; 90 | std::stringstream (line) >> src >> tgt >> wgh; 91 | boost::add_edge (src, tgt, wgh, graph); 92 | } 93 | } 94 | 95 | pcl::console::print_info ("%zu vertices, %zu edges.\n", 96 | boost::num_vertices (graph), 97 | boost::num_edges (graph)); 98 | 99 | stream.close (); 100 | return true; 101 | } 102 | 103 | #endif /* IO_HPP */ 104 | 105 | -------------------------------------------------------------------------------- /include/io.h: -------------------------------------------------------------------------------- 1 | #ifndef IO_H 2 | #define IO_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | template bool 10 | load (const std::string& filename, 11 | typename pcl::PointCloud::Ptr cloud, 12 | pcl::PointCloud::Ptr normals = pcl::PointCloud::Ptr ()); 13 | 14 | bool 15 | hasColor (const std::string& filename); 16 | 17 | template bool 18 | saveGraph (const std::string& filename, 19 | const Graph& graph); 20 | 21 | template bool 22 | loadGraph (const std::string& filename, 23 | Graph& graph); 24 | 25 | void 26 | saveSparseMatrix (const std::string& filename, 27 | const Eigen::SparseMatrix& M); 28 | 29 | void 30 | loadSparseMatrix (const std::string& filename, 31 | Eigen::SparseMatrix& M); 32 | 33 | #include "impl/io.hpp" 34 | 35 | #endif /* IO_H */ 36 | 37 | -------------------------------------------------------------------------------- /include/kde.h: -------------------------------------------------------------------------------- 1 | #ifndef KDE_H 2 | #define KDE_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | class KDE 12 | { 13 | 14 | public: 15 | 16 | KDE (std::vector&& dataset); 17 | 18 | void 19 | setBandwidth (float bandwidth); 20 | 21 | float 22 | evaluateAt (float x, float y) const; 23 | 24 | std::vector 25 | evaluateAt (std::vector& query) const; 26 | 27 | private: 28 | 29 | using KdTree = flann::Index>; 30 | 31 | std::unique_ptr kdtree_; 32 | std::vector dataset_; 33 | std::function kernel_; 34 | float bandwidth_; 35 | float radius_; 36 | 37 | }; 38 | 39 | #endif /* KDE_H */ 40 | 41 | -------------------------------------------------------------------------------- /include/label_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef LABEL_UTILS_H 2 | #define LABEL_UTILS_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace labels 11 | { 12 | 13 | template pcl::PointCloud::Ptr 14 | createLabeledCloudFromColorMap (const typename pcl::PointCloud& cloud, 15 | const std::vector::key_type>& point_to_vertex_map, 16 | ColorMap color_map) 17 | { 18 | typedef typename boost::property_traits::key_type VertexId; 19 | const VertexId NIL = std::numeric_limits::max (); 20 | 21 | pcl::PointCloud::Ptr labeled (new pcl::PointCloud); 22 | pcl::copyPointCloud (cloud, *labeled); 23 | 24 | for (size_t i = 0; i < labeled->size (); ++i) 25 | { 26 | const VertexId& v = point_to_vertex_map[i]; 27 | labeled->at (i).label = (v == NIL) ? 0 : color_map[v]; 28 | } 29 | 30 | return labeled; 31 | } 32 | 33 | /** Creates a new cloud of PointXYZL points with labels that match colors. 34 | * 35 | * The source cloud is copied over. NaN points get 0 label, finite points 36 | * get labels from 1 up depending on their RGBA color. */ 37 | template pcl::PointCloud::Ptr 38 | createLabeledCloudFromColoredCloud (const typename pcl::PointCloud& colored) 39 | { 40 | pcl::PointCloud::Ptr labeled (new pcl::PointCloud); 41 | pcl::copyPointCloud (colored, *labeled); 42 | 43 | std::map color_label_map; 44 | for (size_t i = 0; i < labeled->size (); ++i) 45 | { 46 | if (pcl::isFinite (colored[i])) 47 | { 48 | auto color = colored[i].rgba; 49 | if (color_label_map.count (color) == 0) 50 | color_label_map.insert (std::make_pair (color, color_label_map.size () + 1)); 51 | labeled->at (i).label = color_label_map[color]; 52 | } 53 | } 54 | 55 | return labeled; 56 | } 57 | 58 | } 59 | 60 | 61 | #endif /* LABEL_UTILS_H */ 62 | 63 | -------------------------------------------------------------------------------- /include/measure_runtime.h: -------------------------------------------------------------------------------- 1 | #ifndef MEASURE_RUNTIME_H 2 | #define MEASURE_RUNTIME_H 3 | 4 | #include 5 | #include 6 | 7 | #define MEASURE_RUNTIME(description, function) \ 8 | { \ 9 | pcl::console::TicToc tt; \ 10 | pcl::console::print_info ((description)); \ 11 | tt.tic (); \ 12 | (function); \ 13 | pcl::console::print_value (" [%g ms]\n", tt.toc ()); \ 14 | } 15 | 16 | #define MEASURE_RUNTIME_MULTILINE(description, function) \ 17 | { \ 18 | pcl::console::TicToc tt; \ 19 | pcl::console::print_info ((description " >>>\n")); \ 20 | tt.tic (); \ 21 | (function); \ 22 | pcl::console::print_info (">>> done with %s ", description); \ 23 | pcl::console::print_value ("[%g ms]\n", tt.toc ()); \ 24 | } 25 | 26 | #endif /* MEASURE_RUNTIME_H */ 27 | 28 | -------------------------------------------------------------------------------- /include/mesh_grid.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_GRID_H 2 | #define MESH_GRID_H 3 | 4 | #include 5 | #include 6 | 7 | class MeshGrid 8 | { 9 | 10 | public: 11 | 12 | MeshGrid (float min_x, float max_x, float min_y, float max_y, float cell_size); 13 | 14 | MeshGrid (float min_x, float max_x, float min_y, float max_y, float cell_size_x, float cell_size_y); 15 | 16 | std::vector 17 | getCellCenters (); 18 | 19 | vtkSmartPointer 20 | createPolyData (float z = 0.0); 21 | 22 | vtkSmartPointer 23 | createPolyData (std::vector colors, float z = 0.0); 24 | 25 | private: 26 | 27 | float min_x_; 28 | float max_x_; 29 | float min_y_; 30 | float max_y_; 31 | float cell_size_x_; 32 | float cell_size_y_; 33 | size_t num_cells_x_; 34 | size_t num_cells_y_; 35 | 36 | }; 37 | 38 | #endif /* MESH_GRID_H */ 39 | 40 | -------------------------------------------------------------------------------- /include/scaler.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALER_H 2 | #define SCALER_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | typedef std::function Scaler; 9 | 10 | Scaler getScaler (float min, float max) 11 | { 12 | if (!std::isfinite (min) || !std::isfinite (max)) 13 | throw std::runtime_error ("requested scaler for collection with infinite elements"); 14 | if (max != min) 15 | return [min, max] (float v) { return (v - min) / (max - min); }; 16 | else 17 | return [] (float) { return 1.0; }; 18 | } 19 | 20 | template 21 | Scaler getScaler (const T& a) 22 | { 23 | float min = a.min (); 24 | float max = a.max (); 25 | return getScaler (min, max); 26 | } 27 | 28 | template <> 29 | Scaler getScaler (const Eigen::VectorXf& a) 30 | { 31 | float min = a.minCoeff (); 32 | float max = a.maxCoeff (); 33 | return getScaler (min, max); 34 | } 35 | 36 | template <> 37 | Scaler getScaler (const std::vector& a) 38 | { 39 | float min = *std::min_element (a.begin (), a.end ()); 40 | float max = *std::max_element (a.begin (), a.end ()); 41 | return getScaler (min, max); 42 | } 43 | 44 | #endif /* SCALER_H */ 45 | 46 | -------------------------------------------------------------------------------- /include/seed_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef SEED_UTILS_H 2 | #define SEED_UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "tviewer/color.h" 10 | 11 | typedef pcl::PointXYZL SeedT; 12 | typedef pcl::PointCloud SeedCloudT; 13 | typedef SeedCloudT::Ptr SeedCloudTPtr; 14 | 15 | namespace seeds 16 | { 17 | 18 | typename pcl::PointCloud::Ptr 19 | createColoredCloudFromSeeds (const SeedCloudT& seeds) 20 | { 21 | typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 22 | cloud->resize (seeds.size ()); 23 | std::map color_map; 24 | for (size_t i = 0; i < seeds.size (); ++i) 25 | { 26 | const auto& s = seeds.at (i); 27 | auto& p = cloud->at (i); 28 | p.x = s.x; 29 | p.y = s.y; 30 | p.z = s.z; 31 | if (color_map.count (s.label) == 0) 32 | color_map[s.label] = tviewer::generateRandomColor (); 33 | p.rgba = color_map[s.label]; 34 | } 35 | return cloud; 36 | } 37 | 38 | /* For each point in the given cloud create a seed in the output cloud. 39 | * Each seed gets a distinct label starting from 1. */ 40 | template typename SeedCloudT::Ptr 41 | createSeedCloudFromPointCloud (const pcl::PointCloud& cloud) 42 | { 43 | typename SeedCloudT::Ptr seeds (new SeedCloudT); 44 | seeds->resize (cloud.size ()); 45 | uint32_t label = 1; 46 | for (size_t i = 0; i < cloud.size (); ++i) 47 | { 48 | seeds->at (i).getVector3fMap () = cloud.at (i).getVector3fMap (); 49 | seeds->at (i).label = label++; 50 | } 51 | return seeds; 52 | } 53 | 54 | /* For each distinct label in the seeds cloud this function will create 55 | * PointIndices with the indices of points of the original cloud that 56 | * have exactly the same coordinates (up to epsilon) as seeds with this 57 | * label. The return value is true only if a corresponding point index 58 | * was found for every seed. */ 59 | template bool 60 | findSeedIndicesInCloud (const typename pcl::PointCloud::Ptr& cloud, 61 | const typename SeedCloudT::Ptr& seeds, 62 | std::vector& indices) 63 | { 64 | using namespace pcl::utils; 65 | std::map label_map; 66 | indices.clear (); 67 | bool all_found = true; 68 | for (const auto& seed : seeds->points) 69 | { 70 | if (label_map.count (seed.label) == 0) 71 | { 72 | label_map[seed.label] = indices.size (); 73 | indices.push_back (pcl::PointIndices ()); 74 | } 75 | size_t group = label_map[seed.label]; 76 | bool point_found = false; 77 | for (size_t i = 0; i < cloud->points.size (); ++i) 78 | { 79 | const auto& pt = cloud->points[i]; 80 | const float EPS = 0.00001; 81 | if (equal (seed.x, pt.x, EPS) && equal (seed.y, pt.y, EPS) && equal (seed.z, pt.z, EPS)) 82 | { 83 | indices[group].indices.push_back (i); 84 | point_found = true; 85 | break; 86 | } 87 | } 88 | all_found &= point_found; 89 | } 90 | return all_found; 91 | } 92 | 93 | /* For each distinct label in the seeds cloud this function will create 94 | * PointIndices with the indices of points of the original cloud that 95 | * have approximately the same coordinates as seeds with this label. The 96 | * return value is always true. */ 97 | template bool 98 | findSeedIndicesInCloudApproximate (const typename pcl::PointCloud::Ptr& cloud, 99 | const typename SeedCloudT::Ptr& seeds, 100 | std::vector& indices) 101 | { 102 | indices.clear (); 103 | std::vector v; 104 | typename pcl::PointCloud::Ptr seeds_copy (new pcl::PointCloud); 105 | pcl::copyPointCloud (*seeds, *seeds_copy); 106 | pcl::getApproximateIndices (seeds_copy, cloud, v); 107 | assert (seeds->size () == v.size ()); 108 | std::map label_map; 109 | for (size_t i = 0; i < seeds->size (); ++i) 110 | { 111 | const auto& seed = seeds->at (i); 112 | if (label_map.count (seed.label) == 0) 113 | { 114 | label_map[seed.label] = indices.size (); 115 | indices.push_back (pcl::PointIndices ()); 116 | } 117 | indices[label_map[seed.label]].indices.push_back (v[i]); 118 | } 119 | return true; 120 | } 121 | 122 | } 123 | 124 | #endif /* SEED_UTILS_H */ 125 | 126 | -------------------------------------------------------------------------------- /include/tree_top_detector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "types.h" 6 | 7 | class TreeTopDetector 8 | { 9 | 10 | public: 11 | 12 | TreeTopDetector (); 13 | 14 | void 15 | setInputCloud (typename PointCloud::ConstPtr cloud); 16 | 17 | void 18 | setRadius (float radius); 19 | 20 | virtual ~TreeTopDetector() { } 21 | 22 | void 23 | detect (std::vector& tree_top_indices); 24 | 25 | const std::vector& 26 | getDensity () const; 27 | 28 | private: 29 | 30 | PointCloud::ConstPtr input_; 31 | float cell_size_; 32 | std::vector density_; 33 | 34 | struct Candidate 35 | { 36 | int cell_x, cell_y; 37 | int index; 38 | float z; 39 | }; 40 | 41 | std::queue candidates_; 42 | 43 | float radius_; 44 | int cell_radius_; 45 | 46 | }; 47 | -------------------------------------------------------------------------------- /include/typedefs.h: -------------------------------------------------------------------------------- 1 | #ifndef TYPEDEFS_H 2 | #define TYPEDEFS_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "graph/point_cloud_graph.h" 10 | 11 | typedef pcl::PointXYZRGBA PointT; 12 | typedef pcl::PointXYZRGBNormal PointWithNormalT; 13 | typedef pcl::Normal NormalT; 14 | 15 | typedef pcl::PointCloud PointCloudT; 16 | typedef pcl::PointCloud PointCloudWithNormalT; 17 | typedef pcl::PointCloud NormalCloudT; 18 | typedef typename PointCloudT::Ptr PointCloudTPtr; 19 | typedef typename PointCloudWithNormalT::Ptr PointCloudWithNormalTPtr; 20 | typedef typename NormalCloudT::Ptr NormalCloudTPtr; 21 | 22 | #ifndef WC_SMALL_WEIGHT_POLICY 23 | #define WC_SMALL_WEIGHT_POLICY policy::coerce 24 | #endif 25 | 26 | namespace graph_with_normals 27 | { 28 | 29 | typedef 30 | boost::subgraph< 31 | pcl::graph::point_cloud_graph< 32 | PointWithNormalT 33 | , boost::vecS 34 | , boost::undirectedS 35 | , boost::property 36 | , boost::property> 38 | > 39 | > Graph; 40 | 41 | } 42 | 43 | namespace graph_without_normals 44 | { 45 | 46 | typedef 47 | boost::subgraph< 48 | pcl::graph::point_cloud_graph< 49 | PointT 50 | , boost::vecS 51 | , boost::undirectedS 52 | , boost::property 53 | , boost::property> 55 | > 56 | > Graph; 57 | 58 | } 59 | 60 | #ifndef GRAPH_WITHOUT_NORMALS 61 | typedef graph_with_normals::Graph Graph; 62 | #else 63 | typedef graph_without_normals::Graph Graph; 64 | #endif 65 | 66 | typedef boost::shared_ptr GraphPtr; 67 | typedef boost::reference_wrapper GraphRef; 68 | typedef std::vector GraphRefVector; 69 | typedef boost::graph_traits::vertex_descriptor VertexId; 70 | typedef boost::graph_traits::edge_descriptor EdgeId; 71 | typedef boost::graph_traits::edge_iterator EdgeIterator; 72 | 73 | #endif /* TYPEDEFS_H */ 74 | 75 | -------------------------------------------------------------------------------- /src/create_seeds.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | int 12 | main (int argc, char** argv) 13 | { 14 | if (argc < 3 || pcl::console::find_switch (argc, argv, "--help")) 15 | { 16 | pcl::console::print_error ("Usage: %s \n", argv[0]); 17 | pcl::console::print_info ("Directory should contain multiple PCD files, one file for each label\n"); 18 | return (1); 19 | } 20 | 21 | namespace fs = boost::filesystem; 22 | fs::path directory (argv[1]); 23 | fs::directory_iterator end_iter; 24 | 25 | typedef std::multimap result_set_t; 26 | result_set_t result_set; 27 | 28 | if (!fs::exists (directory) || !fs::is_directory (directory)) 29 | { 30 | pcl::console::print_error ("\"%s\" does not exist or is not a directory\n", argv[1]); 31 | return (2); 32 | } 33 | 34 | typedef pcl::PointCloud SeedCloud; 35 | SeedCloud seeds; 36 | int label = 1; 37 | for (fs::directory_iterator dir_iter(directory) ; dir_iter != end_iter ; ++dir_iter) 38 | { 39 | if (fs::is_regular_file (dir_iter->status ()) && fs::extension (*dir_iter) == ".pcd") 40 | { 41 | pcl::PointCloud xyz_cloud; 42 | if (pcl::io::loadPCDFile (dir_iter->path ().string (), xyz_cloud) >= 0) 43 | { 44 | SeedCloud seed_cloud; 45 | pcl::copyPointCloud (xyz_cloud, seed_cloud); 46 | for (size_t i = 0; i < seed_cloud.size (); ++i) 47 | seed_cloud[i].label = label; 48 | seeds += seed_cloud; 49 | ++label; 50 | } 51 | } 52 | } 53 | 54 | pcl::io::savePCDFile (argv[2], seeds); 55 | 56 | return (0); 57 | } 58 | 59 | -------------------------------------------------------------------------------- /src/hungarian/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SUBSYS_NAME hungarian) 2 | 3 | set(HUNGARIAN_SOURCE_DIR ${PROJECT_SOURCE_DIR}/third-party/hungarian-v2.0) 4 | 5 | include_directories(${CMAKE_SOURCE_DIR}/include/${SUBSYS_NAME}) 6 | include_directories(${HUNGARIAN_SOURCE_DIR}) 7 | 8 | add_library(hungarian 9 | hungarian.cpp 10 | ${HUNGARIAN_SOURCE_DIR}/Assignment.cpp 11 | ${HUNGARIAN_SOURCE_DIR}/BipartiteGraph.cpp 12 | ${HUNGARIAN_SOURCE_DIR}/Hungarian.cpp 13 | ${HUNGARIAN_SOURCE_DIR}/PlotGraph.cpp 14 | ${HUNGARIAN_SOURCE_DIR}/plot/Cgnuplot.cpp 15 | ${HUNGARIAN_SOURCE_DIR}/plot/graph_hungarian.cpp 16 | ) 17 | 18 | set_target_properties(hungarian 19 | PROPERTIES COMPILE_FLAGS -std=c++98 20 | ) 21 | 22 | SET_SOURCE_FILES_PROPERTIES( 23 | hungarian.cpp 24 | PROPERTIES COMPILE_FLAGS -std=gnu++11 25 | ) 26 | -------------------------------------------------------------------------------- /src/hungarian/hungarian.cpp: -------------------------------------------------------------------------------- 1 | #include "hungarian.h" 2 | 3 | #include "Assignment.h" 4 | #include "Hungarian.h" 5 | #include "CmdParser.h" 6 | #include "BipartiteGraph.h" 7 | 8 | std::map 9 | findAssignment (const std::vector>& triples) 10 | { 11 | std::map l1_map; 12 | std::map l1_map_inverse; 13 | std::map l2_map; 14 | std::map l2_map_inverse; 15 | // Iterate over triples and build a label mapping 16 | uint32_t l1; 17 | uint32_t l2; 18 | size_t c; 19 | for (size_t i = 0; i < triples.size (); ++i) 20 | { 21 | std::tie(l1, l2, c) = triples[i]; 22 | if (l1_map.count (l1) == 0) 23 | { 24 | size_t i = l1_map.size (); 25 | l1_map.insert (std::make_pair (l1, i)); 26 | l1_map_inverse.insert (std::make_pair (i, l1)); 27 | } 28 | if (l2_map.count (l2) == 0) 29 | { 30 | size_t i = l2_map.size (); 31 | l2_map.insert (std::make_pair (l2, i)); 32 | l2_map_inverse.insert (std::make_pair (i, l2)); 33 | } 34 | } 35 | // Create matrix of proper size 36 | Matrix m (l1_map.size ()); 37 | for (size_t i = 0; i < l1_map.size (); ++i) 38 | m[i].resize (l2_map.size ()); 39 | // Fill weights 40 | for (size_t i = 0; i < triples.size (); ++i) 41 | { 42 | std::tie(l1, l2, c) = triples[i]; 43 | m[l1_map[l1]][l2_map[l2]].SetWeight (c); 44 | } 45 | // Run the algorithm 46 | std::map assignment; 47 | BipartiteGraph bg (m); 48 | Hungarian h (bg); 49 | h.HungarianAlgo (); 50 | // Create assignment 51 | for (const auto& pair : h.M) 52 | assignment.insert (std::make_pair (l1_map_inverse[pair.first], l2_map_inverse[pair.second])); 53 | return assignment; 54 | } 55 | 56 | std::map 57 | findAssignment (const Eigen::MatrixXi& weights) 58 | { 59 | bool transpose = weights.rows () > weights.cols (); 60 | int rows = !transpose ? weights.rows () : weights.cols (); 61 | int cols = !transpose ? weights.cols () : weights.rows (); 62 | 63 | Matrix m (rows); 64 | for (int i = 0; i < rows; ++i) 65 | { 66 | m[i].resize (cols); 67 | for (int j = 0; j < cols; ++j) 68 | m[i][j].SetWeight (!transpose ? weights (i, j) : weights (j, i)); 69 | } 70 | // Run the algorithm 71 | std::map assignment; 72 | BipartiteGraph bg (m); 73 | Hungarian h (bg); 74 | h.HungarianAlgo (); 75 | // Create assignment 76 | for (const auto& pair : h.M) 77 | assignment.insert (std::make_pair (!transpose ? pair.first : pair.second, 78 | !transpose ? pair.second : pair.first)); 79 | return assignment; 80 | } 81 | 82 | -------------------------------------------------------------------------------- /src/io.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "io.h" 11 | #include "conversions.h" 12 | 13 | template bool 14 | load (const std::string& filename, 15 | typename pcl::PointCloud::Ptr cloud, 16 | pcl::PointCloud::Ptr normals) 17 | { 18 | if (cloud) 19 | cloud->clear (); 20 | else 21 | cloud.reset (new pcl::PointCloud); 22 | 23 | if (normals) 24 | normals->clear (); 25 | else 26 | normals.reset (new pcl::PointCloud); 27 | 28 | pcl::console::print_highlight ("Loading file \"%s\"... ", filename.c_str ()); 29 | if (boost::algorithm::ends_with (filename, ".pcd")) 30 | { 31 | pcl::PCLPointCloud2 blob; 32 | if (pcl::io::loadPCDFile (filename, blob)) 33 | { 34 | pcl::console::print_error ("error!\n"); 35 | return false; 36 | } 37 | pcl::fromPCLPointCloud2 (blob, *cloud); 38 | for (const auto& field : blob.fields) 39 | { 40 | if (field.name == "normal_x") 41 | { 42 | pcl::fromPCLPointCloud2 (blob, *normals); 43 | break; 44 | } 45 | } 46 | } 47 | else if (boost::algorithm::ends_with (filename, ".ply")) 48 | { 49 | pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh); 50 | if (!pcl::io::loadPolygonFile (filename, *mesh)) 51 | { 52 | pcl::console::print_error ("error!\n"); 53 | return false; 54 | } 55 | meshToPointsAndNormals (mesh, cloud, normals); 56 | } 57 | 58 | size_t finite = 0; 59 | for (const auto& pt : *cloud) 60 | if (pcl::isFinite (pt)) 61 | ++finite; 62 | 63 | pcl::console::print_info ("%zu points (%zu finite).\n", cloud->size (), finite); 64 | return true; 65 | } 66 | 67 | bool 68 | hasColor (const std::string& filename) 69 | { 70 | pcl::PCLPointCloud2 cloud2; 71 | pcl::PCDReader reader; 72 | reader.readHeader (filename, cloud2); 73 | for (size_t i = 0; i < cloud2.fields.size (); ++i) 74 | if (cloud2.fields[i].name == "rgb" || 75 | cloud2.fields[i].name == "rgba") 76 | return true; 77 | return false; 78 | } 79 | 80 | void 81 | saveSparseMatrix (const std::string& filename, 82 | const Eigen::SparseMatrix& M) 83 | { 84 | std::ofstream file (filename); 85 | if (file.is_open ()) 86 | { 87 | file << M.rows () << " " << M.cols () << " " << M.nonZeros () << "\n"; 88 | for (int k = 0; k < M.outerSize (); ++k) 89 | for (Eigen::SparseMatrix::InnerIterator it (M, k); it; ++it) 90 | file << it.row () + 1 << " " << it.col () + 1 << " " << it.value () << "\n"; 91 | file.close(); 92 | } 93 | } 94 | 95 | void 96 | loadSparseMatrix (const std::string& filename, 97 | Eigen::SparseMatrix& M) 98 | { 99 | std::ifstream stream (filename); 100 | if (stream) 101 | { 102 | typedef Eigen::Triplet T; 103 | std::vector triplets; 104 | std::string line; 105 | int rows = -1; 106 | int cols = -1; 107 | while (getline (stream, line)) 108 | { 109 | if (!line.size () || line[0] == '%') 110 | { 111 | } 112 | else if (rows == -1 && cols == -1) 113 | { 114 | std::stringstream (line) >> rows >> cols; 115 | } 116 | else 117 | { 118 | int row, col; 119 | float value; 120 | std::stringstream (line) >> row >> col >> value; 121 | triplets.push_back (T (row - 1, col - 1, value)); 122 | } 123 | } 124 | M.resize (rows, cols); 125 | if (triplets.size ()) 126 | M.setFromTriplets (triplets.begin (), triplets.end ()); 127 | } 128 | stream.close (); 129 | } 130 | 131 | template bool 132 | load 133 | (const std::string& filename, 134 | typename pcl::PointCloud::Ptr cloud, 135 | pcl::PointCloud::Ptr normals); 136 | 137 | template bool 138 | load 139 | (const std::string& filename, 140 | typename pcl::PointCloud::Ptr cloud, 141 | pcl::PointCloud::Ptr normals); 142 | 143 | template bool 144 | load 145 | (const std::string& filename, 146 | typename pcl::PointCloud::Ptr cloud, 147 | pcl::PointCloud::Ptr normals); 148 | 149 | template bool 150 | load 151 | (const std::string& filename, 152 | typename pcl::PointCloud::Ptr cloud, 153 | pcl::PointCloud::Ptr normals); 154 | 155 | template bool 156 | load 157 | (const std::string& filename, 158 | typename pcl::PointCloud::Ptr cloud, 159 | pcl::PointCloud::Ptr normals); 160 | -------------------------------------------------------------------------------- /src/kde.cpp: -------------------------------------------------------------------------------- 1 | #include "kde.h" 2 | 3 | KDE::KDE (std::vector&& dataset) 4 | : dataset_ (std::move (dataset)) 5 | { 6 | assert (dataset_.size () % 2 == 0); 7 | flann::Matrix D (dataset_.data (), dataset_.size () / 2, 2); 8 | kdtree_.reset (new KdTree (flann::KDTreeSingleIndexParams ())); 9 | kdtree_->buildIndex (D); 10 | setBandwidth (1.0); 11 | } 12 | 13 | void 14 | KDE::setBandwidth (float bandwidth) 15 | { 16 | bandwidth_ = bandwidth; 17 | auto m = -0.5 / (bandwidth_ * bandwidth_); 18 | // Gaussian kernel sans coefficient in front 19 | kernel_ = [=](float dist){ return std::exp (m * dist * dist); }; 20 | float M = 0.01; // not interested in points contributing less than this 21 | radius_ = bandwidth_ * std::sqrt (-2 * std::log (M)); 22 | } 23 | 24 | float 25 | KDE::evaluateAt (float x, float y) const 26 | { 27 | // TODO: this finds distance to nearest neighbor, not density estimate 28 | float query[] = {x, y}; 29 | flann::Matrix Q (query, 1, 2); 30 | int index; 31 | flann::Matrix I (&index, 1, 1); 32 | float distance; 33 | flann::Matrix D (&distance, 1, 1); 34 | kdtree_->knnSearch(Q, I, D, 1, flann::SearchParams (128)); 35 | return distance; 36 | } 37 | 38 | std::vector 39 | KDE::evaluateAt (std::vector& query) const 40 | { 41 | assert (query.size () % 2 == 0); 42 | size_t size = query.size () / 2; 43 | flann::Matrix Q (query.data (), size, 2); 44 | std::vector> indices; 45 | std::vector> distances; 46 | kdtree_->radiusSearch(Q, indices, distances, radius_, flann::SearchParams (32, 0, false)); 47 | std::vector densities (size, 0.0); 48 | for (size_t i = 0; i < size; ++i) 49 | for (const auto& d : distances[i]) 50 | densities[i] += kernel_ (d); 51 | return densities; 52 | } 53 | 54 | -------------------------------------------------------------------------------- /src/mesh_grid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "tviewer/color.h" 5 | 6 | #include "scaler.h" 7 | #include "mesh_grid.h" 8 | 9 | MeshGrid::MeshGrid (float min_x, float max_x, float min_y, float max_y, float cell_size) 10 | : MeshGrid (min_x, max_x, min_y, max_y, cell_size, cell_size) 11 | { 12 | } 13 | 14 | MeshGrid::MeshGrid (float min_x, float max_x, float min_y, float max_y, float cell_size_x, float cell_size_y) 15 | : min_x_ (min_x) 16 | , max_x_ (max_x) 17 | , min_y_ (min_y) 18 | , max_y_ (max_y) 19 | , cell_size_x_ (cell_size_x) 20 | , cell_size_y_ (cell_size_y) 21 | , num_cells_x_ (ceil ((max_x_ - min_x_) / cell_size_x_)) 22 | , num_cells_y_ (ceil ((max_y_ - min_y_) / cell_size_y_)) 23 | { 24 | assert (max_x_ > min_x_); 25 | assert (max_y_ > min_y_); 26 | assert (cell_size_x_ > 0); 27 | assert (cell_size_y_ > 0); 28 | } 29 | 30 | std::vector 31 | MeshGrid::getCellCenters () 32 | { 33 | std::vector centers; 34 | centers.reserve (num_cells_x_ * num_cells_y_ * 2); 35 | for (size_t i = 0; i < num_cells_y_; ++i) 36 | { 37 | float y = min_y_ + i * cell_size_y_ + cell_size_y_ / 2; 38 | for (size_t j = 0; j < num_cells_x_; ++j) 39 | { 40 | float x = min_x_ + j * cell_size_x_ + cell_size_x_ / 2; 41 | centers.push_back (x); 42 | centers.push_back (y); 43 | } 44 | } 45 | return centers; 46 | } 47 | 48 | vtkSmartPointer 49 | MeshGrid::createPolyData (float z) 50 | { 51 | auto plane = vtkSmartPointer::New (); 52 | plane->SetOrigin (min_x_, min_y_, z); 53 | plane->SetPoint1 (max_x_, min_y_, z); 54 | plane->SetPoint2 (min_x_, max_y_, z); 55 | plane->SetResolution (num_cells_x_, num_cells_y_); 56 | plane->Update (); 57 | return plane->GetOutput (); 58 | } 59 | 60 | vtkSmartPointer 61 | MeshGrid::createPolyData (std::vector colors, float z) 62 | { 63 | auto poly_data = createPolyData (z); 64 | assert (poly_data->GetNumberOfCells () == colors.size ()); 65 | auto scale = getScaler (colors); 66 | vtkSmartPointer cell_data = vtkSmartPointer::New (); 67 | cell_data->SetNumberOfComponents (3); 68 | cell_data->SetNumberOfTuples (colors.size ()); 69 | for (size_t i = 0; i < colors.size (); ++i) 70 | { 71 | unsigned char c[3]; 72 | auto v = scale (colors[i]); 73 | tviewer::getRGBFromColor (tviewer::getColor (v), c); 74 | float rgb[3]; 75 | rgb[0] = c[0]; 76 | rgb[1] = c[1]; 77 | rgb[2] = c[2]; 78 | cell_data->InsertTuple (i, rgb); 79 | } 80 | poly_data->GetCellData ()->SetScalars (cell_data); 81 | return poly_data; 82 | } 83 | 84 | -------------------------------------------------------------------------------- /src/random_walker_segmentation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the copyright holder(s) nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | 37 | #include "impl/random_walker_segmentation.hpp" 38 | 39 | #ifndef PCL_NO_PRECOMPILE 40 | #include 41 | #include 42 | // Instantiations of specific point types 43 | #ifdef PCL_ONLY_CORE_POINT_TYPES 44 | PCL_INSTANTIATE (RandomWalkerSegmentation, (pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)) 45 | #else 46 | PCL_INSTANTIATE (RandomWalkerSegmentation, PCL_XYZ_POINT_TYPES) 47 | #endif /* PCL_ONLY_CORE_POINT_TYPES */ 48 | #endif /* PCL_NO_PRECOMPILE */ 49 | 50 | -------------------------------------------------------------------------------- /src/tree_top_detector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "tree_top_detector.h" 7 | 8 | TreeTopDetector::TreeTopDetector () 9 | : cell_size_ (0.05) 10 | { 11 | setRadius (0.5); 12 | } 13 | 14 | void 15 | TreeTopDetector::setInputCloud (typename PointCloud::ConstPtr cloud) 16 | { 17 | input_ = cloud; 18 | } 19 | 20 | void 21 | TreeTopDetector::setRadius (float radius) 22 | { 23 | radius_ = radius; 24 | cell_radius_ = std::ceil (radius_ / cell_size_); 25 | } 26 | 27 | void 28 | TreeTopDetector::detect (std::vector& tree_top_indices) 29 | { 30 | tree_top_indices.clear (); 31 | 32 | Point min, max; 33 | pcl::getMinMax3D (*input_, min, max); 34 | 35 | float range_x = max.x - min.x; 36 | float range_y = max.y - min.y; 37 | int size_x = std::ceil (range_x / cell_size_); 38 | int size_y = std::ceil (range_y / cell_size_); 39 | 40 | density_.resize (size_x * size_y, 0); 41 | 42 | std::vector indices (input_->size ()); 43 | size_t n = 0; 44 | std::generate (std::begin (indices), std::end (indices), [&]{ return n++; }); 45 | 46 | // Sort in descending order of Z coordinate 47 | std::sort (std::begin (indices), 48 | std::end (indices), 49 | [&](int i1, int i2) { return input_->at (i1).z > input_->at (i2).z; }); 50 | 51 | for (size_t i = 0; i < indices.size (); ++i) 52 | { 53 | auto current_index = indices[i]; 54 | auto current_point = input_->at (current_index); 55 | 56 | int cell_x = std::round ((current_point.x - min.x) / cell_size_); 57 | int cell_y = std::round ((current_point.y - min.y) / cell_size_); 58 | 59 | if (density_[cell_y * size_x + cell_x] == 0.0) 60 | { 61 | // candidates_.push ({cell_x, cell_y, current_index, current_point.z - 0.1f}); 62 | pcl::PointIndices pi; 63 | pi.indices.push_back (current_index); 64 | tree_top_indices.push_back (pi); 65 | } 66 | 67 | for (int cx = std::max (0, cell_x - cell_radius_); cx < std::min (cell_x + cell_radius_, size_x); ++cx) 68 | { 69 | for (int cy = std::max (0, cell_y - cell_radius_); cy < std::min (cell_y + cell_radius_, size_y); ++cy) 70 | { 71 | float mcx = (0.5f + cx) * cell_size_ + min.x; 72 | float mcy = (0.5f + cy) * cell_size_ + min.y; 73 | 74 | if (std::pow (mcx - current_point.x, 2) + std::pow (mcy - current_point.y, 2) < radius_ * radius_) 75 | { 76 | density_[cy * size_x + cx] += 0.1; 77 | } 78 | } 79 | } 80 | } 81 | } 82 | 83 | const std::vector& 84 | TreeTopDetector::getDensity () const 85 | { 86 | return density_; 87 | } 88 | 89 | -------------------------------------------------------------------------------- /src/view_point_cloud_graph.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "typedefs.h" 10 | 11 | #include "io.h" 12 | #include "tviewer/tviewer.h" 13 | #include "measure_runtime.h" 14 | #include "graph_visualizer.h" 15 | #include "factory/graph_factory.h" 16 | 17 | 18 | int main (int argc, char ** argv) 19 | { 20 | factory::GraphFactory g_factory; 21 | 22 | if (argc < 2 || pcl::console::find_switch (argc, argv, "--help")) 23 | { 24 | pcl::console::print_error ("Usage: %s \n" 25 | "%s\n" 26 | , argv[0] 27 | , g_factory.getUsage ().c_str ()); 28 | return (1); 29 | } 30 | 31 | typename PointCloudT::Ptr cloud (new PointCloudT); 32 | typename NormalCloudT::Ptr normals (new NormalCloudT); 33 | 34 | if (!load (argv[1], cloud, normals)) 35 | return (1); 36 | 37 | 38 | /********************************************************************* 39 | * Pre-compute graph * 40 | *********************************************************************/ 41 | 42 | 43 | auto g = g_factory.instantiate (cloud, argc, argv); 44 | auto& graph = g.get (); 45 | 46 | g_factory.printValues (); 47 | 48 | pcl::console::print_info ("Working with component of size: %zu / %zu\n", 49 | boost::num_vertices (graph), 50 | boost::num_edges (graph)); 51 | 52 | 53 | /********************************************************************* 54 | * Visualize graph * 55 | *********************************************************************/ 56 | 57 | 58 | using namespace tviewer; 59 | auto viewer = create (argc, argv); 60 | 61 | typedef GraphVisualizer GraphVisualizer; 62 | GraphVisualizer gv (graph); 63 | 64 | viewer->add 65 | ( CreatePointCloudObject ("vertices", "v") 66 | . description ("Graph vertices") 67 | . pointSize (6) 68 | . visibility (0.95) 69 | . data (gv.getVerticesCloudColorsNatural ()) 70 | ); 71 | 72 | viewer->add 73 | ( CreatePointCloudObject ("curvature", "C") 74 | . description ("Vertex curvature") 75 | . pointSize (6) 76 | . visibility (0.95) 77 | . data (gv.getVerticesCloudColorsCurvature ()) 78 | ); 79 | 80 | viewer->add 81 | ( CreateNormalCloudObject ("normals", "n") 82 | . description ("Vertex normals") 83 | . level (1) 84 | . scale (0.01) 85 | . data (gv.getVerticesNormalsCloud ()) 86 | ); 87 | 88 | viewer->add 89 | ( CreatePolyDataObject ("edges", "a") 90 | . description ("Adjacency edges") 91 | . data (gv.getEdgesPolyData ()) 92 | ); 93 | 94 | viewer->show ("edges"); 95 | viewer->run (); 96 | 97 | return (0); 98 | } 99 | 100 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/Assignment.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Assignment.h" 3 | 4 | 5 | Matrix 6 | Assignment::RandomGenerate(size_t nrows, size_t ncols, int MAX, unsigned int _seed){ 7 | 8 | //accept new seed for random generator 9 | if(_seed != SEED) 10 | srand(_seed); 11 | else 12 | srand(this->seed); 13 | 14 | //update data members 15 | this->num_agents = nrows; 16 | this->num_tasks = ncols; 17 | 18 | //define a matrix 19 | Matrix matrix; 20 | matrix.resize(nrows); 21 | for(unsigned int i=0; i numstream; 39 | 40 | size_t num_rows = 0; 41 | size_t num_cols = 0; 42 | 43 | if (input_file.is_open()) 44 | { 45 | while (!input_file.eof() ) 46 | { 47 | getline (input_file,line); 48 | 49 | size_t local_num_cols=0; 50 | vector local_numstream; 51 | local_numstream.clear(); 52 | string word; 53 | 54 | stringstream parse(line); 55 | while(parse >> word){ 56 | //if comment line, ignore it 57 | if(word[0] == '#') 58 | break; 59 | //numstream.push_back(atoi(word.c_str())); //if not number, then convert to zero 60 | numstream.push_back(atof(word.c_str())); //if not number, then convert to zero 61 | //local_numstream.push_back(atoi(word.c_str())); 62 | local_numstream.push_back(atof(word.c_str())); 63 | 64 | //matrix(num_rows, num_cols)= atoi(word.c_str()); 65 | local_num_cols++; 66 | } //end inner while 67 | 68 | //judge if the matrix format is correct or not 69 | if(num_cols && local_num_cols && num_cols!=local_num_cols){ 70 | cerr<num_agents = num_rows; 86 | this->num_tasks = num_cols; 87 | 88 | //put elements into matrix 89 | //matrix.resize(num_rows, num_cols); 90 | matrix.resize(num_rows); 91 | for(unsigned int i=0; i::iterator itr = numstream.begin(); 95 | for(unsigned int i=0; i30){ 112 | //cout< GetAssignmentSize(void){return pair(num_agents, num_tasks); } 39 | 40 | //Randomly generate an assignment-matrix, the default arguments are pre-set 41 | //Currently can generate only integer numbers 42 | Matrix RandomGenerate(size_t nrows = AGENTS_SIZE, size_t ncols = TASKS_SIZE, 43 | int MAX = 100, unsigned int _seed = SEED); 44 | 45 | //Import an assignment-matrix from external file 46 | Matrix ImportAssignment(ifstream&); 47 | 48 | //Display matrix onto screen 49 | void DisplayMatrix(Matrix&) const; 50 | 51 | 52 | private: 53 | //basic data members 54 | unsigned int seed; 55 | size_t num_agents; 56 | size_t num_tasks; 57 | 58 | }; 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/BipartiteGraph.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "BipartiteGraph.h" 3 | 4 | 5 | Edge 6 | BipartiteGraph::GetMaxWeightEdge(Vertex& agent){ 7 | size_t row_num = agent.GetVID(); 8 | size_t col_num = 0; 9 | double max_wt = bg_matrix[row_num][0].GetWeight(); 10 | //for(vector::iterator itr = bg_matrix[row_num].begin(); itr != bg_matrix[row_num].begin() + bg_matrix.ncols(); itr++) 11 | for(unsigned int i=0; i max_wt){ 13 | max_wt = bg_matrix[row_num][i].GetWeight(); 14 | col_num = i; 15 | } 16 | //cout<::iterator itr = tasks.begin(); itr != tasks.end(); itr++){ 39 | itr->SetVID(_vid++); 40 | itr->SetObj("TASK"); 41 | itr->SetLabel(0); 42 | itr->path.clear(); 43 | } 44 | 45 | //assign agent VID and assign label to be max; 46 | _vid = 0; 47 | for(vector::iterator itr = agents.begin(); itr != agents.end(); itr++){ 48 | itr->SetVID(_vid++); 49 | itr->SetObj("AGENT"); 50 | itr->SetLabel(this->GetMaxWeightEdge(*itr).GetWeight()); 51 | itr->path.clear(); 52 | } 53 | 54 | //init all edges 55 | for(unsigned int i=0; i& v){ 63 | //for(vector::iterator itr = v.begin(); itr != v.end(); itr++) 64 | //cout<GetLabel()<<" "; 65 | //cout<DisplayLabels(this->agents); 73 | //cout<<"Labels for tasks:"<DisplayLabels(this->tasks); 75 | } 76 | 77 | bool 78 | BipartiteGraph::CheckFeasibility(Matrix& _m, 79 | vector& _agents, vector& _tasks){ 80 | //check the size for args 81 | if(_m.size() != _agents.size() || _m[0].size() != _tasks.size()){ 82 | //cout<<"Error: Size discrepency found during feasibility checking. Stopped."< DOUBLE_EPSILON ){ 91 | if(_m[i][j].GetWeight() > _agents[i].GetLabel() + _tasks[j].GetLabel()){ 92 | //cout<<"Labels are not feasible any more!"<CheckFeasibility(bg_matrix, agents, tasks); 107 | //if(res) cout<<"Bipartite graph is feasible."<ConstructBG(m); } 33 | 34 | ~BipartiteGraph(){} 35 | 36 | //basic data operations 37 | void SetNumMatched(size_t _num_matched){ num_matched = _num_matched; } 38 | size_t GetNumMatched(void){ return num_matched; } 39 | size_t GetNumAgents(void){ return num_agents; } 40 | size_t GetNumTasks(void){ return num_tasks; } 41 | double GetMinDelta(void){ return min_delta; } 42 | void SetMinDelta(double _min_delta){ min_delta = _min_delta; } 43 | 44 | Vertex* GetAgent(VID _vid){ return &agents[_vid]; } 45 | Vertex* GetTask(VID _vid){ return &tasks[_vid]; } 46 | 47 | vector* GetAgents(void){ return &agents; } 48 | vector* GetTasks(void){ return &tasks; } 49 | 50 | Matrix* GetMatrix(void){ return &bg_matrix; } 51 | Edge* GetMatrix(EID& _eid){return &bg_matrix[_eid.first][_eid.second];} 52 | Edge* GetMatrix(size_t row_index, size_t col_index){ 53 | return &bg_matrix[row_index][col_index]; } 54 | 55 | void DisplayLabels(void); 56 | void DisplayLabels(vector& v); 57 | 58 | //Get the maximal weighted edge among all outgoing edges from vertex "agent" 59 | Edge GetMaxWeightEdge(Vertex& agent); 60 | 61 | //Construct a bipartite graph based on a specific matrix 62 | //All argments are initialized in it 63 | void ConstructBG(Matrix&); 64 | 65 | //Check if a bipartite graph is feasible or not 66 | bool CheckFeasibility(Matrix&, vector&, vector&); 67 | bool CheckFeasibility(void); 68 | 69 | private: 70 | //basic data members 71 | size_t num_matched; 72 | size_t num_agents; 73 | size_t num_tasks; 74 | double min_delta; 75 | 76 | //data members storing vertices, and matrix that contains edges/utils 77 | vector agents; 78 | vector tasks; 79 | Matrix bg_matrix; 80 | 81 | }; 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/CmdParser.h: -------------------------------------------------------------------------------- 1 | 2 | /////////////////////////////////////////////////////////////////////////////// 3 | // File Name: CmdParser.h 4 | // This file defines a class for parsing command line 5 | // Lantao Liu, Nov 1, 2009 6 | /////////////////////////////////////////////////////////////////////////////// 7 | 8 | 9 | #ifndef CMD_PARSER_H 10 | #define CMD_PARSER_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "Matrix.h" 18 | 19 | using namespace std; 20 | 21 | int _Period; 22 | int _Verbose; 23 | bool _Plot; 24 | 25 | class CmdParser{ 26 | public: 27 | CmdParser(int _argc, char **_argv):seed(SEED), 28 | assignment_size(AGENTS_SIZE), 29 | period(PERIOD), 30 | verbose_level(VERBOSE_LEVEL), 31 | plot(PLOT){ 32 | ParseCmd(_argc, _argv); 33 | _Plot = plot; 34 | _Period = period; 35 | _Verbose = verbose_level; 36 | } 37 | ~CmdParser(){} 38 | 39 | string GetInputFileName(void){ return input_file; } 40 | unsigned int GetSeed(void){ return seed; } 41 | size_t GetAssignmentSize(void){ return assignment_size; } 42 | 43 | void ParseCmd(int argc, char **argv); 44 | 45 | void DisplayInput(void); 46 | 47 | void GiveUsage(char *cmd); 48 | 49 | public: 50 | unsigned int seed; 51 | string input_file; 52 | size_t assignment_size; 53 | unsigned int period; 54 | unsigned int verbose_level; 55 | bool plot; 56 | 57 | }; 58 | 59 | void 60 | CmdParser::ParseCmd(int argc, char **argv){ 61 | 62 | int c; 63 | while ((c = getopt (argc, argv, "i:s:n:p:t:v:h")) != -1) 64 | switch (c) 65 | { 66 | case 'i': 67 | this->input_file = optarg; 68 | break; 69 | case 's': 70 | this->seed = atoi(optarg); 71 | break; 72 | case 'n': 73 | this->assignment_size = atoi(optarg); 74 | break; 75 | case 'p': 76 | this->plot = atoi(optarg); 77 | break; 78 | case 't': 79 | this->period = atoi(optarg); 80 | break; 81 | case 'v': 82 | this->verbose_level = atoi(optarg); 83 | break; 84 | case 'h': 85 | this->GiveUsage(argv[0]); 86 | break; 87 | case '?': 88 | if (isprint (optopt)) 89 | fprintf (stderr, "Unknown option `-%c'.\n", optopt); 90 | else 91 | fprintf (stderr, 92 | "Unknown option character `\\x%x'.\n", 93 | optopt); 94 | this->GiveUsage(argv[0]); 95 | abort(); 96 | default: 97 | //cout<<"Error: Could not understand the command line."<input_file != "") 106 | //cout<<" Accept new input file: "<seed != SEED) 109 | //cout<<" Accept new seed: "<assignment_size != AGENTS_SIZE) 112 | //cout<<" Accept new assignment size: "<period != PERIOD) 115 | //cout<<" Accept new period: "<verbose_level != 0) 118 | //cout<<" Accept new verbose level: "< * 6 | * Nov 1, 2009 * 7 | * * 8 | ******************************************************************************/ 9 | 10 | /////////////////////////////////////////////////////////////////////////////// 11 | // File name: Hungarian.h 12 | // This file defines the main algorithm of Kuhn-Munkres Hungarian Method. 13 | // The algorithm is decomposed into functional modules, and each module is 14 | // encapsulated as a function here. 15 | // Lantao Liu, Nov 1, 2009 16 | /////////////////////////////////////////////////////////////////////////////// 17 | 18 | #ifndef Hungarian_H 19 | #define Hungarian_H 20 | 21 | #include "Assignment.h" 22 | #include "BipartiteGraph.h" 23 | #include "PlotGraph.h" 24 | 25 | 26 | /////////////////////////////////////////////////////////////////////////////// 27 | // 28 | // Hungarian class: defines basic modules (methods) for Hungarian procedure. 29 | // 30 | /////////////////////////////////////////////////////////////////////////////// 31 | 32 | 33 | class Hungarian{ 34 | 35 | public: 36 | Hungarian(){ S.reserve(AGENTS_SIZE); T.reserve(TASKS_SIZE); } 37 | Hungarian(BipartiteGraph& _bg):bg(_bg){ 38 | S.reserve(_bg.GetNumAgents()); 39 | T.reserve(_bg.GetNumTasks()); 40 | N.reserve(_bg.GetNumTasks()); 41 | } 42 | ~Hungarian(){} 43 | 44 | //To access private bipartite graph member 45 | BipartiteGraph* GetBG(void){ return &bg; } 46 | 47 | //Judge if the bipartite graph is perfect or not, to control program to halt 48 | bool IsPerfect(BipartiteGraph& _bg){ 49 | bool result = _bg.GetNumMatched() == _bg.GetNumAgents() ? true: false; 50 | return result; 51 | } 52 | bool IsPerfect(void){ return this->IsPerfect(this->bg); } 53 | 54 | //Start a new alternating tree, pass the root to set S 55 | //and initialize all other sets which need to initialized. 56 | void InitNewRoot(BipartiteGraph& _bg, VID root, vector& _S, vector& _T); 57 | 58 | //Randomly pick an unmatched (free) agent, as new root 59 | VID PickFreeAgent(BipartiteGraph& _bg); 60 | 61 | //Randomly pick a new task in set {N-T} during alternating tree 62 | VID PickAvailableTask(vector& _T, vector& _N); 63 | 64 | //Update labels, forcing N != T, return minimal delta 65 | double UpdateLabels(BipartiteGraph& _bg); 66 | 67 | //Refresh the whole bipartite graph, update all available sets 68 | //based on new state of bipartite graph _bg 69 | void RefreshBG(BipartiteGraph& _bg, 70 | const vector& _S, const vector& _T, 71 | vector& _N, vector& _EG, vector& _M); 72 | 73 | //Judge if need relabel or not, via comparing sets T and N 74 | bool NeedReLabel(vector& _T, vector& _N); 75 | 76 | //Find neighbors and return a neighbor-set, computed from EG and S 77 | vector FindNeighbors(const vector& _EG, const vector& _S); 78 | 79 | //Breadth-first Searching augmenting path, return the path as a vector of EID 80 | //x: root 81 | //y: unmatched task 82 | vector BFSAugmentingPath(BipartiteGraph& _bg, VID x, VID y); 83 | 84 | //Augment the path, via the information of augmenting path 85 | //flip the matched edges and unmatched edges, M size increases 1 86 | //_path: an augmenting path 87 | void AugmentPath(BipartiteGraph& _bg, vector& _path); 88 | 89 | //When found a matched task, put it and its mate (matching) agent 90 | //into T and S correspondingly, and color them to flag as having been 91 | //visited and covered 92 | void ExtendTree(BipartiteGraph& _bg, VID x, vector& _S, vector& _T); 93 | 94 | void HungarianAlgo(BipartiteGraph& _bg, vector& _S, vector& _T, 95 | vector& _N, vector& _EG, vector& _M); 96 | void HungarianAlgo(void); 97 | 98 | //Calculate optimal value: the sum of weights of matched edges 99 | double OptimalValue(BipartiteGraph& _bg, vector& _M); 100 | 101 | //Display data 102 | void DisplayData(vector&); 103 | void DisplayData(vector&); 104 | //The five args are in order of S, T, N, EG, M. Set true if want to display 105 | void DisplayData(bool s=false, bool t=false, bool n=false, 106 | bool eg=false, bool m=false); 107 | 108 | void DisplayLabels(vector&); 109 | 110 | void DisplayMatrix(Matrix&); 111 | void DisplayConfig(BipartiteGraph&); 112 | 113 | //data member 114 | //public: 115 | public: 116 | //A basic bipartite graph is needed 117 | BipartiteGraph bg; 118 | 119 | //All sets required in Hungarian algo 120 | vector S; 121 | vector T; 122 | vector N; 123 | vector EG; 124 | vector M; 125 | 126 | }; 127 | 128 | 129 | #endif 130 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/Log: -------------------------------------------------------------------------------- 1 | 2 | # fixed the gnuplot "buffer overflow" problem. 3 | # 4/28/2012 4 | 5 | # v2.0 6 | # removed the MTL dependency, mtl::matrix->2d vectors 7 | # 9/27/2011 8 | 9 | # modified the data type, allowing double type assignment 10 | # 3/5/2010 11 | 12 | # Pure original Hungarian algorithm. 13 | # 11/1/2009 14 | 15 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/Makefile: -------------------------------------------------------------------------------- 1 | 2 | CXX = g++ 3 | CFLAGS = -O3 -Wall #-fpermissive -Wl,-b64 4 | 5 | DIRS = . plot 6 | 7 | PLOTDIR = plot 8 | PLOTOBJS = $(PLOTDIR)/Cgnuplot.o 9 | HGRN_OBJS = Hungarian.o Assignment.o BipartiteGraph.o PlotGraph.o 10 | 11 | OBJS = $(HGRN_OBJS) $(PLOTOBJS) main.o 12 | 13 | EXE = hungarian 14 | all: $(EXE) 15 | $(EXE): $(OBJS) 16 | $(CXX) $(CFLAGS) -o $(EXE) $(OBJS) 17 | @echo Done. 18 | 19 | %.o: %.cpp %.h 20 | $(CXX) $(CFLAGS) -c $< 21 | 22 | $(PLOTDIR)/Cgnuplot.o: 23 | # @for $(i) in $(DIRS); do \ 24 | # echo "make all in $(i)..."; 25 | # (cd $(i); $(MAKE) $(MFLAGS)); done 26 | (cd $(PLOTDIR); $(MAKE) $(MFLAGS)) 27 | @echo Done making Cgnuplot. 28 | 29 | 30 | main.o: main.cpp Matrix.h CmdParser.h 31 | $(CXX) $(CFLAGS) -c $< 32 | 33 | clean: 34 | rm -f $(EXE) *.o *~ *.swp 35 | (cd $(PLOTDIR); $(MAKE) clean ) 36 | 37 | 38 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/Matrix.h: -------------------------------------------------------------------------------- 1 | 2 | /////////////////////////////////////////////////////////////////////////////// 3 | // File name: Matrix.h 4 | // This file defines the basic classes of vertex and edge, and the matrix data 5 | // type is then defined based on them. 6 | // In the matrix, the grids store the edges, which denote the utilities or 7 | // costs in corresponding assignment matrix. There are two vectors of 8 | // vertices, one for agents, and the other for tasks. 9 | // Lantao Liu, Nov 1, 2009 10 | // Last modified: 09/2011 -> MTL is removed and 2D vectors are used. 11 | /////////////////////////////////////////////////////////////////////////////// 12 | 13 | 14 | #ifndef MATRIX_H 15 | #define MATRIX_H 16 | 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | /* 30 | #include "mtl/mtl.h" 31 | #include "mtl/utils.h" 32 | #include "mtl/matrix.h" 33 | #include "mtl/matrix_implementation.h" 34 | 35 | //#define NUMTYPE Edge 36 | #define SHAPE mtl::rectangle<> 37 | #define STORAGE mtl::dense<> 38 | #define ORIEN mtl::row_major 39 | */ 40 | 41 | #define TASKS_SIZE 3 42 | #define AGENTS_SIZE 3 43 | #define DOUBLE_EPSILON 1e-7 //For double type comparison: <= as valid, > invalid 44 | 45 | #define SEED 0 46 | #define PERIOD 0 47 | #define VERBOSE_LEVEL 1 48 | #define PLOT 0 49 | 50 | #define MAX_RANDOM 100 51 | 52 | 53 | #define POS_INF 10e8 54 | #define NEG_INF -10e8 55 | 56 | #ifndef min 57 | #define min(x, y) (((x) > (y)) ? (y) : (x)) 58 | #endif 59 | 60 | #ifndef max 61 | #define max(x, y) (((x) > (y)) ? (x) : (y)) 62 | #endif 63 | 64 | using namespace std; 65 | //using namespace mtl; 66 | 67 | typedef size_t VID; 68 | typedef pair EID; 69 | 70 | class Edge; 71 | class Vertex; 72 | 73 | ///////////////////////////////////////////////////////////// 74 | // Define the matrix type 75 | // Edge is the basic data type used in it 76 | // The weight of edge denotes the utility or cost 77 | ///////////////////////////////////////////////////////////// 78 | 79 | //typedef mtl::matrix::type Matrix; 80 | typedef vector > Matrix; 81 | 82 | 83 | 84 | //////////////////////////////////////////////////////////// 85 | // 86 | // Edge class: represent an element in matrix 87 | // or an edge in bipartite graph 88 | // 89 | //////////////////////////////////////////////////////////// 90 | 91 | class Edge{ 92 | public: 93 | Edge(){ 94 | weight = 0; 95 | matched_flag = false; //unmatched 96 | admissible_flag = false; //inadmissible 97 | } 98 | Edge(EID _eid):eid(_eid){ 99 | weight = 0; 100 | matched_flag = false; //unmatched 101 | admissible_flag = false; //inadmissible 102 | } 103 | ~Edge(){} 104 | 105 | EID GetEID(void){ return eid; } 106 | void SetEID(EID _eid){ eid = _eid; } 107 | 108 | double GetWeight(void){ return weight; } 109 | void SetWeight(double _weight){ weight = _weight; } 110 | 111 | bool GetMatchedFlag(void){ return matched_flag; } 112 | void SetMatchedFlag(bool _matched_flag){ matched_flag = _matched_flag;} 113 | 114 | bool GetAdmissibleFlag(void){ return admissible_flag; } 115 | void SetAdmissibleFlag(bool _admissible){ admissible_flag = _admissible; } 116 | 117 | private: 118 | //data members describing properties of an edge 119 | EID eid; 120 | double weight; 121 | bool matched_flag; 122 | bool admissible_flag; 123 | 124 | }; 125 | 126 | 127 | 128 | /////////////////////////////////////////////////////////// 129 | // 130 | // Vertex class: represent an agent or a task 131 | // 132 | ////////////////////////////////////////////////////////// 133 | 134 | class Vertex{ 135 | public: 136 | Vertex():label(0), matched(false), colored(false){ 137 | //edges.resize(max(AGENTS_SIZE, TASKS_SIZE)); 138 | //deltas.resize(max(AGENTS_SIZE, TASKS_SIZE), 0); 139 | } 140 | Vertex(VID _vid):vid(_vid),label(0), matched(false), colored(false){ 141 | //edges.resize(max(AGENTS_SIZE, TASKS_SIZE)); 142 | //deltas.resize(max(AGENTS_SIZE, TASKS_SIZE), 0); 143 | } 144 | ~Vertex(){} 145 | 146 | VID GetVID(void){ return vid; } 147 | void SetVID(VID _vid){ vid = _vid; } 148 | 149 | string GetObj(void){ return obj; } 150 | void SetObj(string _obj){ obj = _obj; } 151 | 152 | double GetLabel(void){ return label; } 153 | void SetLabel(double _label){ label = _label; } 154 | 155 | bool GetMatched(void){ return matched; } 156 | void SetMatched(bool _matched){ matched = _matched; } 157 | 158 | bool GetColored(void){ return colored; } 159 | void SetColored(bool _colored){ colored = _colored; } 160 | 161 | bool GetVisited(void){ return visited; } 162 | void SetVisited(bool _visited){ visited = _visited; } 163 | 164 | vector* GetPath(void){ return &path; } 165 | 166 | private: 167 | //data members describing properties of a vertex 168 | VID vid; 169 | string obj; 170 | double label; 171 | bool matched; 172 | bool colored; //colored if in the set of T or S 173 | bool visited; //to flag if visited when go through alternating tree 174 | 175 | public: 176 | vector path; //record previous path so far 177 | 178 | }; 179 | 180 | 181 | #endif 182 | 183 | 184 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/PlotGraph.h: -------------------------------------------------------------------------------- 1 | 2 | /////////////////////////////////////////////////////////////////////////////// 3 | // File name: PlotGraph.h 4 | // Description: This file defines the methods of plotting the Hungarian steps. 5 | // You may need to install gnuplot first. 6 | // A plotting lib is included in ./libCgnuplot (Author: N. Devillard) 7 | // The plotting funtion is stripped from Dr. Dylan Shell's code 8 | // (see ./libCgnuplot/graph_hungarian.c: void draw_graph(int new_member_of_T) ) 9 | // Lantao Liu, Nov 3, 2009 10 | /////////////////////////////////////////////////////////////////////////////// 11 | 12 | #ifndef PLOT_GRAPH_H 13 | #define PLOT_GRAPH_H 14 | 15 | #include "Matrix.h" 16 | #include "BipartiteGraph.h" 17 | #include "plot/Cgnuplot.h" 18 | 19 | //#define PERIOD 4 //uniformly defined in Matrix.h 20 | 21 | extern int _Period; 22 | 23 | class PlotGraph{ 24 | public: 25 | PlotGraph():g(NULL),p(NULL){ period = _Period != 0 ? _Period : PERIOD; } 26 | ~PlotGraph(){} 27 | 28 | void SetPeriod(unsigned int _period){ period = _period; } 29 | unsigned int GetPeriod(void){ return period; } 30 | 31 | //gnuplot_ctrl* InitPlot(void){ return gnuplot_init(); } 32 | void InitPlot(void){ g = gnuplot_init(); } 33 | 34 | void ClosePlot(gnuplot_ctrl* _handle){ gnuplot_close(_handle); } 35 | void ClosePlot(void){ ClosePlot(g); } 36 | 37 | void PlotBipartiteGraph(BipartiteGraph& _bg, 38 | vector& _S, 39 | vector& _T, 40 | vector& _N, 41 | vector& _EG, 42 | vector& _M, 43 | int target_task = -1); 44 | 45 | void PlotAugmentingPath(BipartiteGraph& _bg, vector& _path); 46 | 47 | void DisplayData(const vector& vs); 48 | void DisplayData(const vector& es); 49 | 50 | //public: 51 | private: 52 | gnuplot_ctrl *g; 53 | gnuplot_ctrl *p; 54 | 55 | unsigned int period; //plot refreshing period 56 | 57 | 58 | }; 59 | 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/README: -------------------------------------------------------------------------------- 1 | # 2 | # PLEASE READ THIS FILE FIRST. 3 | # 4 | # This is the implementation of Kuhn-Munkres Hungarian algorithm written in C++ 5 | # and utilizes STL as main data structures. The algorithm assumes the input a 6 | # MAXIMIZATION problem and finds the maximal ouputs (including the optimality 7 | # and the matching pairs) through manipulating the bipartite graph. 8 | # 9 | # NOTE that, the time complexity of this implementation could be more than 10 | # O(N^3) but should be less than O(N^4), due to simple data structure used. 11 | # Time can still be improved. 12 | # 13 | # You can freely use this code, and we are grateful if you can cite our work: 14 | # Lantao Liu, Dylan Shell. "Assessing Optimal Assignment under Uncertainty: An Interval-based Algorithm". International Journal of Robotics Research (IJRR). vol. 30, no. 7, pp 936-953. Jun 2011. 15 | # Or other related work posted on my page: http://students.cse.tamu.edu/lantao/ 16 | # 17 | # Lantao Liu, 18 | # Dept. of Computer Science and Engineering, 19 | # Texas A&M University, TX, 77843-3112. 20 | # Nov 9, 2009. 21 | # Last updated on 9/27/2011 22 | # 23 | 24 | 25 | # ----------------------------------------------- 26 | 27 | # The code has been tested in Ubuntu 9.04+ and Mac OS 10.5., with gcc4.3.3. 28 | # If you are running Linux and you want to see the plot, your can apt-get GNUplot (a little bit flickery due to no double buffering). 29 | 30 | To compile, just type 'make' 31 | To clean, just type 'make clean' 32 | 33 | Command Line: 34 | Type "./hungarian -h" to prompt below usage options. 35 | 36 | ./hungarian 37 | -i #specify which assignment will be 38 | #imported from. contains a matrix. 39 | #see "example.demo" inside folder. 40 | #Default: randomly generating assingment 41 | -s #use random generator instead of input file, 42 | # should specify seed for generator, 43 | #this helps analyze one specific assignment 44 | #Default: time(Null) 45 | -n #if use random generator, tell me the size 46 | #Default: 3x3 47 | -p #specifiy "1" if you want to see plot, 48 | #Default: 0, no plot by default 49 | -t #if plot, and you want to see slide show, 50 | #then specify the interval period by second. 51 | #Default: 0, that is, need manually operated. 52 | -v #specify the verbose level, only allow 0,1,2 53 | #0: nothing will print onto screen but result 54 | #1: only print important steps information 55 | #2: print all information including sets result 56 | #Default: 1 57 | 58 | #The random generater by default can genenerate 59 | #natrual number from 0 to 100. You can grep and 60 | #change macro MAX_RANDOM for other values 61 | 62 | 63 | For instance: 64 | ./hungarian 65 | run Hungarian with a 3x3 random assignment everytime. 66 | ./hungarian -i example 67 | run Hungarian on this example assignment 68 | ./hungarian -s 10 69 | run Hungrian with random seed 10, on a default 3x3 assignment 70 | ./hungarian -s 10 -n 5 71 | run Hungarian with random seed 10, on a 5x5 assignment 72 | 73 | ./hungarian -p 1 74 | run Hungarian with gnuplot 75 | ./hungarian -p 1 -t 2 76 | run Hungarian with gnuplot, result is slide show (period = 2) 77 | ./hungarian -v 0 78 | run Hungarian, nothing will be printed on screen except result 79 | 80 | 81 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/example.demo: -------------------------------------------------------------------------------- 1 | #Lines after '#' is commented 2 | #This is an example file showing rules of specifying an assignment 3 | #The utility should be as below: 4 | 8 10 -4.5 # allow float and negative 5 | abc 12 6 # non-num "abc" is converted to 0 6 | # if you comment line below, it still works (a 2x3 matrix) 7 | 18 -3 5.138 8 | 9 | #The matrix could be square matrix, i.e., n X n, 10 | #if not square matrix, i.e., m X n, then it must be m <= n, i.e., row size <= column size 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /third-party/hungarian-v2.0/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | /////////////////////////////////////////////////////////////////////////////// 3 | // File name: main.cpp 4 | // This file calls the components and algorithms to complete Hungarian task. 5 | // Lantao Liu, Nov 1, 2009 6 | /////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include "CmdParser.h" 9 | #include "Assignment.h" 10 | #include "Hungarian.h" 11 | #include "BipartiteGraph.h" 12 | 13 | 14 | int main(int argc, char** argv) 15 | { 16 | //define a matrix; 17 | Matrix m; 18 | 19 | //parse command line and generate an assignment 20 | CmdParser parser(argc, argv); 21 | parser.DisplayInput(); 22 | 23 | Assignment as; 24 | if(parser.GetInputFileName().empty()){ 25 | if(parser.GetSeed()) 26 | as.SetSeed(parser.GetSeed()); 27 | else 28 | as.SetSeed(time(NULL)); 29 | cout< 5 | #include 6 | 7 | 8 | // FROM http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm 9 | // AUTHOR: x-ray 10 | 11 | #define N 55 //max number of vertices in one part 12 | #define INF 100000000 //just infinity 13 | 14 | int cost[N][N]; //cost matrix 15 | int n, max_match; //n workers and n jobs 16 | int lx[N], ly[N]; //labels of X and Y parts 17 | int xy[N]; //xy[x] - vertex that is matched with x, 18 | int yx[N]; //yx[y] - vertex that is matched with y 19 | bool S[N], T[N]; //sets S and T in algorithm 20 | int slack[N]; //as in the algorithm description 21 | int slackx[N]; //slackx[y] such a vertex, that 22 | // l(slackx[y]) + l(y) - w(slackx[y],y) = slack[y] 23 | int prev[N]; //array for memorizing alternating paths 24 | 25 | 26 | void init_labels(); 27 | 28 | void update_labels(); 29 | 30 | void add_to_tree(int x, int prevx); 31 | 32 | void augment(); 33 | 34 | int hungarian(); 35 | 36 | #endif 37 | 38 | -------------------------------------------------------------------------------- /third-party/tviewer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(ExternalProject) 2 | 3 | ExternalProject_Add(tviewer 4 | GIT_REPOSITORY https://github.com/taketwo/tviewer.git 5 | GIT_TAG 019c53e 6 | TIMEOUT 10 7 | CMAKE_ARGS -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX= -DWITH_QT=ON 8 | ) 9 | 10 | ExternalProject_Get_Property(tviewer INSTALL_DIR) 11 | 12 | set(LIBRARIES ${INSTALL_DIR}/lib/libtviewer.a) 13 | if(${VTK_MAJOR_VERSION} VERSION_LESS "6.0") 14 | list(APPEND LIBRARIES QVTK) 15 | endif() 16 | 17 | set(TVIEWER_LIBRARIES ${LIBRARIES} CACHE INTERNAL "TViewer libraries") 18 | set(TVIEWER_INCLUDE_DIR ${INSTALL_DIR}/include CACHE INTERNAL "TViewer include directory") 19 | set(TVIEWER_DEFINITIONS "-std=c++11 -DBOOST_PP_VARIADICS=1" CACHE STRING "TViewer compiler definitions") 20 | --------------------------------------------------------------------------------