├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake_modules ├── FindEigen3.cmake ├── FindFlann.cmake ├── FindNicp.cmake └── FindQGLViewer.cmake └── nicp ├── CMakeLists.txt ├── nicp ├── CMakeLists.txt ├── Doxyfile ├── aligner.cpp ├── aligner.h ├── alignernn.cpp ├── alignernn.h ├── alignerprojective.cpp ├── alignerprojective.h ├── bm_defs.h ├── bm_se3.h ├── cloud.cpp ├── cloud.h ├── correspondencefinder.cpp ├── correspondencefinder.h ├── correspondencefindernn.cpp ├── correspondencefindernn.h ├── correspondencefinderprojective.cpp ├── correspondencefinderprojective.h ├── definitions.cpp ├── definitions.h ├── depthimageconverter.cpp ├── depthimageconverter.h ├── depthimageconverterintegralimage.cpp ├── depthimageconverterintegralimage.h ├── gaussian.h ├── gaussian3.cpp ├── gaussian3.h ├── homogeneousvector4f.h ├── imageutils.cpp ├── imageutils.h ├── informationmatrix.h ├── informationmatrixcalculator.cpp ├── informationmatrixcalculator.h ├── linearizer.cpp ├── linearizer.h ├── merger.cpp ├── merger.h ├── nicp_aligner.cpp ├── nicp_simple_aligner.cpp ├── parallelcylindricalpointprojector.cpp ├── parallelcylindricalpointprojector.h ├── pinholepointprojector.cpp ├── pinholepointprojector.h ├── pointaccumulator.h ├── pointintegralimage.cpp ├── pointintegralimage.h ├── pointprojector.cpp ├── pointprojector.h ├── se3_prior.cpp ├── se3_prior.h ├── sphericalpointprojector.cpp ├── sphericalpointprojector.h ├── stats.h ├── statscalculator.cpp ├── statscalculator.h ├── statscalculatorintegralimage.cpp ├── statscalculatorintegralimage.h ├── transformable_vector.h ├── unscented.h ├── voxelcalculator.cpp └── voxelcalculator.h ├── nicp_conf ├── gicp_eth_kinect.conf ├── gicp_eth_laser.conf ├── kinfu_eth_kinect.conf ├── ndt_eth_kinect.conf ├── ndt_eth_laser.conf ├── nicp_aligner_1_1.conf ├── nicp_aligner_1_4.conf ├── nicp_eth_kinect.conf ├── nicp_eth_kinect_estimate_visualization.conf ├── nicp_eth_kinect_incremental.conf ├── nicp_eth_laser.conf ├── nicp_eth_laser_estimate_visualization.conf ├── nicp_eth_laser_nscale_33.conf └── nicp_eth_laser_nscale_66.conf ├── nicp_test ├── CMakeLists.txt └── nicp_cloud2spherical.cpp └── nicp_viewer ├── CMakeLists.txt ├── drawable.cpp ├── drawable.h ├── drawable_cloud.cpp ├── drawable_cloud.h ├── drawable_correspondences.cpp ├── drawable_correspondences.h ├── drawable_covariances.cpp ├── drawable_covariances.h ├── drawable_frame.h ├── drawable_normals.cpp ├── drawable_normals.h ├── drawable_points.cpp ├── drawable_points.h ├── drawable_trajectory.cpp ├── drawable_trajectory.h ├── drawable_transform_covariance.cpp ├── drawable_transform_covariance.h ├── drawable_uncertainty.cpp ├── drawable_uncertainty.h ├── gl_parameter.cpp ├── gl_parameter.h ├── gl_parameter_cloud.cpp ├── gl_parameter_cloud.h ├── gl_parameter_correspondences.cpp ├── gl_parameter_correspondences.h ├── gl_parameter_covariances.cpp ├── gl_parameter_covariances.h ├── gl_parameter_normals.cpp ├── gl_parameter_normals.h ├── gl_parameter_points.cpp ├── gl_parameter_points.h ├── gl_parameter_trajectory.cpp ├── gl_parameter_trajectory.h ├── gl_parameter_transform_covariance.cpp ├── gl_parameter_transform_covariance.h ├── gl_parameter_uncertainty.cpp ├── gl_parameter_uncertainty.h ├── imageview.cpp ├── imageview.h ├── nicp_aligner_gui.cpp ├── nicp_aligner_gui_main_window.cpp ├── nicp_aligner_gui_main_window.h ├── nicp_aligner_gui_ui_main_window.h ├── nicp_aligner_gui_ui_main_window.ui ├── nicp_cloud_prop_viewer.cpp ├── nicp_qglviewer.cpp ├── nicp_qglviewer.h ├── nicp_simple_viewer.cpp ├── opengl_primitives.cpp └── opengl_primitives.h /.gitignore: -------------------------------------------------------------------------------- 1 | bin 2 | build 3 | lib 4 | CMakeFiles 5 | CMakeCache.txt -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.5) 2 | 3 | PROJECT(nicp) 4 | 5 | # Optionally compiling part 6 | OPTION(BUILD_NICP_VIEWER "enables NICP viewer" false) 7 | OPTION(BUILD_NICP_TEST "enables NICP test" false) 8 | 9 | # The library prefix 10 | SET(LIB_PREFIX nicp) 11 | 12 | SET(nicp_C_FLAGS) 13 | SET(nicp_CXX_FLAGS) 14 | 15 | # default built type 16 | IF(NOT CMAKE_BUILD_TYPE) 17 | SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." FORCE) 18 | ENDIF(NOT CMAKE_BUILD_TYPE) 19 | 20 | # postfix, based on type 21 | SET(CMAKE_DEBUG_POSTFIX "_d" CACHE STRING "postfix applied to debug build of libraries") 22 | SET(CMAKE_RELEASE_POSTFIX "" CACHE STRING "postfix applied to release build of libraries") 23 | SET(CMAKE_RELWITHDEBINFO_POSTFIX "_rd" CACHE STRING "postfix applied to release-with-debug-information libraries") 24 | SET(CMAKE_MINSIZEREL_POSTFIX "_s" CACHE STRING "postfix applied to minimium-size-build libraries") 25 | 26 | # work out the postfix; required where we use OUTPUT_NAME 27 | IF(CMAKE_BUILD_TYPE MATCHES Release) 28 | SET(EXE_POSTFIX) 29 | ELSEIF(CMAKE_BUILD_TYPE MATCHES Debug) 30 | SET(EXE_POSTFIX ${CMAKE_DEBUG_POSTFIX}) 31 | ELSEIF(CMAKE_BUILD_TYPE MATCHES RelWithDebInfo) 32 | SET(EXE_POSTFIX ${CMAKE_RELWITHDEBINFO_POSTFIX}) 33 | ELSEIF(CMAKE_BUILD_TYPE MATCHES MinSizeRel) 34 | SET(EXE_POSTFIX ${CMAKE_MINSIZEREL_POSTFIX}) 35 | ENDIF(CMAKE_BUILD_TYPE MATCHES Release) 36 | 37 | # Set the output directory for the build executables and libraries 38 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${nicp_SOURCE_DIR}/lib) 39 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${nicp_SOURCE_DIR}/lib) 40 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${nicp_SOURCE_DIR}/bin) 41 | 42 | # Set search directory for looking for our custom CMake scripts to 43 | # look for Eigen3 44 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${nicp_SOURCE_DIR}/cmake_modules) 45 | 46 | ADD_DEFINITIONS(-DUNIX) 47 | MESSAGE(STATUS "Compiling on Unix") 48 | 49 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -msse4.2 -fopenmp -std=gnu++17") 50 | SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -msse4.2 -fopenmp") 51 | SET(nicp_C_FLAGS "${nicp_C_FLAGS} -Wall -W") 52 | SET(nicp_CXX_FLAGS "${nicp_CXX_FLAGS} -Wall -W") 53 | 54 | # Specifying compiler flags 55 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${nicp_CXX_FLAGS}") 56 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${nicp_C_FLAGS}") 57 | 58 | # Find Eigen3 59 | FIND_PACKAGE(Eigen3 3.2.0 REQUIRED) 60 | INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) 61 | 62 | # Find Flann 63 | FIND_PACKAGE(Flann REQUIRED) 64 | INCLUDE_DIRECTORIES(${FLANN_INCLUDE_DIR}) 65 | LINK_DIRECTORIES(${FLANN_LIBRARY} ) 66 | 67 | # Find OpenMP 68 | FIND_PACKAGE(OpenMP) 69 | IF(OPENMP_FOUND) 70 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 71 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -D_NICP_USE_OPENMP_ -DEIGEN_DONT_PARALLELIZE") 72 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 73 | ENDIF(OPENMP_FOUND) 74 | 75 | # Find Opencv 76 | FIND_PACKAGE(OpenCV 3.2.0 REQUIRED) 77 | INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) 78 | 79 | # Set up the top-level include directories 80 | INCLUDE_DIRECTORIES(${nicp_SOURCE_DIR}/nicp) 81 | 82 | IF(BUILD_NICP_VIEWER) 83 | # OpenGL is used in the draw actions for the different types, as well 84 | # as for creating the GUI itself 85 | FIND_PACKAGE(OpenGL REQUIRED) 86 | INCLUDE_DIRECTORIES(${OPENGL_INCLUDE}) 87 | MESSAGE(STATUS "Compiling with OpenGL support") 88 | 89 | # Find Qt5 90 | FIND_PACKAGE(Qt5 COMPONENTS Core Xml OpenGL Gui Widgets REQUIRED) 91 | INCLUDE_DIRECTORIES( 92 | ${Qt5Core_INCLUDE_DIRS} ${Qt5Xml_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} 93 | ${Qt5Widgets_INCLUDE_DIRS} ${Qt5OpenGL_INCLUDE_DIRS} 94 | ) 95 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") 96 | 97 | # For building the GUI 98 | FIND_PACKAGE(QGLViewer REQUIRED) 99 | INCLUDE_DIRECTORIES(${QGLVIEWER_INCLUDE_DIR}) 100 | ENDIF(BUILD_NICP_VIEWER) 101 | 102 | # Include the subdirectories 103 | ADD_SUBDIRECTORY(nicp) 104 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | NICP (Normal Iterative Closest Point) 2 | ==== 3 | 4 | NICP is a novel on-line method to recursively align point clouds. This method 5 | exploits the 3D structure to determine the data association between the two 6 | clouds taking into account each point and its local features of the surface: 7 | normals and curvature. 8 | This method adopt a line of sight criterion to find the corresponding points between 9 | the two clouds to register. This, together with the efficient algorithms and 10 | data structures used by NICP, increase the speed of the method allowing 11 | real-time computation. 12 | NICP solves the alignment problem by casting a least squares formulation that 13 | minimizes an error metric depending on both the point coordinates and the 14 | associated normal. This renders the algorithm more robust and accurate, thus 15 | computing better transformation. 16 | 17 | Tutorials and much more @ http://goo.gl/W3qXbE 18 | ---- 19 | 20 | System Requirements 21 | ---- 22 | 23 | ### Minimum 24 | 25 | - CMake >= 3.5 26 | - Eigen3 >= 3.2.0 27 | - OpenCV >= 3.2.0 28 | - Flann >= 1.7 29 | 30 | ### Complete 31 | 32 | - OpenGL 33 | - Qt5 34 | - QGLViewer 35 | 36 | Building on Linux (Ubuntu 18.04 LTS) 37 | ---- 38 | ``` 39 | $> sudo apt install git cmake libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev-qt5 libqglviewer2-qt5 libqglviewer-headers libflann-dev libopencv-dev freeglut3-dev 40 | 41 | $> git clone https://github.com/yorsh87/nicp.git 42 | 43 | $> cd nicp 44 | 45 | $> mkdir build 46 | 47 | $> cd build 48 | 49 | $> cmake .. 50 | 51 | $> make 52 | ``` 53 | Branches 54 | ---- 55 | 56 | - master : current stable branch 57 | - develop: current development branch 58 | - iros2015_experiments: code snapshot used for IROS 2015 paper publication [[pdf](http://jacoposerafin.com/wp-content/uploads/serafin15iros.pdf)] 59 | - ras2016_experiments: code snapshot used for RAS 2017 paper publication [[pdf](http://jacoposerafin.com/wp-content/uploads/serafin17ras.pdf)] 60 | 61 | Examples 62 | ---- 63 | 64 | Once you compiled the code you will have the following exmaple binaries: 65 | - `nicp_simple_aligner` is a binary that, given a set of depth images and a .txt file containing the list of depth images to align, perform the point cloud registrations 66 | - `nicp_aligner` same as nicp_simple_aligner, but it uses an incremental version of the algorithm 67 | - `nicp_aligner_gui` is a simple GUI for point cloud alignment, several parameters of the algorithm can be modified with GUI buttons 68 | - `nicp_cloud_prop_viewer` simple GUI to see basic properties of a depth image / point cloud 69 | - `nicp_simple_viewer` is a GUI that given a folder allows to visualize all the point clouds, saved in .nicp format, in the folder 70 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 3) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 2) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 1) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 57 | PATHS 58 | /usr/local/include/eigen3 59 | /usr/include/eigen3 60 | NO_DEFAULT_PATH 61 | ) 62 | 63 | if(EIGEN3_INCLUDE_DIR) 64 | _eigen3_check_version() 65 | endif(EIGEN3_INCLUDE_DIR) 66 | 67 | include(FindPackageHandleStandardArgs) 68 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 69 | 70 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 71 | 72 | 73 | -------------------------------------------------------------------------------- /cmake_modules/FindFlann.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find Flann 3 | # 4 | # This sets the following variables: 5 | # FLANN_FOUND - True if FLANN was found. 6 | # FLANN_INCLUDE_DIRS - Directories containing the FLANN include files. 7 | # FLANN_LIBRARIES - Libraries needed to use FLANN. 8 | # FLANN_DEFINITIONS - Compiler flags for FLANN. 9 | 10 | find_package(PkgConfig) 11 | pkg_check_modules(PC_FLANN flann) 12 | set(FLANN_DEFINITIONS ${PC_FLANN_CFLAGS_OTHER}) 13 | 14 | find_path(FLANN_INCLUDE_DIR flann/flann.hpp 15 | HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS}) 16 | 17 | find_library(FLANN_LIBRARY flann 18 | HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS}) 19 | 20 | set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR}) 21 | set(FLANN_LIBRARIES ${FLANN_LIBRARY}) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(Flann DEFAULT_MSG 25 | FLANN_LIBRARY FLANN_INCLUDE_DIR) 26 | 27 | mark_as_advanced(FLANN_LIBRARY FLANN_INCLUDE_DIR) -------------------------------------------------------------------------------- /cmake_modules/FindNicp.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | MESSAGE(STATUS "NICP_ROOT: " $ENV{NICP_ROOT}) 4 | 5 | FIND_PATH(NICP_INCLUDE_DIR nicp/aligner.h 6 | $ENV{NICP_ROOT}/nicp 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | MACRO(FIND_NICP_LIBRARY MYLIBRARY MYLIBRARYNAME) 19 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 20 | NAMES "${MYLIBRARYNAME}_d" 21 | PATHS 22 | $ENV{NICP_ROOT}/lib/Debug 23 | $ENV{NICP_ROOT}/lib 24 | NO_DEFAULT_PATH 25 | ) 26 | 27 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 28 | NAMES "${MYLIBRARYNAME}_d" 29 | PATHS 30 | ~/Library/Frameworks 31 | /Library/Frameworks 32 | /usr/local/lib 33 | /usr/local/lib64 34 | /usr/lib 35 | /usr/lib64 36 | /opt/local/lib 37 | /sw/local/lib 38 | /sw/lib 39 | ) 40 | 41 | FIND_LIBRARY(${MYLIBRARY} 42 | NAMES "${MYLIBRARYNAME}" 43 | PATHS 44 | $ENV{NICP_ROOT}/lib/Release 45 | $ENV{NICP_ROOT}/lib 46 | NO_DEFAULT_PATH 47 | ) 48 | 49 | FIND_LIBRARY(${MYLIBRARY} 50 | NAMES "${MYLIBRARYNAME}" 51 | PATHS 52 | ~/Library/Frameworks 53 | /Library/Frameworks 54 | /usr/local/lib 55 | /usr/local/lib64 56 | /usr/lib 57 | /usr/lib64 58 | /opt/local/lib 59 | /sw/local/lib 60 | /sw/lib 61 | ) 62 | 63 | IF(NOT ${MYLIBRARY}_DEBUG) 64 | IF(MYLIBRARY) 65 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 66 | ENDIF(MYLIBRARY) 67 | ENDIF(NOT ${MYLIBRARY}_DEBUG) 68 | ENDMACRO(FIND_NICP_LIBRARY LIBRARY LIBRARYNAME) 69 | 70 | # Find the core elements 71 | FIND_NICP_LIBRARY(NICP_LIBRARY nicp) 72 | FIND_NICP_LIBRARY(NICP_VIEWER_LIBRARY nicp_viewer) 73 | -------------------------------------------------------------------------------- /cmake_modules/FindQGLViewer.cmake: -------------------------------------------------------------------------------- 1 | FIND_PACKAGE(Qt5 COMPONENTS Core Xml OpenGL Gui Widgets) 2 | IF(NOT Qt5_FOUND) 3 | MESSAGE("Qt5 not found. Install it and set Qt5_DIR accordingly") 4 | IF (WIN32) 5 | MESSAGE(" In Windows, Qt5_DIR should be something like C:/Qt/5.4/msvc2013_64_opengl/lib/cmake/Qt5") 6 | ENDIF() 7 | ENDIF() 8 | 9 | FIND_PATH(QGLVIEWER_INCLUDE_DIR qglviewer.h 10 | /usr/include/QGLViewer 11 | /opt/local/include/QGLViewer 12 | /usr/local/include/QGLViewer 13 | /sw/include/QGLViewer 14 | ENV QGLVIEWERROOT 15 | ) 16 | 17 | find_library(QGLVIEWER_LIBRARY_RELEASE 18 | NAMES qglviewer QGLViewer qglviewer-qt5 QGLViewer-qt5 19 | PATHS /usr/lib 20 | /usr/local/lib 21 | /opt/local/lib 22 | /sw/lib 23 | ENV QGLVIEWERROOT 24 | ENV LD_LIBRARY_PATH 25 | ENV LIBRARY_PATH 26 | PATH_SUFFIXES QGLViewer QGLViewer/release 27 | ) 28 | find_library(QGLVIEWER_LIBRARY_DEBUG 29 | NAMES dqglviewer dQGLViewer dqglviewer-qt5 dQGLViewer-qt5 QGLViewerd2 30 | PATHS /usr/lib 31 | /usr/local/lib 32 | /opt/local/lib 33 | /sw/lib 34 | ENV QGLVIEWERROOT 35 | ENV LD_LIBRARY_PATH 36 | ENV LIBRARY_PATH 37 | PATH_SUFFIXES QGLViewer QGLViewer/debug 38 | ) 39 | 40 | if(QGLVIEWER_LIBRARY_RELEASE) 41 | if(QGLVIEWER_LIBRARY_DEBUG) 42 | set(QGLVIEWER_LIBRARY optimized ${QGLVIEWER_LIBRARY_RELEASE} debug ${QGLVIEWER_LIBRARY_DEBUG}) 43 | else() 44 | set(QGLVIEWER_LIBRARY ${QGLVIEWER_LIBRARY_RELEASE}) 45 | endif() 46 | endif() 47 | 48 | include(FindPackageHandleStandardArgs) 49 | find_package_handle_standard_args(QGLVIEWER DEFAULT_MSG 50 | QGLVIEWER_INCLUDE_DIR QGLVIEWER_LIBRARY) 51 | -------------------------------------------------------------------------------- /nicp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Include the subdirectories 2 | ADD_SUBDIRECTORY(nicp) 3 | IF(BUILD_NICP_VIEWER) 4 | ADD_SUBDIRECTORY(nicp_viewer) 5 | ENDIF(BUILD_NICP_VIEWER) 6 | IF(BUILD_NICP_TEST) 7 | ADD_SUBDIRECTORY(nicp_test) 8 | ENDIF(BUILD_NICP_TEST) -------------------------------------------------------------------------------- /nicp/nicp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_LIBRARY(nicp SHARED 2 | definitions.h 3 | imageutils.cpp imageutils.h 4 | aligner.cpp aligner.h 5 | alignernn.cpp alignernn.h 6 | alignerprojective.cpp alignerprojective.h 7 | correspondencefinder.cpp correspondencefinder.h 8 | correspondencefindernn.cpp correspondencefindernn.h 9 | correspondencefinderprojective.cpp correspondencefinderprojective.h 10 | depthimageconverter.cpp depthimageconverter.h 11 | depthimageconverterintegralimage.cpp depthimageconverterintegralimage.h 12 | cloud.cpp cloud.h 13 | gaussian.h 14 | gaussian3.cpp gaussian3.h 15 | homogeneousvector4f.h 16 | informationmatrix.h 17 | informationmatrixcalculator.h informationmatrixcalculator.cpp 18 | linearizer.cpp linearizer.h 19 | merger.cpp merger.h 20 | pinholepointprojector.cpp pinholepointprojector.h 21 | pointaccumulator.h 22 | pointintegralimage.cpp pointintegralimage.h 23 | pointprojector.cpp pointprojector.h 24 | se3_prior.cpp se3_prior.h 25 | parallelcylindricalpointprojector.cpp parallelcylindricalpointprojector.h 26 | stats.h 27 | statscalculator.cpp statscalculator.h 28 | statscalculatorintegralimage.cpp statscalculatorintegralimage.h 29 | sphericalpointprojector.cpp sphericalpointprojector.h 30 | transformable_vector.h 31 | voxelcalculator.cpp voxelcalculator.h 32 | ) 33 | SET_TARGET_PROPERTIES(nicp PROPERTIES OUTPUT_NAME ${LIB_PREFIX}) 34 | TARGET_LINK_LIBRARIES(nicp ${OpenCV_LIBS} ${FLANN_LIBRARY}) 35 | 36 | ADD_EXECUTABLE(nicp_simple_aligner nicp_simple_aligner.cpp ) 37 | SET_TARGET_PROPERTIES(nicp_simple_aligner PROPERTIES OUTPUT_NAME nicp_simple_aligner) 38 | TARGET_LINK_LIBRARIES(nicp_simple_aligner nicp ${OpenCV_LIBS}) 39 | 40 | ADD_EXECUTABLE(nicp_aligner nicp_aligner.cpp ) 41 | SET_TARGET_PROPERTIES(nicp_aligner PROPERTIES OUTPUT_NAME nicp_aligner) 42 | TARGET_LINK_LIBRARIES(nicp_aligner nicp ${OpenCV_LIBS}) 43 | -------------------------------------------------------------------------------- /nicp/nicp/aligner.cpp: -------------------------------------------------------------------------------- 1 | #include "aligner.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include "nicp/imageutils.h" 8 | 9 | #include "unscented.h" 10 | #include "bm_se3.h" 11 | 12 | #include 13 | #include "sphericalpointprojector.h" 14 | #include "pinholepointprojector.h" 15 | #include 16 | 17 | using namespace std; 18 | 19 | namespace nicp { 20 | 21 | Aligner::Aligner() { 22 | _projector = 0; 23 | _linearizer = 0; 24 | _correspondenceFinder = 0; 25 | _referenceCloud = 0; 26 | _currentCloud = 0; 27 | _outerIterations = 10; 28 | _innerIterations = 1; 29 | _T = Eigen::Isometry3f::Identity(); 30 | _initialGuess = Eigen::Isometry3f::Identity(); 31 | _referenceSensorOffset = Eigen::Isometry3f::Identity(); 32 | _currentSensorOffset = Eigen::Isometry3f::Identity(); 33 | _totalTime = 0.0f; 34 | _error = 0.0f; 35 | _inliers = 0; 36 | _minInliers = 100; 37 | _rotationalMinEigenRatio = 50; 38 | _translationalMinEigenRatio = 50; 39 | _debug = false; 40 | _lambda = 1e3; 41 | }; 42 | 43 | Aligner::Aligner(PointProjector* projector_, 44 | Linearizer* linearizer_, 45 | CorrespondenceFinder* correspondenceFinder_) { 46 | _projector = projector_; 47 | _linearizer = linearizer_; 48 | _correspondenceFinder = correspondenceFinder_; 49 | _referenceCloud = 0; 50 | _currentCloud = 0; 51 | _outerIterations = 10; 52 | _innerIterations = 1; 53 | _T = Eigen::Isometry3f::Identity(); 54 | _initialGuess = Eigen::Isometry3f::Identity(); 55 | _referenceSensorOffset = Eigen::Isometry3f::Identity(); 56 | _currentSensorOffset = Eigen::Isometry3f::Identity(); 57 | _totalTime = 0.0f; 58 | _error = 0.0f; 59 | _inliers = 0; 60 | _minInliers = 100; 61 | _rotationalMinEigenRatio = 50; 62 | _translationalMinEigenRatio = 50; 63 | _debug = false; 64 | _lambda = 1e3; 65 | }; 66 | 67 | void Aligner::addRelativePrior(const Eigen::Isometry3f &mean, const Matrix6f &informationMatrix) { 68 | _priors.push_back(new SE3RelativePrior(mean, informationMatrix)); 69 | } 70 | 71 | void Aligner::addAbsolutePrior(const Eigen::Isometry3f &referenceTransform, const Eigen::Isometry3f &mean, const Matrix6f &informationMatrix) { 72 | _priors.push_back(new SE3AbsolutePrior(referenceTransform, mean, informationMatrix)); 73 | } 74 | 75 | void Aligner::clearPriors() { 76 | for(size_t i = 0; i < _priors.size(); i++) { 77 | delete _priors[i]; 78 | } 79 | _priors.clear(); 80 | } 81 | 82 | void Aligner::_computeStatistics(Vector6f &mean, Matrix6f &Omega, 83 | float &translationalRatio, float &rotationalRatio, bool usePriors) const { 84 | typedef SigmaPoint SigmaPoint; 85 | typedef std::vector > SigmaPointVector; 86 | 87 | // Output init 88 | Matrix6f H; 89 | H.setZero(); 90 | translationalRatio = std::numeric_limits::max(); 91 | rotationalRatio = std::numeric_limits::max(); 92 | 93 | Eigen::Isometry3f invT = _T.inverse(); 94 | invT.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f; 95 | _linearizer->setT(invT); 96 | _linearizer->update(); 97 | H += _linearizer->H(); 98 | 99 | // Add the priors 100 | for(size_t j = 0; usePriors && j < _priors.size(); j++) { 101 | const SE3Prior *prior = _priors[j]; 102 | Matrix6f priorJacobian = prior->jacobian(invT); 103 | Matrix6f priorInformationRemapped = prior->errorInformation(invT); 104 | Matrix6f Hp = priorJacobian.transpose() * priorInformationRemapped * priorJacobian; 105 | H += Hp; 106 | } 107 | 108 | JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV); 109 | Matrix6f localSigma = svd.solve(Matrix6f::Identity()); 110 | SigmaPointVector sigmaPoints; 111 | Vector6f localMean = Vector6f::Zero(); 112 | sampleUnscented(sigmaPoints, localMean, localSigma); 113 | 114 | Eigen::Isometry3f dT = _T; // Transform from current to reference 115 | 116 | // Remap each of the sigma points to their original position 117 | //#pragma omp parallel 118 | for (size_t i = 0; i < sigmaPoints.size(); i++) { 119 | SigmaPoint &p = sigmaPoints[i]; 120 | p._sample = t2v(dT * v2t(p._sample).inverse()); 121 | } 122 | // Reconstruct the gaussian 123 | reconstructGaussian(mean, localSigma, sigmaPoints); 124 | 125 | // Compute the information matrix from the covariance 126 | Omega = localSigma.inverse(); 127 | Omega = .5* (Omega + Omega.transpose()); 128 | // Have a look at the svd of the rotational and the translational part; 129 | JacobiSVD partialSVD; 130 | partialSVD.compute(Omega.block<3, 3>(0, 0)); 131 | translationalRatio = partialSVD.singularValues()(0) / partialSVD.singularValues()(2); 132 | 133 | partialSVD.compute(Omega.block<3, 3>(3, 3)); 134 | rotationalRatio = partialSVD.singularValues()(0) / partialSVD.singularValues()(2); 135 | } 136 | 137 | } 138 | -------------------------------------------------------------------------------- /nicp/nicp/alignernn.cpp: -------------------------------------------------------------------------------- 1 | #include "alignernn.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include "nicp/imageutils.h" 8 | 9 | #include "unscented.h" 10 | #include "bm_se3.h" 11 | 12 | #include 13 | #include "sphericalpointprojector.h" 14 | #include "pinholepointprojector.h" 15 | #include 16 | 17 | using namespace std; 18 | 19 | namespace nicp { 20 | 21 | void AlignerNN::align() { 22 | 23 | assert(_projector && "AlignerProjective: missing _projector"); 24 | assert(_linearizer && "AlignerProjective: missing _linearizer"); 25 | assert(_correspondenceFinder && "AlignerProjective: missing _correspondenceFinder"); 26 | assert(_referenceCloud && "AlignerProjective: missing _referenceCloud"); 27 | assert(_currentCloud && "AlignerProjective: missing _currentCloud"); 28 | 29 | CorrespondenceFinderNN* cfnn = dynamic_cast(_correspondenceFinder); 30 | assert(cfnn && "AlignerProjective: _correspondenceFinder is not of type CorrespondnceFinderNN"); 31 | 32 | struct timeval tvStart, tvEnd; 33 | gettimeofday(&tvStart, 0); 34 | 35 | cfnn->init(*_referenceCloud, *_currentCloud); 36 | _T = _initialGuess; 37 | for(int i = 0; i < _outerIterations; i++) { 38 | /************************************************************************ 39 | * Correspondence Computation * 40 | ************************************************************************/ 41 | // Compute the indices of the current scene from the point of view of the sensor 42 | _T.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f; 43 | 44 | // Correspondences computation. 45 | cfnn->compute(*_referenceCloud, *_currentCloud, _T.inverse()); 46 | 47 | /************************************************************************ 48 | * Alignment * 49 | ************************************************************************/ 50 | Eigen::Isometry3f invT = _T.inverse(); 51 | for(int k = 0; k < _innerIterations; k++) { 52 | invT.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f; 53 | Matrix6f H; 54 | Vector6f b; 55 | 56 | _linearizer->setT(invT); 57 | _linearizer->update(); 58 | H = _linearizer->H(); 59 | b = _linearizer->b(); 60 | H += Matrix6f::Identity() * _lambda; 61 | 62 | // Add the priors 63 | for(size_t j = 0; j < _priors.size(); j++) { 64 | const SE3Prior *prior = _priors[j]; 65 | Vector6f priorError = prior->error(invT); 66 | Matrix6f priorJacobian = prior->jacobian(invT); 67 | Matrix6f priorInformationRemapped = prior->errorInformation(invT); 68 | 69 | Matrix6f Hp = priorJacobian.transpose() * priorInformationRemapped * priorJacobian; 70 | Vector6f bp = priorJacobian.transpose() * priorInformationRemapped * priorError; 71 | 72 | H += Hp; 73 | b += bp; 74 | } 75 | 76 | Vector6f dx = H.ldlt().solve(-b); 77 | Eigen::Isometry3f dT = v2t(dx); 78 | invT = dT * invT; 79 | } 80 | 81 | _T = invT.inverse(); 82 | _T = v2t(t2v(_T)); 83 | _T.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f; 84 | } 85 | 86 | gettimeofday(&tvEnd, 0); 87 | double tStart = tvStart.tv_sec * 1000.0f + tvStart.tv_usec * 0.001f; 88 | double tEnd = tvEnd.tv_sec * 1000.0f + tvEnd.tv_usec * 0.001f; 89 | _totalTime = tEnd - tStart; 90 | _error = _linearizer->error(); 91 | _inliers = _linearizer->inliers(); 92 | 93 | _computeStatistics(_mean, _omega, _translationalEigenRatio, _rotationalEigenRatio); 94 | if (_rotationalEigenRatio > _rotationalMinEigenRatio || 95 | _translationalEigenRatio > _translationalMinEigenRatio) { 96 | _solutionValid = false; 97 | if (_debug) { 98 | cout << endl; 99 | cout << "************** WARNING SOLUTION MIGHT BE INVALID (eigenratio failure) **************" << endl; 100 | cout << "tr: " << _translationalEigenRatio << " rr: " << _rotationalEigenRatio << endl; 101 | cout << "************************************************************************************" << endl; 102 | } 103 | } 104 | else { 105 | _solutionValid = true; 106 | if (_debug) { 107 | cout << "************** I FOUND SOLUTION VALID SOLUTION (eigenratio ok) *******************" << endl; 108 | cout << "tr: " << _translationalEigenRatio << " rr: " << _rotationalEigenRatio << endl; 109 | cout << "************************************************************************************" << endl; 110 | } 111 | } 112 | if (_debug) { 113 | cout << "Solution statistics in (t, mq): " << endl; 114 | cout << "mean: " << _mean.transpose() << endl; 115 | cout << "Omega: " << endl; 116 | cout << _omega << endl; 117 | } 118 | } 119 | 120 | } 121 | -------------------------------------------------------------------------------- /nicp/nicp/alignernn.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "aligner.h" 4 | #include "correspondencefindernn.h" 5 | 6 | namespace nicp { 7 | 8 | /** \class Aligner aligner.h "aligner.h" 9 | * \brief Class for point cloud alignment. 10 | * 11 | * This class allows to compute the homogeneous transformation that brings 12 | * a point cloud to superpose an other reference point cloud. Data association 13 | * is computed via nearest neighbor. 14 | */ 15 | class AlignerNN : public Aligner { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 18 | 19 | /** 20 | * Empty constructor. 21 | * This constructor creates a Projective Aligner with default values for all its attributes. 22 | * All the pointers to objects implementing an algorithm have to be set since 23 | * this constructor sets them to zero. 24 | */ 25 | AlignerNN() : Aligner() {} 26 | 27 | /** 28 | * This constructor creates an Aligner with default values for all its attributes. 29 | * All internal algorithms are set using the input values. 30 | * @param projector_ is a pointer to the point projector that will be used by the Aligner. 31 | * @param linearizer_ is a pointer to the linearizer that will be used by the Aligner. 32 | * @param correspondenceFinder_ is a pointer to the point correspondence finder that will be used by the Aligner. 33 | */ 34 | AlignerNN(PointProjector* projector_, Linearizer* linearizer_, CorrespondenceFinderNN* correspondenceFinder_) : Aligner(projector_, linearizer_, correspondenceFinder_) {} 35 | 36 | /** 37 | * Destructor. 38 | */ 39 | virtual ~AlignerNN() {} 40 | 41 | /** 42 | * This method computes the final transformation that brings the cloud to align to superpose the reference 43 | * cloud. 44 | */ 45 | virtual void align(); 46 | 47 | }; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /nicp/nicp/alignerprojective.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "aligner.h" 4 | #include "correspondencefinderprojective.h" 5 | 6 | namespace nicp { 7 | 8 | /** \class Aligner aligner.h "aligner.h" 9 | * \brief Class for point cloud alignment. 10 | * 11 | * This class allows to compute the homogeneous transformation that brings 12 | * a point cloud to superpose an other reference point cloud. Data association 13 | * is computed via depth image projection. 14 | */ 15 | class AlignerProjective : public Aligner { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 18 | 19 | /** 20 | * Empty constructor. 21 | * This constructor creates a Projective Aligner with default values for all its attributes. 22 | * All the pointers to objects implementing an algorithm have to be set since 23 | * this constructor sets them to zero. 24 | */ 25 | AlignerProjective() : Aligner() {} 26 | 27 | /** 28 | * This constructor creates an Aligner with default values for all its attributes. 29 | * All internal algorithms are set using the input values. 30 | * @param projector_ is a pointer to the point projector that will be used by the Aligner. 31 | * @param linearizer_ is a pointer to the linearizer that will be used by the Aligner. 32 | * @param correspondenceFinder_ is a pointer to the point correspondence finder that will be used by the Aligner. 33 | */ 34 | AlignerProjective(PointProjector* projector_, Linearizer* linearizer_, CorrespondenceFinderProjective* correspondenceFinder_) : Aligner(projector_, linearizer_, correspondenceFinder_) {} 35 | 36 | /** 37 | * Destructor. 38 | */ 39 | virtual ~AlignerProjective() {} 40 | 41 | /** 42 | * This method computes the final transformation that brings the cloud to align to superpose the reference 43 | * cloud. 44 | */ 45 | virtual void align(); 46 | 47 | }; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /nicp/nicp/bm_defs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace nicp { 9 | 10 | typedef Eigen::DiagonalMatrix Diagonal3f; 11 | typedef Eigen::Matrix Vector6f; 12 | typedef Eigen::Matrix Matrix6f; 13 | typedef Eigen::Matrix Matrix2x4f; 14 | 15 | typedef Eigen::Matrix Matrix12f; 16 | typedef Eigen::Matrix Matrix6x12f; 17 | typedef Eigen::Matrix Vector12f; 18 | 19 | 20 | typedef Eigen::DiagonalMatrix Diagonal3d; 21 | typedef Eigen::Matrix Vector6d; 22 | typedef Eigen::Matrix Matrix6d; 23 | typedef Eigen::Matrix Matrix2x4d; 24 | 25 | typedef Eigen::Matrix Matrix12d; 26 | typedef Eigen::Matrix Matrix6x12d; 27 | typedef Eigen::Matrix Vector12d; 28 | 29 | 30 | typedef Eigen::Matrix MatrixXus; 31 | 32 | } 33 | -------------------------------------------------------------------------------- /nicp/nicp/bm_se3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "bm_defs.h" 6 | 7 | namespace nicp { 8 | 9 | template 10 | inline Eigen::Matrix quat2mat(const Eigen::MatrixBase& q) { 11 | const typename Derived::Scalar& qx = q.x(); 12 | const typename Derived::Scalar& qy = q.y(); 13 | const typename Derived::Scalar& qz = q.z(); 14 | typename Derived::Scalar qw = sqrt(1.f - q.squaredNorm()); 15 | Eigen::Matrix R; 16 | R << qw*qw + qx*qx - qy*qy - qz*qz, 2*(qx*qy - qw*qz) , 2*(qx*qz + qw*qy), 17 | 2*(qx*qy + qz*qw) , qw*qw - qx*qx + qy*qy - qz*qz, 2*(qy*qz - qx*qw), 18 | 2*(qx*qz - qy*qw) , 2*(qy*qz + qx*qw), qw*qw - qx*qx - qy*qy + qz*qz; 19 | 20 | return R; 21 | } 22 | 23 | 24 | template 25 | inline Eigen::Matrix mat2quat(const Eigen::MatrixBase& R) { 26 | Eigen::Quaternion q(R); 27 | q.normalize(); 28 | Eigen::Matrix rq; 29 | rq << q.x(), q.y(), q.z(); 30 | if (q.w()<0){ 31 | rq = -rq; 32 | } 33 | return rq; 34 | } 35 | 36 | template 37 | inline Eigen::Transform v2t(const Eigen::MatrixBase& x_) { 38 | Eigen::Transform X; 39 | Eigen::Matrix x(x_); 40 | X.template linear() = quat2mat(x.template block<3,1>(3,0)); 41 | X.template translation() = x.template block<3,1>(0,0); 42 | return X; 43 | } 44 | 45 | template 46 | inline Eigen::Matrix t2v(const Eigen::Transform& X) 47 | { 48 | Eigen::Matrix v; 49 | v.template block<3,1>(0,0) = X.translation(); 50 | v.template block<3,1>(3,0) = mat2quat(X.linear()); 51 | return v; 52 | } 53 | 54 | template 55 | inline Eigen::Matrix::RowsAtCompileTime, Eigen::MatrixBase::RowsAtCompileTime> skew(const Eigen::MatrixBase& v) 56 | { 57 | typename Derived::Scalar tx = 2*v.x(); 58 | typename Derived::Scalar ty = 2*v.y(); 59 | typename Derived::Scalar tz = 2*v.z(); 60 | Eigen::Matrix::RowsAtCompileTime, Eigen::MatrixBase::RowsAtCompileTime> S = Eigen::Matrix::RowsAtCompileTime, Eigen::MatrixBase::RowsAtCompileTime>::Zero(); 61 | S.coeffRef(0,1)= tz; S.coeffRef(1,0)=-tz; 62 | S.coeffRef(0,2)=-ty; S.coeffRef(2,0)= ty; 63 | S.coeffRef(1,2)= tx; S.coeffRef(2,1)=-tx; 64 | 65 | return S; 66 | } 67 | 68 | template 69 | inline Eigen::Matrix homogeneous2vector(const Eigen::MatrixBase& transform){ 70 | Eigen::Matrix x; 71 | x.template block<3,1>(0,0)=transform.template block<1,3>(0,0).transpose(); 72 | x.template block<3,1>(3,0)=transform.template block<1,3>(1,0).transpose(); 73 | x.template block<3,1>(6,0)=transform.template block<1,3>(2,0).transpose(); 74 | x.template block<3,1>(9,0)=transform.template block<3,1>(0,3); 75 | return x; 76 | } 77 | 78 | 79 | template 80 | inline Eigen::Matrix vector2homogeneous(const Eigen::MatrixBase& x){ 81 | Eigen::Matrix transform= Eigen::Matrix::Identity(); 82 | transform.template block<1,3>(0,0)=x.template block<3,1>(0,0).transpose(); 83 | transform.template block<1,3>(1,0)=x.template block<3,1>(3,0).transpose(); 84 | transform.template block<1,3>(2,0)=x.template block<3,1>(6,0).transpose(); 85 | transform.template block<1,3>(3,0)=x.template block<3,1>(9,0); 86 | return transform; 87 | } 88 | 89 | 90 | inline Eigen::Matrix4f skew4f(const Eigen::Vector3f& v) 91 | { 92 | const float& tx = v.x(); 93 | const float& ty = v.y(); 94 | const float& tz = v.z(); 95 | Eigen::Matrix4f S; 96 | S << 0, (2*tz), (-2*ty), 0, 97 | (-2*tz), 0, (2*tx), 0, 98 | (2*ty), (-2*tx), 0, 0, 99 | 0, 0, 0, 0; 100 | return S; 101 | } 102 | 103 | inline float t2angle(const Eigen::Isometry3d& iso){ 104 | Eigen::AngleAxisd aa(iso.linear()); 105 | float angle = aa.angle(); 106 | if (aa.axis().z()<0){ 107 | angle=-angle; 108 | } 109 | return angle; 110 | } 111 | 112 | } 113 | -------------------------------------------------------------------------------- /nicp/nicp/correspondencefinder.cpp: -------------------------------------------------------------------------------- 1 | #include "correspondencefinder.h" 2 | 3 | #include 4 | 5 | using namespace std; 6 | 7 | namespace nicp { 8 | 9 | CorrespondenceFinder::CorrespondenceFinder() { 10 | _inlierDistanceThreshold = 1.0f; 11 | _squaredThreshold = _inlierDistanceThreshold * _inlierDistanceThreshold; 12 | _inlierNormalAngularThreshold = cosf(M_PI/6); 13 | _flatCurvatureThreshold = 0.2f; 14 | _inlierCurvatureRatioThreshold = 1.3f; 15 | _numCorrespondences = 0; 16 | _correspondences.clear(); 17 | _demotedToGICP = false; 18 | } 19 | 20 | } 21 | -------------------------------------------------------------------------------- /nicp/nicp/correspondencefindernn.cpp: -------------------------------------------------------------------------------- 1 | #include "correspondencefindernn.h" 2 | 3 | namespace nicp { 4 | 5 | CorrespondenceFinderNN::CorrespondenceFinderNN(): CorrespondenceFinder() { 6 | _index = 0; 7 | _knn = 1; 8 | _normal_scaling = 1.0f; 9 | } 10 | 11 | CorrespondenceFinderNN::~CorrespondenceFinderNN() { if(_index) { delete _index; } } 12 | 13 | void CorrespondenceFinderNN::_model2linear(std::vector& dest, const Cloud& src, float nscale){ 14 | dest.resize(src.points().size() * 6); 15 | float* dp = &dest[0]; 16 | for(size_t i=0; i < src.points().size(); ++i) { 17 | const Point& p = src.points()[i]; 18 | const Normal& n = nscale * src.normals()[i]; 19 | *dp = p.x(); ++dp; 20 | *dp = p.y(); ++dp; 21 | *dp = p.z(); ++dp; 22 | *dp = n.x(); ++dp; 23 | *dp = n.y(); ++dp; 24 | *dp = n.z(); ++dp; 25 | } 26 | } 27 | 28 | void CorrespondenceFinderNN::_model2linear(std::vector& dest, const Cloud& src, float nscale, const Eigen::Isometry3f& T) { 29 | dest.resize(src.points().size() * 6); 30 | float* dp = &dest[0]; 31 | Eigen::Matrix3f sR = T.linear() * nscale; 32 | for(size_t i = 0; i < src.points().size(); ++i) { 33 | Point p = T * src.points()[i]; 34 | Eigen::Vector3f n = sR * src.normals()[i].head<3>(); 35 | *dp=p.x(); ++dp; 36 | *dp=p.y(); ++dp; 37 | *dp=p.z(); ++dp; 38 | *dp=n.x(); ++dp; 39 | *dp=n.y(); ++dp; 40 | *dp=n.z(); ++dp; 41 | } 42 | } 43 | 44 | void CorrespondenceFinderNN::init(const Cloud &referenceScene, const Cloud ¤tScene) { 45 | _model2linear(_current_points, currentScene, _normal_scaling); 46 | if(_index) { delete(_index); } 47 | _index = 0; 48 | 49 | _current_matrix = flann::Matrix (&_current_points[0], 50 | currentScene.points().size(), 51 | 6); 52 | _index = new flann::Index< flann::L2 >(_current_matrix, flann::KDTreeIndexParams()); 53 | _index->buildIndex(); 54 | _model2linear(_reference_points, referenceScene, _normal_scaling); 55 | _reference_matrix = flann::Matrix (&_reference_points[0], 56 | referenceScene.points().size(), 57 | 6); 58 | _reference_indices.resize(referenceScene.points().size() * _knn); 59 | _indices_matrix = flann::Matrix(&_reference_indices[0], 60 | referenceScene.points().size(), 61 | _knn); 62 | _reference_distances.resize(referenceScene.points().size() * _knn); 63 | _distances_matrix = flann::Matrix(&_reference_distances[0], 64 | referenceScene.points().size(), 65 | _knn); 66 | _correspondences.reserve(_indices_matrix.rows * _indices_matrix.cols); 67 | } 68 | 69 | void CorrespondenceFinderNN::compute(const Cloud &referenceScene, const Cloud ¤tScene, Eigen::Isometry3f T) { 70 | _model2linear(_reference_points, referenceScene, _normal_scaling, T); 71 | 72 | flann::SearchParams params(16); 73 | params.cores = 8; 74 | _index->radiusSearch(_reference_matrix, _indices_matrix, _distances_matrix, 75 | _squaredThreshold, flann::SearchParams(16)); 76 | 77 | _correspondences.clear(); 78 | _numCorrespondences = 0; 79 | for(size_t ridx = 0; ridx < _indices_matrix.rows; ++ridx) { 80 | int* currentIndexPtr = _indices_matrix.ptr() + (ridx * _knn); 81 | Eigen::Vector3f rn = T.linear() * referenceScene.normals()[ridx].head<3>(); 82 | Eigen::Vector3f rp = T * referenceScene.points()[ridx].head<3>(); 83 | for(size_t j = 0; j < _indices_matrix.cols; ++j) { 84 | int cidx = *currentIndexPtr; 85 | currentIndexPtr++; 86 | if(cidx < 0) { continue; } 87 | const Eigen::Vector3f& cn = currentScene.normals()[cidx].head<3>(); 88 | const Eigen::Vector3f& cp = currentScene.points()[cidx].head<3>(); 89 | if(!_demotedToGICP && (rn.squaredNorm() == 0.0f || cn.squaredNorm() == 0.0f)) { continue; } 90 | if((rp - cp).squaredNorm() > _squaredThreshold) { continue; } 91 | if(isNan(cn) || isNan(rn)) { continue; } 92 | if(!_demotedToGICP && rn.dot(cn) < _inlierNormalAngularThreshold) { continue; } 93 | _correspondences.push_back(Correspondence(ridx, cidx)); 94 | _numCorrespondences++; 95 | } 96 | } 97 | } 98 | 99 | } 100 | -------------------------------------------------------------------------------- /nicp/nicp/correspondencefindernn.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "correspondencefinder.h" 6 | 7 | namespace nicp { 8 | 9 | /** \class CorrespondenceFinderNN correspondencefindernn.h "correspondencefindernn.h" 10 | * \brief Class that can be used to find correspondences between two point clouds. 11 | * Data association is computed via nearest neighbor. 12 | * 13 | * This class has the objective to find correspondences between two point clouds using a 14 | * nearest neighbor approach. A correspondence has to satisfy some constraint like 15 | * maximum angle between normals, maximum distance between the points and so on. 16 | */ 17 | class CorrespondenceFinderNN : public CorrespondenceFinder { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 20 | 21 | /** 22 | * Empty constructor. 23 | * This constructor creates a CorrespondenceFinder with default values for all its attributes. 24 | */ 25 | CorrespondenceFinderNN(); 26 | 27 | /** 28 | * Destructor. 29 | */ 30 | virtual ~CorrespondenceFinderNN(); 31 | 32 | /** 33 | * Method that returns the value of the flag normal scaling. 34 | * @return the value of the normal scaling flag. 35 | * @see setNormalScaling() 36 | */ 37 | float normalScaling() const { return _normal_scaling; } 38 | 39 | /** 40 | * Method that set the normal scaling flag. 41 | * @param normal_scaling_ new normal scaling flag value. 42 | * @see normalScaling() 43 | */ 44 | void setNormalScaling(float normal_scaling_) { _normal_scaling = normal_scaling_; } 45 | 46 | /** 47 | * This method initialize the correspondence finder and should be called by the aligner 48 | * before starting the alignment. See AlignerNN to have an idea of how to use it. 49 | * @param referenceScene is a reference to the first point cloud to use to compute the Correspondence. 50 | * @param currentScene is a reference to the second point cloud to use to compute the Correspondence. 51 | */ 52 | void init(const Cloud &referenceScene, const Cloud ¤tScene); 53 | 54 | /** 55 | * This method computes the vector of correspondece of the two point cloud given in input. 56 | * @param referenceScene is a reference to the first point cloud to use to compute the Correspondence. 57 | * @param currentScene is a reference to the second point cloud to use to compute the Correspondence. 58 | * @param T is an isometry that is applied to the first point cloud before to compute the Correspondence. 59 | */ 60 | virtual void compute(const Cloud &referenceScene, const Cloud ¤tScene, Eigen::Isometry3f T); 61 | 62 | protected: 63 | void _model2linear(std::vector& dest, const Cloud& src, float nscale); 64 | void _model2linear(std::vector& dest, const Cloud& src, float nscale, const Eigen::Isometry3f& T); 65 | 66 | int _knn; 67 | float _normal_scaling; /**< scale the weight of the normals in the data association. */ 68 | flann::Matrix _reference_matrix; 69 | flann::Matrix _current_matrix; 70 | flann::Matrix _indices_matrix; 71 | flann::Matrix _distances_matrix; 72 | flann::Index > *_index; 73 | 74 | std::vector _reference_points; 75 | std::vector _current_points; 76 | std::vector _reference_indices; 77 | std::vector _reference_distances; 78 | }; 79 | 80 | } 81 | -------------------------------------------------------------------------------- /nicp/nicp/definitions.cpp: -------------------------------------------------------------------------------- 1 | #include "definitions.h" 2 | 3 | namespace nicp { 4 | 5 | void scaleDepthImage(DepthImage& dest, const DepthImage& src, int step){ 6 | int rows = src.rows/step; 7 | int cols = src.cols/step; 8 | dest = DepthImage(rows,cols,0.0f); 9 | for (int c = 0; c(sr+j,sc+i); 20 | np += src.at(sr+j,sc+i) > 0; 21 | } 22 | } 23 | } 24 | if (np) 25 | dest.at(r,c) = acc/np; 26 | } 27 | } 28 | } 29 | 30 | } 31 | -------------------------------------------------------------------------------- /nicp/nicp/definitions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "opencv2/opencv.hpp" 5 | #include "gaussian.h" 6 | 7 | namespace nicp { 8 | 9 | /** \typedef Diagonal3f 10 | * \brief A 3x3 diagonal matrix. 11 | */ 12 | typedef Eigen::DiagonalMatrix Diagonal3f; 13 | 14 | /** \typedef UnsignedCharImage 15 | * \brief An unsigned char cv::Mat. 16 | */ 17 | typedef cv::Mat_ UnsignedCharImage; 18 | 19 | /** \typedef CharImage 20 | * \brief A char cv::Mat. 21 | */ 22 | typedef cv::Mat_ CharImage; 23 | 24 | /** \typedef UnsignedShortImage 25 | * \brief An unsigned short cv::Mat. 26 | */ 27 | typedef cv::Mat_ UnsignedShortImage; 28 | 29 | /** \typedef UnsignedIntImage 30 | * \brief An unsigned int cv::Mat. 31 | */ 32 | typedef cv::Mat_ UnsignedIntImage; 33 | 34 | /** \typedef IntImage 35 | * \brief An int cv::Mat. 36 | */ 37 | typedef cv::Mat_ IntImage; 38 | 39 | /** \typedef IntervalImage 40 | * \brief An int cv::Mat. 41 | */ 42 | typedef cv::Mat_ IntervalImage; 43 | 44 | /** \typedef FloatImage 45 | * \brief A float cv::Mat. 46 | */ 47 | typedef cv::Mat_ FloatImage; 48 | 49 | /** \typedef DoubleImage 50 | * \brief A double cv::Mat. 51 | */ 52 | typedef cv::Mat_ DoubleImage; 53 | 54 | /** \typedef RawDepthImage 55 | * \brief An unsigned char cv::Mat used to for depth images with depth values expressed in millimeters. 56 | */ 57 | typedef UnsignedShortImage RawDepthImage; 58 | 59 | /** \typedef IndexImage 60 | * \brief An int cv::Mat used to save the indeces of the points of a depth image inside a vector of points. 61 | */ 62 | typedef IntImage IndexImage; 63 | 64 | /** \typedef DepthImage 65 | * \brief A float cv::Mat used to for depth images with depth values expressed in meters. 66 | */ 67 | typedef cv::Mat_< cv::Vec3b > RGBImage; 68 | 69 | typedef std::vector< cv::Vec3b > RGBVector; 70 | 71 | /** \typedef RGBImage 72 | * \brief A three channel unsigned char matrix for RGB images 73 | */ 74 | typedef FloatImage DepthImage; 75 | 76 | /** \typedef Gaussian3f 77 | * \brief A guassian in the 3D dimension. 78 | */ 79 | typedef struct Gaussian Gaussian3f; 80 | 81 | /** 82 | * check if an Eigen type contains a nan element 83 | */ 84 | template 85 | bool isNan(const T& m){ 86 | for (int i=0; i< m.rows(); i++) { 87 | for (int j=0; j< m.cols(); j++) { 88 | float v = m(i,j); 89 | if ( std::isnan( v ) ) 90 | return true; 91 | } 92 | } 93 | return false; 94 | } 95 | 96 | } 97 | -------------------------------------------------------------------------------- /nicp/nicp/depthimageconverter.cpp: -------------------------------------------------------------------------------- 1 | #include "depthimageconverter.h" 2 | #include "pinholepointprojector.h" 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | namespace nicp { 10 | 11 | DepthImageConverter::DepthImageConverter(PointProjector *projector_, 12 | StatsCalculator *statsCalculator_, 13 | PointInformationMatrixCalculator *pointInformationMatrixCalculator_, 14 | NormalInformationMatrixCalculator *normalInformationMatrixCalculator_) { 15 | _projector = projector_; 16 | _statsCalculator = statsCalculator_; 17 | _pointInformationMatrixCalculator = pointInformationMatrixCalculator_; 18 | _normalInformationMatrixCalculator = normalInformationMatrixCalculator_; 19 | } 20 | 21 | void DepthImageConverter::compute(Cloud &cloud, 22 | const DepthImage &depthImage, 23 | const Eigen::Isometry3f &sensorOffset) { 24 | assert(_projector && "DepthImageConverter: missing _projector"); 25 | assert(_statsCalculator && "DepthImageConverter: missing _statsCalculator"); 26 | assert(_pointInformationMatrixCalculator && "DepthImageConverter: missing _pointInformationMatrixCalculator"); 27 | assert(_normalInformationMatrixCalculator && "DepthImageConverter: missing _normalInformationMatrixCalculator"); 28 | assert(depthImage.rows > 0 && depthImage.cols > 0 && "DepthImageConverter: depthImage has zero size"); 29 | 30 | cloud.clear(); 31 | _projector->setImageSize(depthImage.rows, depthImage.cols); 32 | 33 | // Resizing the temporaries 34 | if(depthImage.rows != _indexImage.rows || depthImage.cols != _indexImage.cols) { 35 | _indexImage.create(depthImage.rows, depthImage.cols); 36 | } 37 | 38 | // Unprojecting 39 | _projector->setTransform(Eigen::Isometry3f::Identity()); 40 | _projector->unProject(cloud.points(), cloud.gaussians(), _indexImage, depthImage); 41 | 42 | _statsCalculator->compute(cloud.normals(), 43 | cloud.stats(), 44 | cloud.points(), 45 | _indexImage); 46 | 47 | _pointInformationMatrixCalculator->compute(cloud.pointInformationMatrix(), cloud.stats(), cloud.normals()); 48 | _normalInformationMatrixCalculator->compute(cloud.normalInformationMatrix(), cloud.stats(), cloud.normals()); 49 | 50 | cloud.transformInPlace(sensorOffset); 51 | } 52 | 53 | void DepthImageConverter::compute(Cloud &cloud, 54 | const DepthImage &depthImage, 55 | const RGBImage &rgbImage, 56 | const Eigen::Isometry3f &sensorOffset) { 57 | 58 | assert(_projector && "DepthImageConverter: missing _projector"); 59 | assert(_statsCalculator && "DepthImageConverter: missing _statsCalculator"); 60 | assert(_pointInformationMatrixCalculator && "DepthImageConverter: missing _pointInformationMatrixCalculator"); 61 | assert(_normalInformationMatrixCalculator && "DepthImageConverter: missing _normalInformationMatrixCalculator"); 62 | assert(depthImage.rows > 0 && depthImage.cols > 0 && "DepthImageConverter: depthImage has zero size"); 63 | 64 | cloud.clear(); 65 | _projector->setImageSize(depthImage.rows, depthImage.cols); 66 | 67 | // Resizing the temporaries 68 | if(depthImage.rows != _indexImage.rows || depthImage.cols != _indexImage.cols) { 69 | _indexImage.create(depthImage.rows, depthImage.cols); 70 | } 71 | 72 | // Unprojecting 73 | _projector->setTransform(Eigen::Isometry3f::Identity()); 74 | _projector->unProject(cloud.points(), cloud.gaussians(), _indexImage, depthImage); 75 | 76 | _statsCalculator->compute(cloud.normals(), 77 | cloud.stats(), 78 | cloud.points(), 79 | _indexImage); 80 | cloud.unprojectRGB(rgbImage,_indexImage); 81 | _pointInformationMatrixCalculator->compute(cloud.pointInformationMatrix(), cloud.stats(), cloud.normals()); 82 | _normalInformationMatrixCalculator->compute(cloud.normalInformationMatrix(), cloud.stats(), cloud.normals()); 83 | 84 | cloud.transformInPlace(sensorOffset); 85 | } 86 | 87 | } 88 | -------------------------------------------------------------------------------- /nicp/nicp/depthimageconverterintegralimage.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "depthimageconverter.h" 4 | 5 | namespace nicp { 6 | 7 | /** \class DepthImageConverterIntegralImage depthimageconverterintegralimage.h "depthimageconverterintegralimage.h" 8 | * \brief Class for depth image conversion to point cloud with normal computation using integral image. 9 | * 10 | * This class implements the interface given by the class DepthImageConverter and allows to compute a point 11 | * cloud and relative normals by using images structures called Integral Images. Integral image speed up 12 | * the normal computatio with respect to classical methods since using them it is possible to compute the 13 | * covariance of a point in constant time. 14 | */ 15 | class DepthImageConverterIntegralImage : virtual public DepthImageConverter { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 18 | 19 | /** 20 | * Constructor. 21 | * This constructor creates a DepthImageConverterIntegralImage with default values for all its attributes 22 | * and will set the necessary objects with the objets given in input. If nothing is passed to the 23 | * constructor, then all the pointers to objects implementing an algorithm have to be setted since 24 | * this constructor sets them to zero by default. 25 | */ 26 | DepthImageConverterIntegralImage(PointProjector *_projector = 0, 27 | StatsCalculator *_statsCalculator = 0, 28 | PointInformationMatrixCalculator *_pointInformationMatrixCalculator = 0, 29 | NormalInformationMatrixCalculator *_normalInformationMatrixCalculator = 0); 30 | 31 | /** 32 | * Destructor. 33 | */ 34 | virtual ~DepthImageConverterIntegralImage() {} 35 | 36 | /** 37 | * This method computes the point cloud included the properties of the points like the normals starting 38 | * from the depth image given in input. If given, it also applies a sensor offset to the cloud. 39 | * @param cloud is the output parameter that will contain the computed point cloud. 40 | * @param depthImage is an input parameter containing the depth image to transform in a point cloud. 41 | * @param sensorOffset is an optional input parameter which represents a sensor offset to apply to the 42 | * computed point cloud. 43 | */ 44 | virtual void compute(Cloud &cloud, 45 | const DepthImage &depthImage, 46 | const Eigen::Isometry3f &sensorOffset = Eigen::Isometry3f::Identity()); 47 | 48 | 49 | /** 50 | * This method computes the point cloud included the properties of the points starting from the depth image 51 | * given in input. If given, it also applies a sensor offset to the cloud. 52 | * @param cloud is the output parameter that will contain the computed point cloud. 53 | * @param depthImage is an input parameter containing the depth image to transform in a point cloud. 54 | * @param rgbImage is an input parameter containing the registered rgb image 55 | * @param sensorOffset is an optional input parameter which represents a sensor offset to apply to the 56 | * computed point cloud. 57 | */ 58 | virtual void compute(Cloud &cloud, 59 | const DepthImage &depthImage, 60 | const RGBImage &rgbImage, 61 | const Eigen::Isometry3f &sensorOffset = Eigen::Isometry3f::Identity()); 62 | 63 | }; 64 | 65 | } 66 | -------------------------------------------------------------------------------- /nicp/nicp/gaussian.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | template 9 | struct Gaussian { 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 11 | typedef Scalar_ Scalar; 12 | typedef typename Eigen::Matrix MatrixType; 13 | typedef typename Eigen::Matrix VectorType; 14 | static const int DimensionAtCompileTime = DimensionAtCompileTime_; 15 | 16 | Gaussian() { 17 | _covarianceMatrix.setZero(); 18 | _informationMatrix.setZero(); 19 | _informationVector.setZero(); 20 | _mean.setZero(); 21 | } 22 | 23 | Gaussian(int dimension_): 24 | _informationMatrix(dimension_, dimension_), 25 | _informationVector(dimension_), 26 | _covarianceMatrix(dimension_, dimension_), 27 | _mean(dimension_) 28 | { 29 | _covarianceMatrix.setZero(); 30 | _informationMatrix.setZero(); 31 | _informationVector.setZero(); 32 | _mean.setZero(); 33 | } 34 | 35 | 36 | Gaussian(const VectorType& v, const MatrixType& m, bool useInfoForm = false) { 37 | _momentsUpdated = _infoUpdated = false; 38 | if (useInfoForm){ 39 | _informationVector=v; 40 | _informationMatrix = m; 41 | _infoUpdated = true; 42 | } else { 43 | _mean=v; 44 | _covarianceMatrix = m; 45 | _momentsUpdated = true; 46 | } 47 | } 48 | 49 | inline Gaussian& addInformation(const Gaussian& g){ 50 | _updateInfo(); 51 | _informationMatrix += g.informationMatrix(); 52 | _informationVector += g.informationVector(); 53 | _momentsUpdated = false; 54 | return *this; 55 | } 56 | 57 | inline Gaussian& addNoise(const Gaussian& g){ 58 | _updateMoments(); 59 | _covarianceMatrix += g.covarianceMatrix(); 60 | _mean += g.mean(); 61 | _infoUpdated = false; 62 | return *this; 63 | } 64 | 65 | 66 | inline int dimension() const {return _mean.rows();} 67 | inline const Eigen::Matrix3f& covarianceMatrix() const {_updateMoments(); return _covarianceMatrix; } 68 | inline const Eigen::Vector3f& mean() const {_updateMoments(); return _mean; } 69 | inline const Eigen::Matrix3f& informationMatrix() const { _updateInfo(); return _informationMatrix; } 70 | inline const Eigen::Vector3f& informationVector() const { _updateInfo();return _informationVector; } 71 | 72 | 73 | protected: 74 | inline void _updateMoments() const { 75 | if (_momentsUpdated) 76 | return; 77 | _covarianceMatrix = _informationMatrix.inverse(); 78 | _mean = _covarianceMatrix*_informationVector; 79 | _momentsUpdated = true; 80 | } 81 | 82 | inline void _updateInfo() const { 83 | if (_infoUpdated) 84 | return; 85 | _informationMatrix = _covarianceMatrix.inverse(); 86 | _informationVector = _informationMatrix*_mean; 87 | _infoUpdated = true; 88 | } 89 | 90 | mutable MatrixType _informationMatrix; 91 | mutable VectorType _informationVector; 92 | mutable MatrixType _covarianceMatrix; 93 | mutable VectorType _mean; 94 | mutable bool _momentsUpdated; 95 | mutable bool _infoUpdated; 96 | }; 97 | -------------------------------------------------------------------------------- /nicp/nicp/gaussian3.cpp: -------------------------------------------------------------------------------- 1 | #include "gaussian3.h" 2 | #include "pinholepointprojector.h" 3 | 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | namespace nicp { 11 | 12 | Gaussian3fVector::Gaussian3fVector(size_t s, const Gaussian3f &p) { 13 | resize(s); 14 | std::fill(begin(), end(), p); 15 | } 16 | 17 | void Gaussian3fVector::fromDepthImage(const DepthImage &depthImage, 18 | const PinholePointProjector &pointProjector, 19 | float dmax, float baseline, float alpha) { 20 | assert(depthImage.rows > 0 && depthImage.cols > 0 && "Gaussian3fVector: Depth image has zero dimensions"); 21 | clear(); 22 | resize(depthImage.rows * depthImage.cols, Gaussian3f()); 23 | const float *f = (float*)depthImage.data; 24 | int k = 0; 25 | float fB = (baseline * pointProjector.cameraMatrix()(0, 0)); 26 | Matrix3f inverseCameraMatrix = pointProjector.cameraMatrix().inverse(); 27 | Matrix3f J; 28 | for(int c = 0; c < depthImage.cols; c++) { 29 | for(int r = 0; r < depthImage.rows; r++, f++) { 30 | if(*f >= dmax || *f <= 0) 31 | continue; 32 | Point mean; 33 | pointProjector.unProject(mean, r, c, *f); 34 | float z = mean.z(); 35 | float zVariation = (alpha * z * z) / (fB + z * alpha); 36 | J << 37 | z, 0, (float)r, 38 | 0, z, (float)c, 39 | 0, 0, 1; 40 | J = inverseCameraMatrix * J; 41 | Diagonal3f imageCovariance(1.0f, 1.0f, zVariation); 42 | Matrix3f cov = J * imageCovariance * J.transpose(); 43 | at(k) = Gaussian3f(mean.head<3>(), cov); 44 | k++; 45 | } 46 | } 47 | 48 | resize(k); 49 | } 50 | 51 | void Gaussian3fVector::toPointAndNormalVector(PointVector &destPoints, NormalVector &destNormals, bool eraseNormals) const { 52 | destPoints.resize(size()); 53 | destNormals.resize(size()); 54 | for(size_t k = 0; k < size(); k++) { 55 | Point &point = destPoints[k]; 56 | Normal &normal = destNormals[k]; 57 | point.head<3>() = at(k).mean(); 58 | if(eraseNormals) 59 | normal.setZero(); 60 | } 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /nicp/nicp/gaussian3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "definitions.h" 4 | #include "homogeneousvector4f.h" 5 | 6 | namespace nicp { 7 | 8 | class PinholePointProjector; 9 | 10 | /** \class Gaussian3fVector gaussian3.h "gaussian3.h" 11 | * \brief Class for Gaussian3f vector manipulation. 12 | * 13 | * This class implements a structure for memorize, create and make operations on 14 | * a vector of Gausian3f. A Gaussian is a struct used to represent a Gaussian allowing 15 | * to define the dimension at compile time. This struct memorize all the foundamental 16 | * informations about a gaussian like the mean, the covariance matrix and the information matrix. 17 | */ 18 | class Gaussian3fVector : public TransformableVector { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 21 | 22 | /** 23 | * Constructor. 24 | * This constructor creates a Gaussian3fVector of dimesion and initialized with the values 25 | * given in input. 26 | * @param s is the dimension of the vector to create. 27 | * @param p is a Gaussian3f to which all the vector elements are initialized. 28 | */ 29 | Gaussian3fVector(size_t s = 0, const Gaussian3f &p = Gaussian3f()); 30 | 31 | /** 32 | * Destructor. 33 | */ 34 | virtual ~Gaussian3fVector() {} 35 | 36 | /** 37 | * This method loads and fills the Gaussian3fVector starting from a depth image 38 | * and some sensor parameters. The Gaussian3fVector will represents the intrinsic error 39 | * of each point due to the sensor uncertainty. 40 | * @param depthImage is a reference to the DepthImage used to fill the Gaussian3fVector. 41 | * @param pointProjector is a reference to the PinholePointProjector used to unproject the points. 42 | * @param dmax is the max depth value for which points over it are discarded. 43 | * @param baseline is the horizontal baseline between the cameras used by the depth sensor (in meters). 44 | * @param alpha is a little perturbation to apply during the Gaussian3fVector computation. 45 | * @see toPointAndNormalVector() 46 | */ 47 | void fromDepthImage(const DepthImage &depthImage, 48 | const PinholePointProjector &pointProjector, 49 | float dmax = std::numeric_limits::max(), 50 | float baseline = 0.075f, float alpha = 0.1f); 51 | 52 | /** 53 | * This method updates the points coordinates using the Gaussian3fVector. 54 | * @param destPoints is a reference to the vector of Point that will be updated using the Gaussian3fVector. 55 | * @param destNormals is a reference to the vector of Normal. 56 | * @param eraseNormals is an input boolean parameter, if it is true the vector of Normal will be erased. 57 | * @see tfromDepthImage() 58 | */ 59 | void toPointAndNormalVector(PointVector &destPoints, NormalVector &destNormals, bool eraseNormals = false) const; 60 | 61 | /** 62 | * This method applies a transformation like an isometry to the Gaussian3fVector. 63 | * @param m is a reference to the transformation to apply to the Gaussian3fVector. 64 | */ 65 | template 66 | inline void transformInPlace(const OtherDerived &m) { 67 | const Eigen::Matrix4f t = m; 68 | const Eigen::Matrix3f rotation = t.block<3, 3>(0, 0); 69 | const Eigen::Vector3f translation = t.block<3, 1>(0, 3); 70 | for (size_t i = 0; i < size(); ++i) { 71 | at(i) = Gaussian3f(rotation * at(i).mean() + translation, rotation * at(i).covarianceMatrix() * rotation.transpose(), false); 72 | } 73 | } 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /nicp/nicp/homogeneousvector4f.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "transformable_vector.h" 7 | 8 | namespace nicp { 9 | 10 | /** \class HomogeneousVector4f homogeneousvector4f.h "homogeneousvector4f.h" 11 | * \brief Class for homogenous points manipulation. 12 | * 13 | * This class allows to creates and operates on homogeneous points. The homogeneous 14 | * corrdinates is specified via template. 15 | */ 16 | template 17 | class HomogeneousVector4f : public Eigen::Vector4f { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 20 | 21 | static constexpr float wCoordinate = wCoordinate_; 22 | 23 | /** 24 | * Empty constructor. 25 | */ 26 | inline HomogeneousVector4f() : Eigen::Vector4f() { this->data()[3] = wCoordinate; } 27 | 28 | /** 29 | * This constructor creates an homogeneous points with the values of the input 30 | * 3D vector. 31 | * @param other is a 3D vector used to initialize the Homogeneous4fVector. 32 | */ 33 | inline HomogeneousVector4f(const Eigen::Vector3f &other) { 34 | this->data()[0] = other.data()[0]; 35 | this->data()[1] = other.data()[1]; 36 | this->data()[2] = other.data()[2]; 37 | this->data()[3] = wCoordinate; 38 | } 39 | 40 | /** 41 | * This constructor creates an homogeneous points with the values of the input object. 42 | * @param other is an Eigen::MatrixBase object used to initialize the Homogeneous4fVector. 43 | */ 44 | template 45 | inline HomogeneousVector4f(const Eigen::MatrixBase &other) : Eigen::Vector4f(other) { this->data()[3] = wCoordinate; } 46 | 47 | /** 48 | * This methods implements the operator equal between an Homogeneous4fVector and an Eigen::MatrixBase object. 49 | */ 50 | template 51 | inline HomogeneousVector4f& operator = (const Eigen::MatrixBase &other) { 52 | this->Eigen::Vector4f::operator = (other); 53 | this->data()[3] = wCoordinate; 54 | return *this; 55 | } 56 | 57 | /** 58 | * This methods implements the operator equal between an Homogeneous4fVector and a 3D vector. 59 | */ 60 | inline HomogeneousVector4f& operator = (const Eigen::Vector3f &other) { 61 | this->data()[0] = other.data()[0]; 62 | this->data()[1] = other.data()[1]; 63 | this->data()[2] = other.data()[2]; 64 | this->data()[3] = wCoordinate; 65 | return *this; 66 | } 67 | 68 | }; 69 | 70 | /** \typedef Point 71 | * \brief An Homogeneous4fVector where the homogeneous coordinates is equal to 1. 72 | */ 73 | typedef HomogeneousVector4f<1> Point; 74 | 75 | /** \typedef Normal 76 | * \brief An Homogeneous4fVector where the homogeneous coordinates is equal to 0. 77 | */ 78 | typedef HomogeneousVector4f<0> Normal; 79 | 80 | /** \typedef PointVector 81 | * \brief A vector of Point. 82 | */ 83 | typedef TransformableVector PointVector; 84 | 85 | /** \typedef NormalVector 86 | * \brief A vector of Normal. 87 | */ 88 | typedef TransformableVector NormalVector; 89 | 90 | } 91 | -------------------------------------------------------------------------------- /nicp/nicp/imageutils.cpp: -------------------------------------------------------------------------------- 1 | #include "imageutils.h" 2 | #include 3 | 4 | namespace nicp { 5 | 6 | void DepthImage_scale(DepthImage &dest, const DepthImage &src, int step, float maxDepthCov) { 7 | int rows = src.rows / step; 8 | int cols = src.cols / step; 9 | dest.create(rows, cols); 10 | dest.setTo(cv::Scalar(0)); 11 | for(int r = 0; r < dest.rows; r++) { 12 | for(int c = 0; c < dest.cols; c++) { 13 | float acc = 0; 14 | float acc2 = 0; 15 | int np = 0; 16 | int sr = r * step; 17 | int sc = c * step; 18 | for(int i = 0; i < step; i++) { 19 | for(int j = 0; j < step; j++) { 20 | if(sr + i < src.rows && sc + j < src.cols) { 21 | const float& f = src(sr + i, sc + j); 22 | acc += f; 23 | acc2 += f*f; 24 | np += src(sr + i, sc + j) > 0; 25 | } 26 | } 27 | } 28 | if(np){ 29 | float mu = acc/np; 30 | float sigma = acc2/np-mu*mu; 31 | if (sigma>maxDepthCov) 32 | continue; 33 | dest(r, c) = mu; 34 | } 35 | } 36 | } 37 | } 38 | 39 | void RGBImage_scale(RGBImage &dest, const RGBImage &src, int step) { 40 | int rows = src.rows / step; 41 | int cols = src.cols / step; 42 | dest.create(rows, cols); 43 | dest.setTo(cv::Scalar(0)); 44 | for(int r = 0; r < dest.rows; r++) { 45 | for(int c = 0; c < dest.cols; c++) { 46 | cv::Vec3i acc(0,0,0); 47 | int np = 0; 48 | int sr = r * step; 49 | int sc = c * step; 50 | for(int i = 0; i < step; i++) { 51 | for(int j = 0; j < step; j++) { 52 | if(sr + i < src.rows && sc + j < src.cols) { 53 | const cv::Vec3b& p = src(sr + i, sc + j); 54 | cv::Vec3i ip(p[0], p[1], p[2]); 55 | acc += ip; 56 | np++; 57 | } 58 | } 59 | } 60 | if(np){ 61 | float inp=1./np; 62 | cv::Vec3b mu(acc[0]*inp, acc[1]*inp, acc[2]*inp); 63 | dest(r, c) = mu; 64 | } 65 | } 66 | } 67 | } 68 | 69 | void DepthImage_convert_32FC1_to_16UC1(cv::Mat &dest, const cv::Mat &src, float scale) { 70 | assert(src.type() != CV_32FC1 && "DepthImage_convert_32FC1_to_16UC1: source image of different type from 32FC1"); 71 | const float *sptr = (const float*)src.data; 72 | int size = src.rows * src.cols; 73 | const float *send = sptr + size; 74 | dest.create(src.rows, src.cols, CV_16UC1); 75 | dest.setTo(cv::Scalar(0)); 76 | unsigned short *dptr = (unsigned short*)dest.data; 77 | while(sptr::max()) 79 | *dptr = scale * (*sptr); 80 | dptr ++; 81 | sptr ++; 82 | } 83 | } 84 | 85 | void DepthImage_convert_16UC1_to_32FC1(cv::Mat &dest, const cv::Mat &src, float scale) { 86 | assert(src.type() != CV_16UC1 && "DepthImage_convert_16UC1_to_32FC1: source image of different type from 16UC1"); 87 | const unsigned short *sptr = (const unsigned short*)src.data; 88 | int size = src.rows * src.cols; 89 | const unsigned short *send = sptr + size; 90 | dest.create(src.rows, src.cols, CV_32FC1); 91 | dest.setTo(cv::Scalar(0.0f)); 92 | float *dptr = (float*)dest.data; 93 | while(sptr < send) { 94 | if(*sptr) 95 | *dptr = scale * (*sptr); 96 | dptr ++; 97 | sptr ++; 98 | } 99 | } 100 | 101 | } 102 | -------------------------------------------------------------------------------- /nicp/nicp/imageutils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "definitions.h" 4 | 5 | namespace nicp { 6 | 7 | /** 8 | * This method scales a depth image to the size specified. 9 | * @param dest is where the resized depth image will be saved. 10 | * @param src is the source depth image to resize. 11 | * @param step is the resize factor. If step is greater than 1 the image will be smaller 12 | * (for example in case it's 2 the size of the image will be half the size of the original one). 13 | */ 14 | void DepthImage_scale(DepthImage &dest, const DepthImage &src, int step, float maxDepthCov = 0.01f); 15 | 16 | /** 17 | * This method scales a depth image to the size specified. 18 | * @param dest is where the resized depth image will be saved. 19 | * @param src is the source depth image to resize. 20 | * @param step is the resize factor. If step is greater than 1 the image will be smaller 21 | * (for example in case it's 2 the size of the image will be half the size of the original one). 22 | */ 23 | void RGBImage_scale(RGBImage &dest, const RGBImage &src, int step); 24 | 25 | 26 | /** 27 | * This method converts a float cv::Mat to an unsigned char cv::Mat. 28 | * @param dest is where the converted image will be saved. 29 | * @param src is the source image to convert. 30 | * @param scale is a parameter that for example in the case of a depth image lets to convert 31 | * the depth values from a unit measure to another. Here it is assumed by default that the elements have 32 | * to be converted from millimeters to meters and so the scale is 1000. 33 | */ 34 | void DepthImage_convert_32FC1_to_16UC1(cv::Mat &dest, const cv::Mat &src, float scale = 1000.0f); 35 | 36 | /** 37 | * This method converts an unsigned char cv::Mat to a float cv::Mat. 38 | * @param dest is where the converted image will be saved. 39 | * @param src is the source image to convert. 40 | * @param scale is a parameter that for example in the case of a depth image lets to convert 41 | * the depth values from a unit measure to another. Here it is assumed by default that the elements have 42 | * to be converted from meters to millimeters and so the scale is 0.001. 43 | */ 44 | void DepthImage_convert_16UC1_to_32FC1(cv::Mat &dest, const cv::Mat &src, float scale = 0.001f); 45 | 46 | } 47 | -------------------------------------------------------------------------------- /nicp/nicp/informationmatrixcalculator.cpp: -------------------------------------------------------------------------------- 1 | #include "informationmatrixcalculator.h" 2 | 3 | #include 4 | 5 | using namespace Eigen; 6 | 7 | namespace nicp { 8 | 9 | void PointInformationMatrixCalculator::compute(InformationMatrixVector &informationMatrix, 10 | const StatsVector &statsVector, 11 | const NormalVector &imageNormals) { 12 | assert(statsVector.size() > 0 && "PointInformationMatrixCalculator: statsVector has zero size"); 13 | assert(imageNormals.size() > 0 && "PointInformationMatrixCalculator: imageNormals has zero size"); 14 | 15 | informationMatrix.resize(statsVector.size()); 16 | 17 | #pragma omp parallel for 18 | for(size_t i = 0; i < statsVector.size(); i++) { 19 | const Stats &stats = statsVector[i]; 20 | InformationMatrix U = Matrix4f::Zero(); 21 | U.block<3, 3>(0, 0) = stats.eigenVectors(); 22 | if(imageNormals[i].squaredNorm() > 0) { 23 | if(stats.curvature() < _curvatureThreshold) 24 | informationMatrix[i] = U * _flatInformationMatrix * U.transpose(); 25 | else { 26 | informationMatrix[i] = U * _nonFlatInformationMatrix * U.transpose(); 27 | } 28 | } 29 | else 30 | informationMatrix[i] = InformationMatrix(); 31 | } 32 | } 33 | 34 | void NormalInformationMatrixCalculator::compute(InformationMatrixVector &informationMatrix, 35 | const StatsVector &statsVector, 36 | const NormalVector &imageNormals) { 37 | assert(statsVector.size() > 0 && "PointInformationMatrixCalculator: statsVector has zero size"); 38 | assert(imageNormals.size() > 0 && "PointInformationMatrixCalculator: imageNormals has zero size"); 39 | 40 | informationMatrix.resize(statsVector.size()); 41 | 42 | #pragma omp parallel for 43 | for(size_t i = 0; i < statsVector.size(); i++) { 44 | const Stats &stats = statsVector[i]; 45 | InformationMatrix U = Matrix4f::Zero(); 46 | U.block<3, 3>(0, 0) = stats.eigenVectors(); 47 | if(imageNormals[i].squaredNorm()>0) { 48 | if(stats.curvature() < _curvatureThreshold) 49 | informationMatrix[i] = U * _flatInformationMatrix * U.transpose(); 50 | else { 51 | informationMatrix[i] = U * _nonFlatInformationMatrix * U.transpose(); 52 | } 53 | } 54 | else 55 | informationMatrix[i] = InformationMatrix(); 56 | } 57 | } 58 | 59 | } 60 | -------------------------------------------------------------------------------- /nicp/nicp/linearizer.cpp: -------------------------------------------------------------------------------- 1 | #include "aligner.h" 2 | 3 | #include 4 | 5 | using namespace std; 6 | 7 | namespace nicp { 8 | 9 | Linearizer::Linearizer() { 10 | _aligner = 0; 11 | _H.setZero(); 12 | _b.setZero(); 13 | _inlierMaxChi2 = 9e3; 14 | _robustKernel = true; 15 | _demotedToGeneralizedICP = false; 16 | _zScaling = false; 17 | _scale = 1.0f; 18 | _T.matrix() = Eigen::Matrix4f::Identity(); 19 | } 20 | 21 | void Linearizer::update() { 22 | assert(_aligner && "Aligner: missing _aligner"); 23 | 24 | // Variables initialization. 25 | _b = Vector6f::Zero(); 26 | _H = Matrix6f::Zero(); 27 | const InformationMatrixVector &pointOmegas = _aligner->currentCloud()->pointInformationMatrix(); 28 | const InformationMatrixVector &normalOmegas = _aligner->currentCloud()->normalInformationMatrix(); 29 | 30 | // Allocate the variables for the sum reduction; 31 | int numThreads = omp_get_max_threads(); 32 | Matrix4f _Htt[numThreads], _Htr[numThreads], _Hrr[numThreads]; 33 | Vector4f _bt[numThreads], _br[numThreads]; 34 | int _inliers[numThreads]; 35 | float _errors[numThreads]; 36 | int iterationsPerThread = _aligner->correspondenceFinder()->numCorrespondences() / numThreads; 37 | #pragma omp parallel 38 | { 39 | int threadId = omp_get_thread_num(); 40 | int imin = iterationsPerThread * threadId; 41 | int imax = imin + iterationsPerThread; 42 | if(imax > _aligner->correspondenceFinder()->numCorrespondences()) 43 | imax = _aligner->correspondenceFinder()->numCorrespondences(); 44 | 45 | Eigen::Matrix4f Htt; 46 | Eigen::Matrix4f Htr; 47 | Eigen::Matrix4f Hrr; 48 | Eigen::Vector4f bt; 49 | Eigen::Vector4f br; 50 | int inliers; 51 | float error; 52 | Htt = Matrix4f::Zero(); 53 | Htr = Matrix4f::Zero(); 54 | Hrr = Matrix4f::Zero(); 55 | bt = Vector4f::Zero(); 56 | br = Vector4f::Zero(); 57 | error = 0; 58 | inliers = 0; 59 | for(int i = imin; i < imax; i++) { 60 | const Correspondence &correspondence = _aligner->correspondenceFinder()->correspondences()[i]; 61 | const Point referencePoint = _T * _aligner->referenceCloud()->points()[correspondence.referenceIndex]; 62 | const Normal referenceNormal = _T * _aligner->referenceCloud()->normals()[correspondence.referenceIndex]; 63 | const Point ¤tPoint = _aligner->currentCloud()->points()[correspondence.currentIndex]; 64 | const Normal ¤tNormal = _aligner->currentCloud()->normals()[correspondence.currentIndex]; 65 | InformationMatrix omegaP = pointOmegas[correspondence.currentIndex]; 66 | InformationMatrix omegaN = _scale * normalOmegas[correspondence.currentIndex]; 67 | if(_zScaling) { 68 | omegaP *= 1.0f / fabs(currentPoint.z()); 69 | omegaN *= 1.0f / fabs(currentPoint.z()); 70 | } 71 | 72 | if(_demotedToGeneralizedICP) { omegaN.setZero(); } 73 | 74 | const Vector4f pointError = referencePoint - currentPoint; 75 | const Vector4f normalError = referenceNormal - currentNormal; 76 | const Vector4f ep = omegaP * pointError; 77 | const Vector4f en = omegaN * normalError; 78 | 79 | float localError = pointError.dot(ep) + normalError.dot(en); 80 | 81 | float kscale = 1; 82 | if(localError > _inlierMaxChi2) { 83 | if (_robustKernel) { 84 | kscale = _inlierMaxChi2 / localError; 85 | } 86 | else { 87 | continue; 88 | } 89 | } 90 | inliers++; 91 | error += kscale * localError; 92 | 93 | Matrix4f Sp = skew(referencePoint); 94 | Matrix4f Sn = skew(referenceNormal); 95 | Htt.noalias() += omegaP * kscale; 96 | Htr.noalias() += omegaP * Sp * kscale; 97 | Hrr.noalias() += (Sp.transpose() * omegaP * Sp + Sn.transpose() * omegaN * Sn) * kscale; 98 | bt.noalias() += kscale * ep; 99 | br.noalias() += kscale * (Sp.transpose() * ep + Sn.transpose() * en); 100 | } 101 | 102 | _Htt[threadId] = Htt; 103 | _Htr[threadId] = Htr; 104 | _Hrr[threadId] = Hrr; 105 | _bt[threadId] = bt; 106 | _br[threadId] = br; 107 | _errors[threadId] = error; 108 | _inliers[threadId] = inliers; 109 | } 110 | 111 | // Now do the reduce 112 | Eigen::Matrix4f Htt = Eigen::Matrix4f::Zero(); 113 | Eigen::Matrix4f Htr = Eigen::Matrix4f::Zero(); 114 | Eigen::Matrix4f Hrr = Eigen::Matrix4f::Zero(); 115 | Eigen::Vector4f bt = Eigen::Vector4f::Zero(); 116 | Eigen::Vector4f br = Eigen::Vector4f::Zero(); 117 | this->_inliers = 0; 118 | this->_error = 0; 119 | for(int t = 0; t < numThreads; t++) { 120 | Htt += _Htt[t]; 121 | Htr += _Htr[t]; 122 | Hrr += _Hrr[t]; 123 | bt += _bt[t]; 124 | br += _br[t]; 125 | this->_inliers += _inliers[t]; 126 | this->_error += _errors[t]; 127 | } 128 | _H.block<3, 3>(0, 0) = Htt.block<3, 3>(0, 0); 129 | _H.block<3, 3>(0, 3) = Htr.block<3, 3>(0, 0); 130 | _H.block<3, 3>(3, 3) = Hrr.block<3, 3>(0, 0); 131 | _H.block<3, 3>(3, 0) = _H.block<3, 3>(0, 3).transpose(); 132 | _b.block<3, 1>(0, 0) = bt.block<3, 1>(0, 0); 133 | _b.block<3, 1>(3, 0) = br.block<3, 1>(0, 0); 134 | } 135 | 136 | } 137 | -------------------------------------------------------------------------------- /nicp/nicp/merger.cpp: -------------------------------------------------------------------------------- 1 | #include "merger.h" 2 | 3 | namespace nicp { 4 | 5 | Merger::Merger() { 6 | _distanceThreshold = 0.1f; 7 | _normalThreshold = cosf(10 * M_PI / 180.0f); 8 | _maxPointDepth = 10.0f; 9 | _depthImageConverter = 0; 10 | _indexImage.create(0, 0); 11 | _depthImage.create(0, 0); 12 | _collapsedIndices.resize(0); 13 | } 14 | 15 | void Merger::merge(Cloud *cloud, Eigen::Isometry3f transform) { 16 | assert(_indexImage.rows > 0 && _indexImage.cols > 0 && "Merger: _indexImage has zero size"); 17 | assert(_depthImageConverter && "Merger: missing _depthImageConverter"); 18 | assert(_depthImageConverter->projector() && "Merger: missing projector in _depthImageConverter"); 19 | 20 | PointProjector *pointProjector = _depthImageConverter->projector(); 21 | Eigen::Isometry3f oldTransform = pointProjector->transform(); 22 | 23 | pointProjector->setTransform(transform); 24 | pointProjector->project(_indexImage, 25 | _depthImage, 26 | cloud->points()); 27 | 28 | int target = 0; 29 | int distance = 0; 30 | _collapsedIndices.resize(cloud->points().size()); 31 | std::fill(_collapsedIndices.begin(), _collapsedIndices.end(), -1); 32 | 33 | int killed = 0; 34 | int currentIndex = 0; 35 | for(size_t i = 0; i < cloud->points().size(); currentIndex++ ,i++) { 36 | const Point currentPoint = cloud->points()[i]; 37 | // const Normal currentNormal = cloud->normals()[i]; 38 | 39 | int r = -1, c = -1; 40 | float depth = 0.0f; 41 | pointProjector->project(c, r, depth, currentPoint); 42 | if(depth < 0 || depth > _maxPointDepth || 43 | r < 0 || r >= _depthImage.rows || 44 | c < 0 || c >= _depthImage.cols) { 45 | distance++; 46 | continue; 47 | } 48 | 49 | float &targetZ = _depthImage(r, c); 50 | int targetIndex = _indexImage(r, c); 51 | if(targetIndex < 0) { 52 | target++; 53 | continue; 54 | } 55 | // const Normal &targetNormal = cloud->normals().at(targetIndex); 56 | 57 | Eigen::Vector4f viewPointDirection = transform.matrix().col(3)-currentPoint; 58 | viewPointDirection(3)=0; 59 | if(targetIndex == currentIndex) { 60 | _collapsedIndices[currentIndex] = currentIndex; 61 | } 62 | else if(fabs(depth - targetZ) < _distanceThreshold /*&& 63 | currentNormal.dot(targetNormal) > _normalThreshold && 64 | (viewPointDirection.dot(targetNormal)>cos(0))*/ ) { 65 | Gaussian3f &targetGaussian = cloud->gaussians()[targetIndex]; 66 | Gaussian3f ¤tGaussian = cloud->gaussians()[currentIndex]; 67 | targetGaussian.addInformation(currentGaussian); 68 | _collapsedIndices[currentIndex] = targetIndex; 69 | killed++; 70 | } 71 | } 72 | 73 | int murdered = 0; 74 | int k = 0; 75 | for(size_t i = 0; i < _collapsedIndices.size(); i++) { 76 | int collapsedIndex = _collapsedIndices[i]; 77 | if(collapsedIndex == (int)i) { 78 | cloud->points()[i].head<3>() = cloud->gaussians()[i].mean(); 79 | } 80 | if(collapsedIndex < 0 || collapsedIndex == (int)i) { 81 | cloud->points()[k] = cloud->points()[i]; 82 | cloud->normals()[k] = cloud->normals()[i]; 83 | cloud->stats()[k] = cloud->stats()[i]; 84 | cloud->pointInformationMatrix()[k] = cloud->pointInformationMatrix()[i]; 85 | cloud->normalInformationMatrix()[k] = cloud->normalInformationMatrix()[i]; 86 | cloud->gaussians()[k] = cloud->gaussians()[i]; 87 | if(cloud->rgbs().size()) 88 | cloud->rgbs()[k]=cloud->rgbs()[i]; 89 | k++; 90 | } 91 | else { 92 | murdered ++; 93 | } 94 | } 95 | int originalSize = cloud->points().size(); 96 | 97 | // Kill the leftover points 98 | cloud->points().resize(k); 99 | cloud->normals().resize(k); 100 | cloud->stats().resize(k); 101 | cloud->pointInformationMatrix().resize(k); 102 | cloud->normalInformationMatrix().resize(k); 103 | if(cloud->rgbs().size()) 104 | cloud->rgbs().resize(k); 105 | std::cout << "[INFO]: number of suppressed points " << murdered << std::endl; 106 | std::cout << "[INFO]: resized cloud from " << originalSize << " to " << k << " points" <setTransform(oldTransform); 109 | } 110 | 111 | } 112 | -------------------------------------------------------------------------------- /nicp/nicp/pointaccumulator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "definitions.h" 4 | #include "homogeneousvector4f.h" 5 | 6 | namespace nicp { 7 | 8 | /** \class PointAccumulator pointaccumulator.h "pointaccumulator.h" 9 | * \brief Class to be used as point accumulator. 10 | * 11 | * This class provides the operators and the structures to accumulate points, 12 | * in particular accumulates the sum and the squared sum of the points. 13 | */ 14 | class PointAccumulator { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 17 | 18 | /** 19 | * Empty constructor. 20 | */ 21 | inline PointAccumulator() { clear(); } 22 | 23 | /** 24 | * Destructor. 25 | */ 26 | virtual ~PointAccumulator() {} 27 | 28 | /** 29 | * This method reset to zero the sum of the points and put a zero 4x4 matrix in 30 | * the squared sum of the points. 31 | */ 32 | inline void clear() { 33 | _sum.setZero(); 34 | _squaredSum.setZero(); 35 | } 36 | 37 | /** 38 | * This methods implements the plus operator between two PointAccumulator. 39 | */ 40 | inline void operator +=(const PointAccumulator &pa) { 41 | _sum += pa._sum; 42 | _squaredSum += pa._squaredSum; 43 | } 44 | 45 | /** 46 | * This methods implements the minus operator between two PointAccumulator. 47 | */ 48 | inline void operator -=(const PointAccumulator &pa) { 49 | _sum -= pa._sum; 50 | _squaredSum -= pa._squaredSum; 51 | } 52 | 53 | /** 54 | * This methods implements the plus operator between a PointAccumulator and a Point. 55 | */ 56 | inline void operator +=(const Point &v) { 57 | _sum += (Eigen::Vector4f&)v; 58 | _squaredSum += v * v.transpose(); 59 | } 60 | 61 | /** 62 | * This method returns the mean of the accumulated points. 63 | * @return the mean of the accumulated points. 64 | * @see covariance() 65 | */ 66 | inline Point mean() const { 67 | const float &d = _sum.coeff(3, 0); 68 | if(d) 69 | return Point(_sum * (1.0f / d)); 70 | return Point::Zero(); 71 | } 72 | 73 | /** 74 | * This method returns the covariance matrix of the accumulated points. 75 | * @return the covariance matrix of the accumulated points. 76 | * @see mean() 77 | */ 78 | inline Eigen::Matrix4f covariance() const { 79 | float d = _sum.coeff(3, 0); 80 | if(d) { 81 | d = 1.0f / d; 82 | Eigen::Vector4f mean_ = _sum * d; 83 | return _squaredSum * d - mean_ * mean_.transpose(); 84 | } 85 | return Eigen::Matrix4f::Zero(); 86 | } 87 | 88 | /** 89 | * This method returns the sum of the accumulated points. 90 | * @return the sum of the accumulated points. 91 | * @see squaredSum() 92 | * @see n() 93 | */ 94 | inline const Eigen::Vector4f& sum() const { return _sum; } 95 | 96 | /** 97 | * This method returns the squared sum of the accumulated points. 98 | * @return the squared sum of the accumulated points. 99 | * @see sum() 100 | * @see n() 101 | */ 102 | inline const Eigen::Matrix4f& squaredSum() const { return _squaredSum; } 103 | 104 | /** 105 | * This method returns the number of accumulated points. 106 | * @return the number of accumulated points. 107 | * @see sum() 108 | * @see squaredSum() 109 | */ 110 | inline int n() const { return _sum.coeff(3, 0); } 111 | 112 | protected: 113 | Eigen::Vector4f _sum; /**< Sum of the points accumulated. */ 114 | Eigen::Matrix4f _squaredSum; /**< Squared sum of the points accumulated. */ 115 | }; 116 | 117 | } 118 | 119 | -------------------------------------------------------------------------------- /nicp/nicp/pointintegralimage.cpp: -------------------------------------------------------------------------------- 1 | #include "pointintegralimage.h" 2 | 3 | #include 4 | 5 | namespace nicp { 6 | 7 | void PointIntegralImage::compute(const IntImage &indices, const PointVector &points) { 8 | assert(points.size() > 0 && "PointIntegralImage: points has zero size"); 9 | assert(indices.rows > 0 && indices.cols > 0 && "PointIntegralImage: indices has zero size"); 10 | 11 | if(cols() != indices.cols || rows() != indices.rows) 12 | resize(indices.cols, indices.rows); 13 | clear(); 14 | 15 | // Fill the accumulators with the points 16 | #pragma omp parallel for 17 | for(int c = 0; c < cols(); c++) { 18 | const int* rowptr = &indices(c,0); 19 | for(int r = 0; r < rows(); r++) { 20 | const int &index = rowptr[r]; 21 | PointAccumulator &acc = coeffRef(r, c); 22 | if(index < 0) 23 | continue; 24 | const Point &point = points[index]; 25 | acc.operator += (point); 26 | } 27 | } 28 | 29 | // Fill by column 30 | #pragma omp parallel for 31 | for(int c = 0; c < cols(); c++) { 32 | for(int r = 1; r < rows(); r++) { 33 | coeffRef(r, c) += coeffRef(r - 1, c); 34 | } 35 | } 36 | 37 | // Fill by row 38 | #pragma omp parallel for 39 | for(int r = 0; r < rows(); r++) { 40 | for(int c = 1; c < cols(); c++) { 41 | coeffRef(r, c) += coeffRef(r, c - 1); 42 | } 43 | } 44 | } 45 | 46 | void PointIntegralImage::clear() { 47 | int s = rows() * cols(); 48 | PointAccumulator *p = data(); 49 | for(int i = 0; i < s; i++, p++) 50 | p->clear(); 51 | } 52 | 53 | PointAccumulator PointIntegralImage::getRegion(int xmin, int xmax, int ymin, int ymax) { 54 | assert(rows() > 0 && cols() > 0 && "PointIntegralImage: this has zero size"); 55 | 56 | PointAccumulator pa; 57 | xmin = _clamp(xmin - 1, 0, rows() - 1); 58 | xmax = _clamp(xmax - 1, 0, rows() - 1); 59 | ymin = _clamp(ymin - 1, 0, cols() - 1); 60 | ymax = _clamp(ymax - 1, 0, cols() - 1); 61 | pa = coeffRef(xmax, ymax); // Total 62 | pa += coeffRef(xmin, ymin); // Upper right 63 | pa -= coeffRef(xmin, ymax); // Upper rectangle 64 | pa -= coeffRef(xmax, ymin); // Rightmost rectangle 65 | return pa; 66 | } 67 | 68 | inline int PointIntegralImage::_clamp(int v, int min, int max) { 69 | v = (v < min) ? min : v; 70 | v = (v > max) ? max : v; 71 | return v; 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /nicp/nicp/pointintegralimage.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pointaccumulator.h" 4 | 5 | namespace nicp { 6 | 7 | /** \class PointIntegralImage pointintegralimage.h "pointintegralimage.h" 8 | * \brief Class for integral image computation. 9 | * 10 | * This class extends the Eigen::Matrix class and implements all the methods necessary 11 | * to compute an integral image of the points in a depth image. The elements of a PointIntegralImage 12 | * are PointAccumulator. 13 | */ 14 | class PointIntegralImage : public Eigen::Matrix { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 17 | 18 | /** 19 | * Empty constructor. 20 | * This constructor creates a PointIntegralImage of zero size. 21 | */ 22 | PointIntegralImage() : Eigen::Matrix(0, 0) {} 23 | 24 | /** 25 | * Destructor. 26 | */ 27 | virtual ~PointIntegralImage() {} 28 | 29 | /** 30 | * This method computes the integral image, filling the PointIntegralImage matrix, starting from the vector of points 31 | * and the associated index image. 32 | * @param pointIndices is the IndexImage associated to the vector of points. 33 | * @param points is the vector of points. 34 | */ 35 | void compute(const IntImage &pointIndices, const PointVector &points); 36 | 37 | /** 38 | * This method clear the PointIntegralImage matrix. 39 | */ 40 | void clear(); 41 | 42 | /** 43 | * This method returns the PointAccumulator associated to a certain region of the PointIntegralImage matrix. 44 | * @param xmin is the smaller row index of the region to compute. 45 | * @param xmax is the larger row index of the region to compute. 46 | * @param ymin is the smaller lower column index of the region to compute. 47 | * @param ymax is the larger lower column index of the region to compute. 48 | * @return the PointAccumulator of the input region. 49 | */ 50 | PointAccumulator getRegion(int xmin, int xmax, int ymin, int ymax); 51 | 52 | protected: 53 | /** 54 | * This method check if the input parameter v is inside the interval [min;max]. 55 | * @param v is the input parameter to check. 56 | * @param min is lower bound. 57 | * @param max is upper bound. 58 | * @return v if it is inside the interval [min;max], otherwise it returns the 59 | * lower/upper bound depending which one was exceded. 60 | */ 61 | inline int _clamp(int v, int min, int max); 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /nicp/nicp/pointprojector.cpp: -------------------------------------------------------------------------------- 1 | #include "pointprojector.h" 2 | #include "bm_se3.h" 3 | 4 | namespace nicp { 5 | 6 | PointProjector::PointProjector() { 7 | _transform.setIdentity(); 8 | _transform.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f; 9 | _minDistance = 0.01f; 10 | _maxDistance = 6.0f; 11 | _originalImageRows = 0; 12 | _originalImageCols = 0; 13 | _imageRows = 0; 14 | _imageCols = 0; 15 | _scalingFactor = 1.0f; 16 | } 17 | 18 | void PointProjector::project(IntImage &indexImage, 19 | DepthImage &depthImage, 20 | const PointVector &points) const { 21 | assert(indexImage.rows > 0 && indexImage.cols > 0 && "PointProjector: Index image has zero dimensions"); 22 | 23 | depthImage.create(indexImage.rows, indexImage.cols); 24 | depthImage.setTo(0.0f); 25 | indexImage.setTo(cv::Scalar(-1)); 26 | const Point *point = &points[0]; 27 | for(size_t i = 0; i < points.size(); i++, point++) { 28 | int x, y; 29 | float d; 30 | if(!project(x, y, d, *point) || 31 | x < 0 || x >= indexImage.rows || 32 | y < 0 || y >= indexImage.cols) { continue; } 33 | float &otherDistance = depthImage(x, y); 34 | int &otherIndex = indexImage(x, y); 35 | if(!otherDistance || otherDistance > d) { 36 | otherDistance = d; 37 | otherIndex = i; 38 | } 39 | } 40 | } 41 | 42 | void PointProjector::unProject(PointVector &points, 43 | IntImage &indexImage, 44 | const DepthImage &depthImage) const { 45 | assert(depthImage.rows > 0 && depthImage.cols > 0 && "PointProjector: Depth image has zero dimensions"); 46 | points.resize(depthImage.rows * depthImage.cols); 47 | int count = 0; 48 | indexImage.create(depthImage.rows, depthImage.cols); 49 | Point *point = &points[0]; 50 | for(int r = 0; r < depthImage.rows; r++){ 51 | const float* f = &depthImage(r, 0); 52 | int *i = &indexImage(r, 0); 53 | for(int c = 0; c < depthImage.cols; c++, f++, i++) { 54 | if(!unProject(*point, r, c, *f)) { 55 | *i = -1; 56 | continue; 57 | } 58 | point++; 59 | *i = count; 60 | count++; 61 | } 62 | } 63 | 64 | points.resize(count); 65 | } 66 | 67 | void PointProjector::unProject(PointVector &points, 68 | Gaussian3fVector &gaussians, 69 | IntImage &indexImage, 70 | const DepthImage &depthImage) const { 71 | assert(depthImage.rows > 0 && depthImage.cols > 0 && "PointProjector: Depth image has zero dimensions"); 72 | points.resize(depthImage.rows * depthImage.cols); 73 | int count = 0; 74 | indexImage.create(depthImage.rows, depthImage.cols); 75 | Point *point = &points[0]; 76 | for(int r = 0; r < depthImage.rows; r++) { 77 | const float *f = &depthImage(r, 0); 78 | int *i = &indexImage(r, 0); 79 | for(int c = 0; c < depthImage.cols; c++, f++, i++) { 80 | if(!unProject(*point, r, c, *f)) { 81 | *i = -1; 82 | continue; 83 | } 84 | point++; 85 | *i = count; 86 | count++; 87 | } 88 | } 89 | 90 | gaussians.resize(count); 91 | points.resize(count); 92 | } 93 | 94 | void PointProjector::projectIntervals(IntervalImage &intervalImage, 95 | const DepthImage &depthImage, 96 | const float worldRadius) const { 97 | assert(depthImage.rows > 0 && depthImage.cols > 0 && "PointProjector: Depth image has zero dimensions"); 98 | intervalImage.create(depthImage.rows, depthImage.cols); 99 | for(int r = 0; r < depthImage.rows; r++) { 100 | const float *f = &depthImage(r, 0); 101 | cv::Vec2i *i = &intervalImage(r, 0); 102 | for(int c = 0; c < depthImage.cols; c++, f++, i++) { *i = projectInterval(r, c, *f, worldRadius); } 103 | } 104 | } 105 | 106 | void PointProjector::scale(float scalingFactor) { 107 | _scalingFactor = scalingFactor; 108 | _imageRows = _originalImageRows; 109 | _imageCols = _originalImageCols; 110 | _imageRows *= scalingFactor; 111 | _imageCols *= scalingFactor; 112 | } 113 | 114 | } 115 | -------------------------------------------------------------------------------- /nicp/nicp/se3_prior.cpp: -------------------------------------------------------------------------------- 1 | #include "se3_prior.h" 2 | 3 | namespace nicp { 4 | 5 | SE3Prior::SE3Prior(const Eigen::Isometry3f &priorMean_, const Matrix6f &priorInformation_) : 6 | _priorMean(priorMean_), _priorInformation(priorInformation_) {} 7 | 8 | Matrix6f SE3Prior::jacobian(const Eigen::Isometry3f &invT) const { 9 | Matrix6f J; 10 | float epsilon = 1e-3; 11 | float iEpsilon = 0.5f / epsilon; 12 | Vector6f incrementsUp = Vector6f::Zero(); 13 | Vector6f incrementsDown = Vector6f::Zero(); 14 | 15 | for(int i = 0; i < J.cols(); i++) { 16 | incrementsUp(i) = epsilon; 17 | incrementsDown(i) = -epsilon; 18 | J.col(i) = iEpsilon * (error(v2t(incrementsUp) * invT) - error(v2t(incrementsDown) * invT)); 19 | incrementsUp(i) = 0; 20 | incrementsDown(i) = 0; 21 | } 22 | return J; 23 | } 24 | 25 | Matrix6f SE3Prior::jacobianZ(const Eigen::Isometry3f &invT) const { 26 | Matrix6f J; 27 | float epsilon = 1e-3; 28 | float iEpsilon = 0.5f / epsilon; 29 | Vector6f incrementsUp = Vector6f::Zero(); 30 | Vector6f incrementsDown = Vector6f::Zero(); 31 | Eigen::Isometry3f savedPrior = _priorMean; 32 | for(int i = 0; i < J.cols(); i++) { 33 | incrementsUp(i) = epsilon; 34 | _priorMean = savedPrior * v2t(incrementsUp); 35 | Vector6f eUp = error(invT); 36 | 37 | incrementsDown(i) = -epsilon; 38 | _priorMean = savedPrior * v2t(incrementsDown); 39 | Vector6f eDown = error(invT); 40 | J.col(i) = iEpsilon * (eUp - eDown); 41 | incrementsUp(i) = 0; 42 | incrementsDown(i) = 0; 43 | } 44 | _priorMean = savedPrior; 45 | return J; 46 | } 47 | 48 | Matrix6f SE3Prior::errorInformation(const Eigen::Isometry3f &invT) const { 49 | Matrix6f Jz = jacobianZ(invT); 50 | Matrix6f iJz = Jz.inverse(); 51 | return iJz.transpose() * _priorInformation * iJz; 52 | } 53 | 54 | SE3RelativePrior::SE3RelativePrior(const Eigen::Isometry3f &priorMean_, 55 | const Matrix6f &information_): 56 | SE3Prior(priorMean_, information_) {} 57 | 58 | Vector6f SE3RelativePrior::error(const Eigen::Isometry3f &invT) const { 59 | return t2v(invT * _priorMean); 60 | } 61 | 62 | SE3AbsolutePrior::SE3AbsolutePrior(const Eigen::Isometry3f &referenceTransform_, 63 | const Eigen::Isometry3f &priorMean_, 64 | const Matrix6f &information_): 65 | SE3Prior(priorMean_, information_) { 66 | setReferenceTransform(referenceTransform_); 67 | } 68 | 69 | Vector6f SE3AbsolutePrior::error(const Eigen::Isometry3f &invT) const { 70 | //Vector6f r; 71 | //r.setZero(); 72 | //return r; 73 | return t2v(invT * _inverseReferenceTransform * _priorMean); 74 | } 75 | 76 | } 77 | -------------------------------------------------------------------------------- /nicp/nicp/se3_prior.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "bm_se3.h" 4 | 5 | /* 6 | Base class that implements a simple prior on te transformation to be used in SE2 pose. 7 | The prior is defined as a gaussian distribution centered in a certain value (priorMean), 8 | and having a certain information matrix; 9 | 10 | The prior has to affect the transform between the current and the treference vertex. 11 | So it is relative by nature. However we can also enforce absolute priors, 12 | by remapping a global constraint as a relative one. 13 | 14 | 15 | Since we are operating on a manifold measurement space, the infromation matrix in the error 16 | space depends on the linearization point. 17 | 18 | To use this class: 19 | 0) derive it and overwrite the error(...) function in the desired classes to reflect the prior type. 20 | 1) set the priorMean and the priorInformation to the desired values 21 | 2) get the information of the error (omega), through the 22 | errorInformation(invT) function 23 | 3) get the error and the jacobian to compute H and b; 24 | */ 25 | 26 | namespace nicp { 27 | 28 | class SE3Prior { 29 | public: 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 31 | 32 | SE3Prior(const Eigen::Isometry3f &priorMean = Eigen::Isometry3f::Identity(), 33 | const Matrix6f &information = Matrix6f::Zero()); 34 | virtual ~SE3Prior() {} 35 | 36 | inline void setMean(const Eigen::Isometry3f &priorMean_) { _priorMean = priorMean_; } 37 | inline const Eigen::Isometry3f& mean() const { return _priorMean; } 38 | 39 | inline void setInformation(const Matrix6f &priorInformation_) { _priorInformation = priorInformation_; } 40 | inline const Matrix6f& information() const { return _priorInformation; } 41 | 42 | // Computes the error of the prior at the inverse transform 43 | virtual Vector6f error(const Eigen::Isometry3f &invT) const = 0; 44 | 45 | // Computes the jacobian of the error 46 | Matrix6f jacobian(const Eigen::Isometry3f &invT) const; 47 | 48 | // Projects the information matrix of the prior in the error space 49 | Matrix6f errorInformation(const Eigen::Isometry3f &invT) const; 50 | 51 | protected: 52 | // Computes the jacobian of the error w.r.t. the priorMean 53 | // Used internally to project the information matrix in the measurement space 54 | Matrix6f jacobianZ(const Eigen::Isometry3f &invT) const; 55 | 56 | mutable Eigen::Isometry3f _priorMean; 57 | Matrix6f _priorInformation; 58 | }; 59 | 60 | /* 61 | The error induced by the prior is defined as 62 | t2v(invT * priorMean), 63 | where invT s the *inverse* of the transformation we are looking for. 64 | */ 65 | 66 | class SE3RelativePrior: public SE3Prior{ 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 69 | 70 | SE3RelativePrior(const Eigen::Isometry3f &priorMean = Eigen::Isometry3f::Identity(), 71 | const Matrix6f &information = Matrix6f::Zero()); 72 | virtual ~SE3RelativePrior() {} 73 | 74 | // Computes the error of the prior at the inverse transform 75 | virtual Vector6f error(const Eigen::Isometry3f &invT) const; 76 | }; 77 | 78 | 79 | /* 80 | 81 | The error induced by the prior is defined as 82 | t2v(invT * _referenceTransform.inverse()*priorMean), 83 | where invT s the *inverse* of the transformation we are looking for. 84 | */ 85 | 86 | class SE3AbsolutePrior : public SE3Prior{ 87 | public: 88 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 89 | 90 | SE3AbsolutePrior(const Eigen::Isometry3f &referenceTransform_ = Eigen::Isometry3f::Identity(), 91 | const Eigen::Isometry3f &priorMean = Eigen::Isometry3f::Identity(), 92 | const Matrix6f &information = Matrix6f::Zero()); 93 | virtual ~SE3AbsolutePrior() {} 94 | 95 | // Computes the error of the prior at the inverse transform 96 | virtual Vector6f error(const Eigen::Isometry3f &invT) const; 97 | 98 | inline const Eigen::Isometry3f& referenceTransform() const { return _referenceTransform; } 99 | inline void setReferenceTransform(const Eigen::Isometry3f &referenceTransform_) { 100 | _referenceTransform = referenceTransform_; 101 | _inverseReferenceTransform = referenceTransform_.inverse(); 102 | } 103 | 104 | protected: 105 | Eigen::Isometry3f _referenceTransform; 106 | Eigen::Isometry3f _inverseReferenceTransform; 107 | }; 108 | 109 | } 110 | -------------------------------------------------------------------------------- /nicp/nicp/statscalculator.cpp: -------------------------------------------------------------------------------- 1 | #include "statscalculator.h" 2 | 3 | namespace nicp { 4 | 5 | void StatsCalculator::compute(NormalVector &normals, 6 | StatsVector &statsVector, 7 | const PointVector &points, 8 | const IntImage &indexImage) { 9 | assert(indexImage.rows > 0 && indexImage.cols > 0 && "StatsCalculator: indexImage has zero size"); 10 | assert(points.size() > 0 && "StatsCalculator: points has zero size"); 11 | 12 | // HAKKE in order to avoid an annoying warning 13 | if(indexImage.rows > 0 && indexImage.cols > 0) { 14 | assert(1); 15 | } 16 | 17 | if(statsVector.size() != points.size()) 18 | statsVector.resize(points.size()); 19 | if(normals.size() != points.size()) 20 | normals.resize(points.size()); 21 | Normal dummyNormal = Normal::Zero(); 22 | std::fill(statsVector.begin(), statsVector.end(), Stats()); 23 | std::fill(normals.begin(), normals.end(), dummyNormal); 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /nicp/nicp/statscalculator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stats.h" 4 | #include "definitions.h" 5 | 6 | namespace nicp { 7 | 8 | /** \class StatsCalculator statscalculator.h "statscalculator.h" 9 | * \brief Base class interface for point properties computation. 10 | * 11 | * This class provides an easy interface for point properties computation including 12 | * normals computation. 13 | * It should be extended in order to implement your own properties to compute. 14 | */ 15 | class StatsCalculator { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 18 | 19 | /** 20 | * Empty constructor. 21 | */ 22 | StatsCalculator() {} 23 | 24 | /** 25 | * Destructor. 26 | */ 27 | virtual ~StatsCalculator() {} 28 | 29 | /** 30 | * This virtual method computes the normals and additional properties of an input vector of points. 31 | * @param normals is the vector where the normals computed are stored. 32 | * @param statsVector is the vector where the additional properties computed are stored. 33 | * @param points is the input vector of points. 34 | * @param indexImage is the IndexImage containing the indeces of the depth image points in the vector of points. 35 | */ 36 | virtual void compute(NormalVector &normals, 37 | StatsVector &statsVector, 38 | const PointVector &points, 39 | const IntImage &indexImage); 40 | }; 41 | 42 | } 43 | -------------------------------------------------------------------------------- /nicp/nicp/statscalculatorintegralimage.cpp: -------------------------------------------------------------------------------- 1 | #include "statscalculatorintegralimage.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "cloud.h" 8 | 9 | #include 10 | 11 | namespace nicp { 12 | StatsCalculatorIntegralImage::StatsCalculatorIntegralImage() : StatsCalculator() { 13 | _worldRadius = 0.1f; 14 | _maxImageRadius = 30; 15 | _minImageRadius = 10; 16 | _minPoints = 50; 17 | _curvatureThreshold = 0.02f; 18 | } 19 | 20 | void StatsCalculatorIntegralImage::compute(NormalVector &normals, 21 | StatsVector &statsVector, 22 | const PointVector &points, 23 | const IntImage &indexImage, 24 | bool suppressImageRadius) { 25 | assert(points.size() > 0 && "StatsCalculatorIntegralImage: points has zero size"); 26 | assert(indexImage.rows > 0 && indexImage.cols > 0 && "StatsCalculatorIntegralImage: indexImage has zero size"); 27 | assert(_intervalImage.rows > 0 && _intervalImage.cols > 0 && "StatsCalculatorIntegralImage: _intervalImage has zero size"); 28 | if(statsVector.size() != points.size()) 29 | statsVector.resize(points.size()); 30 | if(normals.size() != points.size()) 31 | normals.resize(points.size()); 32 | Normal dummyNormal = Normal::Zero(); 33 | std::fill(normals.begin(), normals.end(), dummyNormal); 34 | std::fill(statsVector.begin(), statsVector.end(), Stats()); 35 | // Computing the integral image 36 | _integralImage.compute(indexImage, points); 37 | 38 | #pragma omp parallel for 39 | for(int r = 0; r < indexImage.rows; ++r) { 40 | const int *index = &indexImage(r, 0); 41 | const cv::Vec2i *interval = &_intervalImage(r, 0); 42 | for(int c = 0; c < indexImage.cols; ++c, ++index, ++interval) { 43 | // is the point valid, is its range valid? 44 | cv::Vec2i imageRadius = *interval; 45 | if(*index < 0 || (*interval)(0) < 0 || (*interval)(1) < 0) { continue; } 46 | 47 | if(!suppressImageRadius) { 48 | if(imageRadius(0) < _minImageRadius || imageRadius(1) < _minImageRadius) { continue; } 49 | if(imageRadius(0) > _maxImageRadius) { imageRadius(0) = _maxImageRadius; } 50 | if(imageRadius(1) > _maxImageRadius) { imageRadius(1) = _maxImageRadius; } 51 | 52 | } 53 | 54 | assert(*index < (int)statsVector.size() && "StatsCalculatorIntegralImage: index value greater than statsVector size"); 55 | 56 | const PointAccumulator &acc = _integralImage.getRegion(c - imageRadius(0), c + imageRadius(0), 57 | r - imageRadius(1), r + imageRadius(1)); 58 | if(acc.n() < _minPoints) { continue; } 59 | 60 | Eigen::SelfAdjointEigenSolver eigenSolver; 61 | eigenSolver.computeDirect(acc.covariance().block<3, 3>(0, 0), Eigen::ComputeEigenvectors); 62 | 63 | Stats &stats = statsVector[*index]; 64 | Normal &normal = normals[*index]; 65 | const Point &point = points[*index]; 66 | 67 | stats.setZero(); 68 | stats.setEigenVectors(eigenSolver.eigenvectors()); 69 | stats.setMean(acc.mean()); 70 | Eigen::Vector3f eigenValues = eigenSolver.eigenvalues(); 71 | if(eigenValues(0) < 0.0f) { eigenValues(0) = 0.0f; } 72 | stats.setEigenValues(eigenValues); 73 | stats.setN(acc.n()); 74 | 75 | normal = stats.block<4, 1>(0, 0); 76 | if(stats.curvature() < _curvatureThreshold) { 77 | if(normal.dot(point) > 0) { normal = -normal; } 78 | } 79 | else { normal.setZero(); } 80 | } 81 | } 82 | } 83 | 84 | } 85 | -------------------------------------------------------------------------------- /nicp/nicp/transformable_vector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** \struct TransformableVector 10 | * \brief Base class for the creation and manipulation of vectors of objects. 11 | * 12 | * This class allows to create and manipulate a vector of object where the type of the objects composing 13 | * the vector is specified inside the template. The only manipulation allowed is the transformation of all 14 | * obejcts composing the vector by means of a matrix multiplication. 15 | */ 16 | 17 | template 18 | struct TransformableVector: public std::vector > { 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 20 | 21 | /** 22 | * Empty constructor. 23 | * This constructor creates an TransformableVector object using the empty constructor of 24 | * the std vector class and imposing the types of the objects to use to be equal to the type 25 | * specified inside the template. 26 | */ 27 | TransformableVector() : std::vector >() {} 28 | 29 | /** 30 | * Constructor specifying the vector dimension. 31 | * This constructor creates an TransformableVector object using the constructor of 32 | * the std vector class and imposing the types of the objects to use to be equal to the type 33 | * specified inside the template. The dimension of the vector is initialized to the value given in input. 34 | * @param s is the size value used to initialize the vector. 35 | */ 36 | TransformableVector(size_t s) : std::vector >(s) {} 37 | 38 | /** 39 | * This method transforms all the objects inside the vector by multiplying each element with the object given 40 | * in input. The type of the input object is defined inside the template. 41 | * @param m is a generic object used to transform the vector elements via multiplication. The type of this 42 | * object must be defined inside the template. 43 | */ 44 | template 45 | inline void transformInPlace(const OtherDerived& m) { 46 | Transformable* t = &(*this)[0]; 47 | for (size_t i = 0; i < std::vector >::size(); i++, t++) { 48 | *t = m * (*t); 49 | } 50 | } 51 | 52 | /** 53 | * This method transforms all the objects inside the vector by multiplying each element with the object m given 54 | * in input. The type of m must be defined inside the template. The original vector is not modified, the result is 55 | * inserted in the TransformableVector dest given in input. 56 | * @param dest is the destination vector where the trasformed elements are saved. 57 | * @param m is a generic object used to transform the vector elements via multiplication. The type of this 58 | * object must be defined inside the template. 59 | */ 60 | template 61 | inline void transform(TransformableVector& dest, const OtherDerived& m) const { 62 | dest.resize(this->size()); 63 | const Transformable* tSrc= &(*this)[0]; 64 | Transformable* tDest= &dest[0]; 65 | for (size_t i = 0; i < std::vector >::size(); ++i, ++tSrc, ++tDest ) { 66 | *tDest = m * (*tSrc); 67 | } 68 | } 69 | 70 | }; 71 | -------------------------------------------------------------------------------- /nicp/nicp/unscented.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | 9 | namespace nicp { 10 | 11 | template 12 | struct SigmaPoint { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | SigmaPoint(const SampleType& sample, double wi, double wp): 15 | _sample(sample), _wi(wi), _wp(wp){} 16 | SigmaPoint(): _wi(0), _wp(0) {} 17 | SampleType _sample; 18 | double _wi; 19 | double _wp; 20 | }; 21 | 22 | 23 | template 24 | bool sampleUnscented(std::vector, Eigen::aligned_allocator > >& sigmaPoints, const SampleType& mean, const CovarianceType& covariance){ 25 | 26 | const int dim = mean.size(); 27 | const int numPoints = 2 * dim + 1; 28 | assert (covariance.rows() == covariance.cols() && covariance.cols() == mean.size() && "Dimension Mismatch"); 29 | const double alpha = 1e-3; 30 | const double beta = 2.; 31 | const double lambda = alpha * alpha * dim; 32 | const double wi = 1./(2. * (dim + lambda) ); 33 | 34 | sigmaPoints.resize(numPoints); 35 | sigmaPoints[0] = SigmaPoint(mean, 36 | lambda/(dim + lambda), 37 | lambda/(dim + lambda) + (1.-alpha*alpha+beta) ); 38 | LLT cholDecomp; 39 | cholDecomp.compute(covariance*(dim+lambda)); 40 | if (cholDecomp.info()==Eigen::NumericalIssue) 41 | return false; 42 | const CovarianceType& L=cholDecomp.matrixL(); 43 | int k=1; 44 | for (int i=0; i(mean + s, wi, wi); 47 | sigmaPoints[k++]=SigmaPoint(mean - s, wi, wi); 48 | } 49 | return true; 50 | } 51 | 52 | template 53 | void reconstructGaussian(SampleType& mean, CovarianceType& covariance, 54 | const std::vector, Eigen::aligned_allocator > >& sigmaPoints){ 55 | 56 | mean.fill(0); 57 | covariance.fill(0); 58 | for (size_t i=0; isecond; 38 | voxelAccumulator.add(point); 39 | } 40 | } 41 | 42 | std::cout << "Voxelization resized the cloud from " << cloud.points().size() << " to "; 43 | // HAKKE 44 | // cloud.clear(); 45 | Cloud tmpCloud; 46 | tmpCloud.clear(); 47 | for(AccumulatorMap::iterator it = accumulatorMap.begin(); it != accumulatorMap.end(); it++) { 48 | VoxelAccumulator &voxelAccumulator = it->second; 49 | // HAKKE 50 | // Point average = voxelAccumulator.average(); 51 | // cloud.points().push_back(average); 52 | tmpCloud.points().push_back(cloud.points()[voxelAccumulator.index]); 53 | tmpCloud.normals().push_back(cloud.normals()[voxelAccumulator.index]); 54 | tmpCloud.stats().push_back(cloud.stats()[voxelAccumulator.index]); 55 | if(cloud.pointInformationMatrix().size() == cloud.points().size() && 56 | cloud.normalInformationMatrix().size() == cloud.points().size()) { 57 | tmpCloud.pointInformationMatrix().push_back(cloud.pointInformationMatrix()[voxelAccumulator.index]); 58 | tmpCloud.normalInformationMatrix().push_back(cloud.normalInformationMatrix()[voxelAccumulator.index]); 59 | } 60 | // if(cloud.traversabilityVector().size() == cloud.points().size()) { 61 | // tmpCloud.traversabilityVector().push_back(cloud.traversabilityVector()[voxelAccumulator.index]); 62 | // } 63 | if(cloud.gaussians().size() == cloud.points().size()) { 64 | tmpCloud.gaussians().push_back(cloud.gaussians()[voxelAccumulator.index]); 65 | } 66 | } 67 | 68 | // HAKKE 69 | cloud.clear(); 70 | cloud = tmpCloud; 71 | 72 | std::cout << cloud.points().size() << " points" << std::endl; 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /nicp/nicp/voxelcalculator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "cloud.h" 4 | 5 | #include 6 | 7 | using namespace std; 8 | using namespace Eigen; 9 | 10 | namespace nicp { 11 | 12 | class VoxelCalculator { 13 | struct VoxelAccumulator { 14 | Point accumulator; 15 | int numPoints; 16 | // HAKKE 17 | int index; 18 | 19 | VoxelAccumulator() { 20 | accumulator[0] = accumulator[1] = accumulator[2] = 0.0f; 21 | numPoints = 0; 22 | // HAKKE 23 | index = -1; 24 | } 25 | 26 | void add(const Point &/*v*/) { 27 | // HAKKE 28 | /* numPoints++; */ 29 | /* accumulator += v; */ 30 | } 31 | 32 | Point average() const { 33 | float f = 1.0f / numPoints; 34 | return accumulator * f; 35 | } 36 | }; 37 | 38 | struct IndexComparator { 39 | int indeces[3]; 40 | bool operator < (const IndexComparator &s) const { 41 | if(indeces[0] < s.indeces[0]) return true; 42 | if(indeces[0] == s.indeces[0] && indeces[1] < s.indeces[1]) return true; 43 | if(indeces[1] == s.indeces[1] && indeces[2] < s.indeces[2]) return true; 44 | return false; 45 | } 46 | }; 47 | 48 | typedef map AccumulatorMap; 49 | 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 52 | 53 | VoxelCalculator() { _resolution = 0.01f; } 54 | virtual ~VoxelCalculator() {} 55 | 56 | inline float resolution() const { return _resolution; } 57 | inline void setResolution(float resolution_) { _resolution = resolution_; } 58 | 59 | void compute(Cloud &cloud, float resolution); 60 | void compute(Cloud &cloud); 61 | 62 | protected: 63 | float _resolution; 64 | }; 65 | 66 | } 67 | -------------------------------------------------------------------------------- /nicp/nicp_conf/gicp_eth_kinect.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 5 | breakingAngle 0.0 6 | breakingDistance 0.0 7 | breakingInlierRatio 1.0 8 | demoteToGICP 1 9 | 10 | // Projector 11 | K 131.25 131.25 79.875 59.875 12 | minDistance 0.01 13 | maxDistance 4 14 | imageSize 120 160 15 | 16 | // Stats calculator and information matrix calculators 17 | minImageRadius 5 18 | maxImageRadius 20 19 | minPoints 10 20 | statsCalculatorCurvatureThreshold 1 21 | worldRadius 0.1 22 | informationMatrixCurvatureThreshold 1 23 | 24 | // Correspondence finder 25 | inlierDistanceThreshold 0.5 26 | inlierNormalAngularThreshold 0.95 27 | inlierCurvatureRatioThreshold 1.3 28 | correspondenceFinderCurvatureThreshold 1 29 | 30 | // Linearizer 31 | inlierMaxChi2 1000 32 | robustKernel 1 33 | zScaling 1 34 | 35 | // Aligner 36 | outerIterations 10 37 | innerIterations 1 38 | minInliers 10000 39 | translationalMinEigenRatio 200 40 | rotationalMinEigenRatio 200 41 | lambda 10000 42 | 43 | // Merger 44 | depthThreshold 4.5 45 | normalThreshold 1.0 46 | distanceThreshold 0.0 -------------------------------------------------------------------------------- /nicp/nicp_conf/gicp_eth_laser.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | sensorOffset 0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 5 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 6 | breakingAngle 0.0 7 | breakingDistance 0.0 8 | breakingInlierRatio 1.0 9 | demoteToGICP 1 10 | 11 | // Projector 12 | horizontalFov 3.14 13 | verticalFov 1.57 14 | minDistance 1.5 15 | maxDistance 10 16 | imageSize 1000 1500 17 | 18 | // Stats calculator and information matrix calculators 19 | minImageRadius 10 20 | maxImageRadius 100 21 | minPoints 30 22 | statsCalculatorCurvatureThreshold 1 23 | worldRadius 0.25 24 | informationMatrixCurvatureThreshold 1 25 | 26 | // Correspondence finder 27 | inlierDistanceThreshold 1.5 28 | inlierNormalAngularThreshold 0.9 29 | inlierCurvatureRatioThreshold 1.3 30 | correspondenceFinderCurvatureThreshold 1 31 | 32 | // Linearizer 33 | inlierMaxChi2 9000 34 | robustKernel 1 35 | zScaling 0 36 | omegaNScale 0 37 | 38 | // Aligner 39 | outerIterations 50 40 | innerIterations 1 41 | minInliers 10000 42 | translationalMinEigenRatio 20000000 43 | rotationalMinEigenRatio 20000000 44 | lambda 1000 -------------------------------------------------------------------------------- /nicp/nicp_conf/kinfu_eth_kinect.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScale 0.001 3 | imageScale 1 4 | tx 0.0 5 | ty 0.0 6 | tz 0.0 7 | qx 0.0 8 | qy 0.0 9 | qz 0.0 10 | qw 1.0 11 | 12 | // Projector 13 | fx 131.25 14 | fy 131.25 15 | cx 79.875 16 | cy 59.875 17 | rows 120 18 | cols 160 19 | 20 | // KinFu 21 | volumeSize 3.0 22 | shiftDistance 1.5 -------------------------------------------------------------------------------- /nicp/nicp_conf/ndt_eth_kinect.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScale 0.001 3 | imageScale 1 4 | tx 0.0 5 | ty 0.0 6 | tz 0.0 7 | qx 0.0 8 | qy 0.0 9 | qz 0.0 10 | qw 1.0 11 | 12 | // Projector 13 | fx 131.25 14 | fy 131.25 15 | cx 79.875 16 | cy 59.875 17 | rows 120 18 | cols 160 19 | 20 | // NDT 21 | r0 0.1 22 | r1 0.2 23 | r2 0.3 24 | r3 0.6 25 | transformationEpsilon 0.001 26 | resolution 0.1 27 | maximumIterations 30 -------------------------------------------------------------------------------- /nicp/nicp_conf/ndt_eth_laser.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | sensorOffset 0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 5 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 6 | breakingAngle 0.0 7 | breakingDistance 0.0 8 | breakingInlierRatio 1.0 9 | demoteToGICP 0 10 | 11 | // Projector 12 | horizontalFov 3.14 13 | verticalFov 1.57 14 | minDistance 0.5 15 | maxDistance 10 16 | imageSize 1000 1500 17 | 18 | // Stats calculator and information matrix calculators 19 | minImageRadius 10 20 | maxImageRadius 100 21 | minPoints 30 22 | statsCalculatorCurvatureThreshold 0.05 23 | worldRadius 0.25 24 | informationMatrixCurvatureThreshold 0.05 25 | 26 | // NDT 27 | res 0.1 0.2 0.5 1.0 28 | transformationEpsilon 0.001 29 | resolution 0.1 30 | maximumIterations 30 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_aligner_1_1.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScale 0.001 3 | imageScale 1 4 | tx 0.0 5 | ty 0.0 6 | tz 0.0 7 | qx 0.0 8 | qy 0.0 9 | qz 0.0 10 | qw 1.0 11 | chunkStep 5 12 | 13 | // Projector 14 | fx 525.0 15 | fy 525.0 16 | cx 319.5 17 | cy 239.5 18 | minDistance 0.5 19 | maxDistance 4.5 20 | 21 | // Stats calculator and information matrix calculators 22 | minImageRadius 10 23 | maxImageRadius 30 24 | minPoints 50 25 | curvatureThreshold 0.2 26 | worldRadius 0.1 27 | informationMatrixCurvatureThreshold 0.02 28 | 29 | // Correspondence finder 30 | inlierDistanceThreshold 1.0 31 | inlierNormalAngularThreshold 0.95 32 | inlierCurvatureRatioThreshold 1.3 33 | flatCurvatureThreshold 0.02 34 | 35 | // Linearizer 36 | inlierMaxChi2 9000 37 | robustKernel 1 38 | 39 | // Aligner 40 | outerIterations 10 41 | innerIterations 1 42 | minInliers 100 43 | translationalMinEigenRatio 50 44 | rotationalMinEigenRatio 50 45 | 46 | // Merger 47 | depthThreshold 4.5 48 | normalThreshold 0.99 49 | distanceThreshold 0.1 50 | -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_aligner_1_4.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScale 0.001 3 | imageScale 4 4 | tx 0.0 5 | ty 0.0 6 | tz 0.0 7 | qx 0.0 8 | qy 0.0 9 | qz 0.0 10 | qw 1.0 11 | chunkStep 20 12 | 13 | // Projector 14 | fx 525.0 15 | fy 525.0 16 | cx 319.5 17 | cy 239.5 18 | minDistance 0.5 19 | maxDistance 4.5 20 | 21 | // Stats calculator and information matrix calculators 22 | minImageRadius 3 23 | maxImageRadius 6 24 | minPoints 10 25 | curvatureThreshold 0.2 26 | worldRadius 0.1 27 | informationMatrixCurvatureThreshold 0.02 28 | 29 | // Correspondence finder 30 | inlierDistanceThreshold 0.5 31 | inlierNormalAngularThreshold 0.95 32 | inlierCurvatureRatioThreshold 1.3 33 | flatCurvatureThreshold 0.02 34 | 35 | // Linearizer 36 | inlierMaxChi2 9000 37 | robustKernel 1 38 | 39 | // Aligner 40 | outerIterations 10 41 | innerIterations 1 42 | minInliers 100 43 | translationalMinEigenRatio 50 44 | rotationalMinEigenRatio 50 45 | 46 | // Merger 47 | depthThreshold 4.5 48 | normalThreshold 0.99 49 | distanceThreshold 0.1 50 | -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_kinect.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 5 | breakingAngle 0.0 6 | breakingDistance 0.0 7 | breakingInlierRatio 1.0 8 | demoteToGICP 0 9 | 10 | // Projector 11 | K 131.25 131.25 79.875 59.875 12 | minDistance 0.01 13 | maxDistance 4 14 | imageSize 120 160 15 | 16 | // Stats calculator and information matrix calculators 17 | minImageRadius 5 18 | maxImageRadius 20 19 | minPoints 10 20 | statsCalculatorCurvatureThreshold 0.3 21 | worldRadius 0.1 22 | informationMatrixCurvatureThreshold 0.1 23 | 24 | // Correspondence finder 25 | inlierDistanceThreshold 0.5 26 | inlierNormalAngularThreshold 0.95 27 | inlierCurvatureRatioThreshold 1.3 28 | correspondenceFinderCurvatureThreshold 0.3 29 | 30 | // Linearizer 31 | inlierMaxChi2 1000 32 | robustKernel 1 33 | zScaling 1 34 | 35 | // Aligner 36 | outerIterations 10 37 | innerIterations 1 38 | minInliers 10000 39 | translationalMinEigenRatio 200 40 | rotationalMinEigenRatio 200 41 | lambda 10000 42 | 43 | // Merger 44 | depthThreshold 4.5 45 | normalThreshold 1.0 46 | distanceThreshold 0.0 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_kinect_estimate_visualization.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 5 | breakingAngle 1.57 6 | breakingDistance 1.0 7 | breakingInlierRatio 0.6 8 | demoteToGICP 0 9 | 10 | // Projector 11 | K 131.25 131.25 79.875 59.875 12 | minDistance 0.01 13 | maxDistance 3 14 | imageSize 120 160 15 | 16 | // Stats calculator and information matrix calculators 17 | minImageRadius 2 18 | maxImageRadius 30 19 | minPoints 5 20 | statsCalculatorCurvatureThreshold 1 21 | worldRadius 0.1 22 | informationMatrixCurvatureThreshold 1 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_kinect_incremental.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 5 | breakingAngle 1.57 6 | breakingDistance 1.0 7 | breakingInlierRatio 0.6 8 | demoteToGICP 0 9 | 10 | // Projector 11 | K 131.25 131.25 79.875 59.875 12 | minDistance 0.01 13 | maxDistance 4 14 | imageSize 120 160 15 | 16 | // Stats calculator and information matrix calculators 17 | minImageRadius 5 18 | maxImageRadius 20 19 | minPoints 10 20 | statsCalculatorCurvatureThreshold 0.3 21 | worldRadius 0.1 22 | informationMatrixCurvatureThreshold 0.1 23 | 24 | // Correspondence finder 25 | inlierDistanceThreshold 0.5 26 | inlierNormalAngularThreshold 0.95 27 | inlierCurvatureRatioThreshold 1.3 28 | correspondenceFinderCurvatureThreshold 0.3 29 | 30 | // Linearizer 31 | inlierMaxChi2 1000 32 | robustKernel 1 33 | zScaling 1 34 | 35 | // Aligner 36 | outerIterations 10 37 | innerIterations 1 38 | minInliers 10000 39 | translationalMinEigenRatio 200 40 | rotationalMinEigenRatio 200 41 | lambda 10000 42 | 43 | // Merger 44 | depthThreshold 4.5 45 | normalThreshold 0.9 46 | distanceThreshold 0.1 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_laser.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | sensorOffset 0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 5 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 6 | breakingAngle 0.0 7 | breakingDistance 0.0 8 | breakingInlierRatio 1.0 9 | demoteToGICP 0 10 | 11 | // Projector 12 | horizontalFov 3.14 13 | verticalFov 1.57 14 | minDistance 1.5 15 | maxDistance 10 16 | imageSize 1000 1500 17 | 18 | // Stats calculator and information matrix calculators 19 | minImageRadius 10 20 | maxImageRadius 100 21 | minPoints 30 22 | statsCalculatorCurvatureThreshold 0.075 23 | worldRadius 0.25 24 | informationMatrixCurvatureThreshold 0.075 25 | 26 | // Correspondence finder 27 | inlierDistanceThreshold 1.5 28 | inlierNormalAngularThreshold 0.9 29 | inlierCurvatureRatioThreshold 1.3 30 | correspondenceFinderCurvatureThreshold 0.075 31 | 32 | // Linearizer 33 | inlierMaxChi2 9000 34 | robustKernel 1 35 | zScaling 0 36 | omegaNScale 1 37 | 38 | // Aligner 39 | outerIterations 50 40 | innerIterations 1 41 | minInliers 10000 42 | translationalMinEigenRatio 20000000 43 | rotationalMinEigenRatio 20000000 44 | lambda 1000 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_laser_estimate_visualization.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | sensorOffset 0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 5 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 6 | 7 | // Projector 8 | horizontalFov 3.14 9 | verticalFov 1.57 10 | minDistance 1.5 11 | maxDistance 10 12 | imageSize 1000 1500 13 | 14 | // Stats calculator and information matrix calculators 15 | minImageRadius 10 16 | maxImageRadius 100 17 | minPoints 30 18 | statsCalculatorCurvatureThreshold 0.075 19 | worldRadius 0.25 20 | informationMatrixCurvatureThreshold 0.075 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_laser_nscale_33.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | sensorOffset 0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 5 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 6 | breakingAngle 0.0 7 | breakingDistance 0.0 8 | breakingInlierRatio 1.0 9 | demoteToGICP 0 10 | 11 | // Projector 12 | horizontalFov 3.14 13 | verticalFov 1.57 14 | minDistance 1.5 15 | maxDistance 10 16 | imageSize 1000 1500 17 | 18 | // Stats calculator and information matrix calculators 19 | minImageRadius 10 20 | maxImageRadius 100 21 | minPoints 30 22 | statsCalculatorCurvatureThreshold 0.075 23 | worldRadius 0.25 24 | informationMatrixCurvatureThreshold 0.075 25 | 26 | // Correspondence finder 27 | inlierDistanceThreshold 1.5 28 | inlierNormalAngularThreshold 0.9 29 | inlierCurvatureRatioThreshold 1.3 30 | correspondenceFinderCurvatureThreshold 0.075 31 | 32 | // Linearizer 33 | inlierMaxChi2 9000 34 | robustKernel 1 35 | zScaling 0 36 | omegaNScale 0.33 37 | 38 | // Aligner 39 | outerIterations 50 40 | innerIterations 1 41 | minInliers 10000 42 | translationalMinEigenRatio 20000000 43 | rotationalMinEigenRatio 20000000 44 | lambda 1000 -------------------------------------------------------------------------------- /nicp/nicp_conf/nicp_eth_laser_nscale_66.conf: -------------------------------------------------------------------------------- 1 | // General 2 | depthScaling 0.001 3 | imageScaling 1 4 | sensorOffset 0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 5 | startingPose 0.0 0.0 0.0 0.0 0.0 0.0 1.0 6 | breakingAngle 0.0 7 | breakingDistance 0.0 8 | breakingInlierRatio 1.0 9 | demoteToGICP 0 10 | 11 | // Projector 12 | horizontalFov 3.14 13 | verticalFov 1.57 14 | minDistance 1.5 15 | maxDistance 10 16 | imageSize 1000 1500 17 | 18 | // Stats calculator and information matrix calculators 19 | minImageRadius 10 20 | maxImageRadius 100 21 | minPoints 30 22 | statsCalculatorCurvatureThreshold 0.075 23 | worldRadius 0.25 24 | informationMatrixCurvatureThreshold 0.075 25 | 26 | // Correspondence finder 27 | inlierDistanceThreshold 1.5 28 | inlierNormalAngularThreshold 0.9 29 | inlierCurvatureRatioThreshold 1.3 30 | correspondenceFinderCurvatureThreshold 0.075 31 | 32 | // Linearizer 33 | inlierMaxChi2 9000 34 | robustKernel 1 35 | zScaling 0 36 | omegaNScale 0.66 37 | 38 | // Aligner 39 | outerIterations 50 40 | innerIterations 1 41 | minInliers 10000 42 | translationalMinEigenRatio 20000000 43 | rotationalMinEigenRatio 20000000 44 | lambda 1000 -------------------------------------------------------------------------------- /nicp/nicp_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(nicp_cloud2spherical nicp_cloud2spherical.cpp) 2 | SET_TARGET_PROPERTIES(nicp_cloud2spherical PROPERTIES OUTPUT_NAME nicp_cloud2spherical) 3 | TARGET_LINK_LIBRARIES(nicp_cloud2spherical nicp ${OpenCV_LIB}) -------------------------------------------------------------------------------- /nicp/nicp_viewer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(UI_HEADERS gl_parameter.h) 2 | 3 | QT5_WRAP_UI(UI_HEADERS nicp_aligner_gui_ui_main_window.ui) 4 | QT5_WRAP_CPP(UI_SOURCES ${UI_HEADERS} nicp_aligner_gui_main_window.h) 5 | 6 | ADD_LIBRARY(nicp_viewer SHARED 7 | drawable.cpp drawable.h 8 | drawable_correspondences.cpp drawable_correspondences.h 9 | drawable_covariances.cpp drawable_covariances.h 10 | drawable_cloud.cpp drawable_cloud.h 11 | drawable_normals.cpp drawable_normals.h 12 | drawable_points.cpp drawable_points.h 13 | drawable_trajectory.cpp drawable_trajectory.h 14 | drawable_transform_covariance.cpp drawable_transform_covariance.h 15 | drawable_uncertainty.cpp drawable_uncertainty.h 16 | gl_parameter.cpp gl_parameter.h 17 | gl_parameter_correspondences.cpp gl_parameter_correspondences.h 18 | gl_parameter_covariances.cpp gl_parameter_covariances.h 19 | gl_parameter_cloud.cpp gl_parameter_cloud.h 20 | gl_parameter_normals.cpp gl_parameter_normals.h 21 | gl_parameter_points.cpp gl_parameter_points.h 22 | gl_parameter_trajectory.cpp gl_parameter_trajectory.h 23 | gl_parameter_transform_covariance.cpp gl_parameter_transform_covariance.h 24 | gl_parameter_uncertainty.cpp gl_parameter_uncertainty.h 25 | imageview.cpp imageview.h 26 | nicp_qglviewer.cpp nicp_qglviewer.h 27 | opengl_primitives.cpp opengl_primitives.h 28 | nicp_aligner_gui_ui_main_window.h 29 | nicp_aligner_gui_main_window.cpp nicp_aligner_gui_main_window.h 30 | ${UI_HEADERS} 31 | ${UI_SOURCES} 32 | ) 33 | SET_TARGET_PROPERTIES(nicp_viewer PROPERTIES OUTPUT_NAME ${LIB_PREFIX}_viewer) 34 | TARGET_LINK_LIBRARIES(nicp_viewer nicp ${OpenCV_LIBS} ${QGLVIEWER_LIBRARY}) 35 | 36 | ADD_EXECUTABLE(nicp_cloud_prop_viewer nicp_cloud_prop_viewer.cpp) 37 | SET_TARGET_PROPERTIES(nicp_cloud_prop_viewer PROPERTIES OUTPUT_NAME nicp_cloud_prop_viewer) 38 | TARGET_LINK_LIBRARIES(nicp_cloud_prop_viewer nicp_viewer nicp 39 | ${QGLVIEWER_LIBRARY} Qt5::Xml Qt5::OpenGL Qt5::Gui Qt5::Core Qt5::Widgets 40 | ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 41 | ${OpenCV_LIBS} 42 | ) 43 | 44 | ADD_EXECUTABLE(nicp_aligner_gui nicp_aligner_gui.cpp) 45 | SET_TARGET_PROPERTIES(nicp_aligner_gui PROPERTIES OUTPUT_NAME nicp_aligner_gui) 46 | TARGET_LINK_LIBRARIES(nicp_aligner_gui nicp nicp_viewer 47 | ${QGLVIEWER_LIBRARY} Qt5::Xml Qt5::OpenGL Qt5::Gui Qt5::Core Qt5::Widgets 48 | ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 49 | ${OpenCV_LIBS} 50 | ) 51 | 52 | ADD_EXECUTABLE(nicp_simple_viewer nicp_simple_viewer.cpp ) 53 | INCLUDE_DIRECTORIES( 54 | ${CMAKE_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR} 55 | ${QGLVIEWER_INCLUDE_DIR} ${QT_QTCORE_INCLUDE_DIR} ${QT_QTCORE_INCLUDE_DIR}/..) 56 | TARGET_LINK_LIBRARIES(nicp_simple_viewer nicp_viewer nicp 57 | ${QGLVIEWER_LIBRARY} Qt5::Xml Qt5::OpenGL Qt5::Gui Qt5::Core Qt5::Widgets 58 | ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 59 | ) 60 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | Drawable::Drawable() { 6 | _transformation = Eigen::Isometry3f::Identity(); 7 | _viewer = 0; 8 | } 9 | 10 | Drawable::Drawable(Eigen::Isometry3f transformation_) { 11 | _transformation = transformation_; 12 | _viewer = 0; 13 | } 14 | 15 | } 16 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class NICPQGLViewer; 10 | 11 | class Drawable { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | Drawable(); 16 | Drawable(Eigen::Isometry3f transformation_); 17 | virtual ~Drawable() {} 18 | 19 | virtual void draw() {} 20 | 21 | Eigen::Isometry3f transformation() { return _transformation; } 22 | virtual void setTransformation(Eigen::Isometry3f transformation_) { _transformation = transformation_; } 23 | 24 | NICPQGLViewer* viewer() { return _viewer; } 25 | virtual void setViewer(NICPQGLViewer *viewer_) { _viewer = viewer_; } 26 | 27 | virtual GLParameter* parameter() = 0; 28 | virtual bool setParameter(GLParameter *parameter_) = 0; 29 | 30 | protected: 31 | Eigen::Isometry3f _transformation; 32 | NICPQGLViewer *_viewer; 33 | }; 34 | 35 | } 36 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_cloud.h" 2 | #include "opengl_primitives.h" 3 | 4 | namespace nicp_viewer { 5 | 6 | DrawableCloud::DrawableCloud(const Eigen::Isometry3f &transformation_, GLParameter *parameter_, 7 | Cloud *cloud_) : Drawable(transformation_) { 8 | setParameter(parameter_); 9 | _cloud = cloud_; 10 | _drawablePoints = 0; 11 | _drawableCorrespondences = 0; 12 | _drawableCovariances = 0; 13 | _drawableCorrespondences = 0; 14 | _previousDrawableCloud = 0; 15 | constructDrawableObjects(); 16 | } 17 | 18 | bool DrawableCloud::setParameter(GLParameter *parameter_) { 19 | GLParameterCloud *cloudParameter = (GLParameterCloud*)parameter_; 20 | if(cloudParameter == 0) { 21 | _parameter = 0; 22 | return false; 23 | } 24 | _parameter = cloudParameter; 25 | return true; 26 | } 27 | 28 | void DrawableCloud::clearDrawableObjects() { 29 | if (! _cloud) 30 | return; 31 | if(_drawablePoints) 32 | delete _drawablePoints; 33 | if(_drawableNormals) 34 | delete _drawableNormals; 35 | if(_drawableCovariances) 36 | delete _drawableCovariances; 37 | if(_drawableCorrespondences) 38 | delete _drawableCorrespondences; 39 | _drawablePoints = 0; 40 | _drawableNormals = 0; 41 | _drawableCovariances = 0; 42 | _drawableCorrespondences = 0; 43 | } 44 | 45 | void DrawableCloud::constructDrawableObjects(){ 46 | if(_cloud) { 47 | _drawablePoints = new DrawablePoints(Eigen::Isometry3f::Identity(), 48 | (GLParameter*)_parameter->parameterPoints(), &_cloud->points(), &_cloud->normals()); 49 | if (_cloud->rgbs().size()) { 50 | _drawablePoints->setRGBs(&(_cloud->rgbs())); 51 | } 52 | _drawableNormals = new DrawableNormals(Eigen::Isometry3f::Identity(), 53 | (GLParameter*)_parameter->parameterNormals(), &_cloud->points(), &_cloud->normals()); 54 | _drawableCovariances = new DrawableCovariances(Eigen::Isometry3f::Identity(), 55 | (GLParameter*)_parameter->parameterCovariances(), &_cloud->stats()); 56 | _drawableCorrespondences = new DrawableCorrespondences(); 57 | _drawableCorrespondences->setParameter((GLParameter*)_parameter->parameterCorrespondences()); 58 | } 59 | } 60 | 61 | void DrawableCloud::setCloud(Cloud *f) { 62 | if(f && f != _cloud) { 63 | clearDrawableObjects(); 64 | _cloud = f; 65 | constructDrawableObjects(); 66 | } 67 | } 68 | 69 | void DrawableCloud::draw() { 70 | if(_parameter->show() && _cloud) { 71 | glPushMatrix(); 72 | glMultMatrixf(_transformation.data()); 73 | if(_drawablePoints) 74 | _drawablePoints->draw(); 75 | if(_drawableNormals) 76 | _drawableNormals->draw(); 77 | if(_drawableCovariances) 78 | _drawableCovariances->draw(); 79 | if(_drawableCorrespondences) 80 | _drawableCorrespondences->draw(); 81 | 82 | glPushMatrix(); 83 | Eigen::Isometry3f sensorOffset; 84 | sensorOffset.translation() = Eigen::Vector3f(0.0f, 0.0f, 0.0f); 85 | Eigen::Quaternionf quaternion = Eigen::Quaternionf(-.5f, -0.5f, 0.5f, 0.5f); 86 | sensorOffset.linear() = quaternion.toRotationMatrix(); 87 | sensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f; 88 | glMultMatrixf(sensorOffset.data()); 89 | glColor4f(1,0,0,0.5); 90 | glPopMatrix(); 91 | 92 | glPopMatrix(); 93 | } 94 | } 95 | 96 | } 97 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_cloud.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp_qglviewer.h" 4 | #include "imageview.h" 5 | #include "drawable_points.h" 6 | #include "drawable_normals.h" 7 | #include "drawable_covariances.h" 8 | #include "drawable_correspondences.h" 9 | #include "gl_parameter.h" 10 | #include "gl_parameter_points.h" 11 | #include "gl_parameter_normals.h" 12 | #include "gl_parameter_covariances.h" 13 | #include "gl_parameter_correspondences.h" 14 | #include "gl_parameter_cloud.h" 15 | 16 | namespace nicp_viewer { 17 | 18 | class DrawableCloud : public Drawable { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 21 | 22 | DrawableCloud(const Eigen::Isometry3f &transformation_ = Eigen::Isometry3f::Identity(), 23 | GLParameter *parameter_ = 0, Cloud *cloud_ = 0); 24 | virtual ~DrawableCloud() { clearDrawableObjects(); } 25 | 26 | const Eigen::Isometry3f& localTransformation() const { return _localTransform; } 27 | void setLocalTransformation(const Eigen::Isometry3f &localTransform_) { _localTransform = localTransform_; } 28 | 29 | virtual GLParameter* parameter() { return _parameter; } 30 | virtual bool setParameter(GLParameter *parameter_); 31 | 32 | Cloud* cloud() const { return _cloud; } 33 | void setCloud(Cloud *cloud_); 34 | 35 | DrawableCorrespondences* drawableCorrespondences() { return _drawableCorrespondences; } 36 | void setDrawableCorrespondences(DrawableCorrespondences* drawableCorrespondences_) { 37 | if(_drawableCorrespondences) 38 | delete _drawableCorrespondences; 39 | _drawableCorrespondences = drawableCorrespondences_; 40 | } 41 | 42 | DrawablePoints* drawablePoints() { return _drawablePoints; } 43 | DrawableNormals* drawableNormals() { return _drawableNormals; } 44 | DrawableCovariances* drawableCovariances() { return _drawableCovariances; } 45 | 46 | void clearDrawableObjects(); 47 | void constructDrawableObjects(); 48 | 49 | void draw(); 50 | 51 | protected: 52 | Eigen::Isometry3f _localTransform; 53 | Cloud *_cloud; 54 | DrawableCloud* _previousDrawableCloud; 55 | GLParameterCloud * _parameter; 56 | DrawablePoints *_drawablePoints; 57 | DrawableNormals *_drawableNormals; 58 | DrawableCovariances *_drawableCovariances; 59 | DrawableCorrespondences *_drawableCorrespondences; 60 | }; 61 | 62 | } 63 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_correspondences.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_correspondences.h" 2 | #include "gl_parameter_correspondences.h" 3 | 4 | namespace nicp_viewer { 5 | 6 | DrawableCorrespondences::DrawableCorrespondences() : Drawable() { 7 | _parameter = 0; 8 | _numCorrespondences = 0; 9 | _correspondences = 0; 10 | _referencePoints = 0; 11 | _currentPoints = 0; 12 | _referencePointsTransformation = Eigen::Isometry3f::Identity(); 13 | _correspondenceDrawList = glGenLists(1); 14 | updateCorrespondenceDrawList(); 15 | } 16 | 17 | DrawableCorrespondences::DrawableCorrespondences(Eigen::Isometry3f transformation_, GLParameter *parameter_, int numCorrespondences_, PointVector *referencePoints_, 18 | PointVector *currentPoints_, CorrespondenceVector *correspondences_) : Drawable(transformation_) { 19 | setParameter(parameter_); 20 | _numCorrespondences = numCorrespondences_; 21 | _correspondences = correspondences_; 22 | _referencePoints = referencePoints_; 23 | _currentPoints = currentPoints_; 24 | _referencePointsTransformation = Eigen::Isometry3f::Identity(); 25 | _correspondenceDrawList = glGenLists(1); 26 | updateCorrespondenceDrawList(); 27 | } 28 | 29 | bool DrawableCorrespondences::setParameter(GLParameter *parameter_) { 30 | GLParameterCorrespondences *correspondencesParameter = (GLParameterCorrespondences*)parameter_; 31 | if (correspondencesParameter == 0) { 32 | _parameter = 0; 33 | return false; 34 | } 35 | _parameter = correspondencesParameter; 36 | return true; 37 | } 38 | 39 | void DrawableCorrespondences::draw() { 40 | GLParameterCorrespondences *correspondencesParameter = dynamic_cast(_parameter); 41 | if(correspondencesParameter && 42 | correspondencesParameter->show() && 43 | correspondencesParameter->lineWidth() > 0.0f) { 44 | glPushMatrix(); 45 | correspondencesParameter->applyGLParameter(); 46 | glCallList(_correspondenceDrawList); 47 | glPopMatrix(); 48 | } 49 | } 50 | 51 | void DrawableCorrespondences::updateCorrespondenceDrawList() { 52 | GLParameterCorrespondences *correspondencesParameter = dynamic_cast(_parameter); 53 | glNewList(_correspondenceDrawList, GL_COMPILE); 54 | if(_referencePoints && 55 | _currentPoints && 56 | _correspondences && 57 | correspondencesParameter && 58 | correspondencesParameter->show() && 59 | correspondencesParameter->lineWidth() > 0.0f) { 60 | 61 | glBegin(GL_LINES); 62 | for(int i = 0; i < _numCorrespondences; i += correspondencesParameter->step()) { 63 | const Correspondence &correspondence = _correspondences->at(i); 64 | const Point &referencePoint = _referencePointsTransformation * _referencePoints->at(correspondence.referenceIndex); 65 | const Point ¤tPoint = _transformation * _currentPoints->at(correspondence.currentIndex); 66 | glVertex3f(referencePoint.x(), referencePoint.y(), referencePoint.z()); 67 | glVertex3f(currentPoint.x(), currentPoint.y(), currentPoint.z()); 68 | } 69 | glEnd(); 70 | } 71 | glEndList(); 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_correspondences.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp/correspondencefinder.h" 4 | #include "drawable.h" 5 | #include "gl_parameter_correspondences.h" 6 | 7 | using namespace nicp; 8 | 9 | namespace nicp_viewer { 10 | 11 | class DrawableCorrespondences : public Drawable { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | DrawableCorrespondences(); 16 | DrawableCorrespondences(Eigen::Isometry3f transformation_, GLParameter *parameter_, int numCorrespondences_, PointVector *referencePoints_, 17 | PointVector *currentPoints_, CorrespondenceVector *correspondences_); 18 | virtual ~DrawableCorrespondences() { glDeleteLists(_correspondenceDrawList, 1); } 19 | 20 | virtual GLParameter* parameter() { return _parameter; }; 21 | virtual bool setParameter(GLParameter *parameter_); 22 | 23 | Eigen::Isometry3f referencePointsTransformation() { return _referencePointsTransformation; } 24 | void setReferencePointsTransformation(Eigen::Isometry3f referencePointsTransformation_) { 25 | _referencePointsTransformation = referencePointsTransformation_; 26 | updateCorrespondenceDrawList(); 27 | } 28 | 29 | int numCorrespondances() { return _numCorrespondences; } 30 | void setNumCorrespondences(int numCorrespondences_) { 31 | _numCorrespondences = numCorrespondences_; 32 | updateCorrespondenceDrawList(); 33 | } 34 | 35 | CorrespondenceVector* correspondences() { return _correspondences; } 36 | void setCorrespondences(CorrespondenceVector *correspondences_) { 37 | _correspondences = correspondences_; 38 | updateCorrespondenceDrawList(); 39 | } 40 | 41 | PointVector* referencePoints() { return _referencePoints; } 42 | void setReferencePoints(PointVector *referencePoints_) { 43 | _referencePoints = referencePoints_; 44 | updateCorrespondenceDrawList(); 45 | } 46 | 47 | PointVector* currentPoints() { return _currentPoints; } 48 | void setCurrentPoints(PointVector *currentPoints_) { 49 | _currentPoints = currentPoints_; 50 | updateCorrespondenceDrawList(); 51 | } 52 | 53 | inline GLuint correspondenceDrawList() { return _correspondenceDrawList; } 54 | 55 | void setStep(int step_) { 56 | _parameter->setStep(step_); 57 | updateCorrespondenceDrawList(); 58 | } 59 | void setLineWidth(float lineWidth_) { 60 | _parameter->setLineWidth(lineWidth_); 61 | updateCorrespondenceDrawList(); 62 | } 63 | 64 | virtual void draw(); 65 | void updateCorrespondenceDrawList(); 66 | 67 | protected: 68 | Eigen::Isometry3f _referencePointsTransformation; 69 | GLParameterCorrespondences *_parameter; 70 | int _numCorrespondences; 71 | CorrespondenceVector *_correspondences; 72 | PointVector *_referencePoints; 73 | PointVector *_currentPoints; 74 | GLuint _correspondenceDrawList; 75 | }; 76 | 77 | } 78 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_covariances.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_covariances.h" 2 | #include "gl_parameter_covariances.h" 3 | #include "nicp_qglviewer.h" 4 | #include "opengl_primitives.h" 5 | 6 | namespace nicp_viewer { 7 | 8 | DrawableCovariances::DrawableCovariances() : Drawable() { 9 | _parameter = 0; 10 | _covariances = 0; 11 | _viewer = 0; 12 | _covarianceDrawList = glGenLists(1); 13 | _sphereDrawList = glGenLists(1); 14 | glNewList(_sphereDrawList, GL_COMPILE); 15 | drawSphere(1.0f); 16 | glEndList(); 17 | updateCovarianceDrawList(); 18 | } 19 | 20 | DrawableCovariances::DrawableCovariances(Eigen::Isometry3f transformation_, GLParameter *parameter_, StatsVector *covariances_) : Drawable(transformation_) { 21 | setParameter(parameter_); 22 | _covariances = covariances_; 23 | _covarianceDrawList = glGenLists(1); 24 | _sphereDrawList = glGenLists(1); 25 | glNewList(_sphereDrawList, GL_COMPILE); 26 | drawSphere(1.0f); 27 | glEndList(); 28 | updateCovarianceDrawList(); 29 | } 30 | 31 | bool DrawableCovariances::setParameter(GLParameter *parameter_) { 32 | GLParameterCovariances *covariancesParameter = (GLParameterCovariances*)parameter_; 33 | if (covariancesParameter == 0) { 34 | _parameter = 0; 35 | return false; 36 | } 37 | _parameter = covariancesParameter; 38 | return true; 39 | } 40 | 41 | void DrawableCovariances::draw() { 42 | GLParameterCovariances *covariancesParameter = dynamic_cast(_parameter); 43 | if(_covarianceDrawList && 44 | covariancesParameter && 45 | covariancesParameter->show() && 46 | covariancesParameter->ellipsoidScale() > 0.0f) { 47 | glPushMatrix(); 48 | glMultMatrixf(_transformation.data()); 49 | glCallList(_covarianceDrawList); 50 | glPopMatrix(); 51 | } 52 | } 53 | 54 | void DrawableCovariances::updateCovarianceDrawList() { 55 | GLParameterCovariances *covariancesParameter = dynamic_cast(_parameter); 56 | glNewList(_covarianceDrawList, GL_COMPILE); 57 | if(_covarianceDrawList && 58 | _covariances && 59 | covariancesParameter && 60 | covariancesParameter->ellipsoidScale() > 0.0f) { 61 | covariancesParameter->applyGLParameter(); 62 | float ellipsoidScale = covariancesParameter->ellipsoidScale(); 63 | // Find max curvature 64 | float maxCurvature = 0.0f; 65 | for(size_t i = 0; i < _covariances->size(); i += covariancesParameter->step()) { 66 | if(_covariances->at(i).curvature() > maxCurvature) 67 | maxCurvature = _covariances->at(i).curvature(); 68 | } 69 | maxCurvature = maxCurvature / 2.0f ; 70 | for(size_t i = 0; i < _covariances->size(); i += covariancesParameter->step()) { 71 | Stats cov = _covariances->at(i); 72 | Eigen::Vector3f lambda = cov.eigenValues(); 73 | Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); 74 | I.linear() = cov.eigenVectors(); 75 | if(cov.n() == 0) 76 | continue; 77 | I.translation() = Eigen::Vector3f(cov.mean()[0], cov.mean()[1], cov.mean()[2]); 78 | float sx = sqrt(lambda[0]) * ellipsoidScale; 79 | float sy = sqrt(lambda[1]) * ellipsoidScale; 80 | float sz = sqrt(lambda[2]) * ellipsoidScale; 81 | float curvature = cov.curvature(); 82 | glPushMatrix(); 83 | glMultMatrixf(I.data()); 84 | //std::cerr << "Curvature: " << curvature << std::endl; 85 | glColor4f(curvature/maxCurvature, 1.0f - curvature/maxCurvature, 0.0f, 1.0f); 86 | sx = 1e-03; 87 | sy = ellipsoidScale; 88 | sz = ellipsoidScale; 89 | glScalef(sx, sy, sz); 90 | glCallList(_sphereDrawList); 91 | glPopMatrix(); 92 | } 93 | } 94 | glEndList(); 95 | } 96 | 97 | } 98 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_covariances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp/stats.h" 4 | #include "gl_parameter_covariances.h" 5 | #include "drawable.h" 6 | 7 | using namespace nicp; 8 | 9 | namespace nicp_viewer { 10 | 11 | class DrawableCovariances : public Drawable { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | DrawableCovariances(); 16 | DrawableCovariances(Eigen::Isometry3f transformation_, GLParameter *parameter_, StatsVector *covariances_); 17 | virtual ~DrawableCovariances() { 18 | glDeleteLists(_covarianceDrawList, 1); 19 | glDeleteLists(_sphereDrawList, 1); 20 | } 21 | 22 | virtual GLParameter* parameter() { return _parameter; } 23 | virtual bool setParameter(GLParameter *parameter_); 24 | 25 | virtual StatsVector* covariances() { return _covariances; } 26 | virtual void setCovariances(StatsVector *covariances_) { 27 | _covariances = covariances_; 28 | updateCovarianceDrawList(); 29 | } 30 | 31 | inline GLuint covarianceDrawList() { return _covarianceDrawList; } 32 | inline GLuint sphereDrawList() { return _sphereDrawList; } 33 | 34 | void setStep(int step_) { 35 | _parameter->setStep(step_); 36 | updateCovarianceDrawList(); 37 | } 38 | void setEllipsoidScale(float ellipsoidScale_) { 39 | _parameter->setEllipsoidScale(ellipsoidScale_); 40 | updateCovarianceDrawList(); 41 | } 42 | 43 | virtual void draw(); 44 | void updateCovarianceDrawList(); 45 | 46 | protected: 47 | GLParameterCovariances *_parameter; 48 | StatsVector *_covariances; 49 | GLuint _covarianceDrawList; 50 | GLuint _sphereDrawList; 51 | }; 52 | 53 | } 54 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_frame.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp_qglviewer.h" 4 | #include "imageview.h" 5 | #include "drawable_points.h" 6 | #include "drawable_normals.h" 7 | #include "drawable_covariances.h" 8 | #include "drawable_correspondences.h" 9 | #include "gl_parameter.h" 10 | #include "gl_parameter_points.h" 11 | #include "gl_parameter_normals.h" 12 | #include "gl_parameter_covariances.h" 13 | #include "gl_parameter_correspondences.h" 14 | #include "gl_parameter_cloud.h" 15 | 16 | namespace nicp_viewer { 17 | 18 | class DrawableCloud : public Drawable { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 21 | 22 | DrawableCloud(const Eigen::Isometry3f &transformation_ = Eigen::Isometry3f::Identity(), 23 | GLParameter *parameter_ = 0, Cloud *cloud_ = 0); 24 | virtual ~DrawableCloud() { clearDrawableObjects(); } 25 | 26 | const Eigen::Isometry3f& localTransformation() const { return _localTransform; } 27 | void setLocalTransformation(const Eigen::Isometry3f &localTransform_) { _localTransform = localTransform_; } 28 | 29 | virtual GLParameter* parameter() { return _parameter; } 30 | virtual bool setParameter(GLParameter *parameter_); 31 | 32 | Cloud* cloud() const { return _cloud; } 33 | void setCloud(Cloud *cloud_); 34 | 35 | DrawableCorrespondences* drawableCorrespondences() { return _drawableCorrespondences; } 36 | void setDrawableCorrespondences(DrawableCorrespondences* drawableCorrespondences_) { 37 | if(_drawableCorrespondences) 38 | delete _drawableCorrespondences; 39 | _drawableCorrespondences = drawableCorrespondences_; 40 | } 41 | 42 | DrawablePoints* drawablePoints() { return _drawablePoints; } 43 | DrawableNormals* drawableNormals() { return _drawableNormals; } 44 | DrawableCovariances* drawableCovariances() { return _drawableCovariances; } 45 | 46 | void clearDrawableObjects(); 47 | void constructDrawableObjects(); 48 | 49 | void draw(); 50 | 51 | protected: 52 | Eigen::Isometry3f _localTransform; 53 | Cloud *_cloud; 54 | DrawableCloud* _previousDrawableCloud; 55 | GLParameterCloud * _parameter; 56 | DrawablePoints *_drawablePoints; 57 | DrawableNormals *_drawableNormals; 58 | DrawableCovariances *_drawableCovariances; 59 | DrawableCorrespondences *_drawableCorrespondences; 60 | }; 61 | 62 | } 63 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_normals.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_normals.h" 2 | #include "gl_parameter_normals.h" 3 | 4 | namespace nicp_viewer { 5 | 6 | DrawableNormals::DrawableNormals() : Drawable() { 7 | _parameter = 0; 8 | _points = 0; 9 | _normals = 0; 10 | _normalDrawList = glGenLists(1); 11 | updateNormalDrawList(); 12 | } 13 | 14 | DrawableNormals::DrawableNormals(const Eigen::Isometry3f &transformation_, GLParameter *parameter_, PointVector *points_, 15 | NormalVector *normals_) : Drawable(transformation_) { 16 | setParameter(parameter_); 17 | _points = points_; 18 | _normals = normals_; 19 | _normalDrawList = glGenLists(1); 20 | updateNormalDrawList(); 21 | } 22 | 23 | bool DrawableNormals::setParameter(GLParameter *parameter_) { 24 | GLParameterNormals *normalsParameter = (GLParameterNormals*)parameter_; 25 | if (normalsParameter == 0) { 26 | _parameter = 0; 27 | return false; 28 | } 29 | _parameter = normalsParameter; 30 | return true; 31 | } 32 | 33 | void DrawableNormals::draw() { 34 | GLParameterNormals *normalsParameter = dynamic_cast(_parameter); 35 | if(_normalDrawList && 36 | normalsParameter && 37 | normalsParameter->show() && 38 | normalsParameter->normalLength() > 0.0f) { 39 | glPushMatrix(); 40 | glMultMatrixf(_transformation.data()); 41 | glCallList(_normalDrawList); 42 | glPopMatrix(); 43 | } 44 | } 45 | 46 | void DrawableNormals::updateNormalDrawList() { 47 | GLParameterNormals *normalsParameter = dynamic_cast(_parameter); 48 | glNewList(_normalDrawList, GL_COMPILE); 49 | if(_normalDrawList && 50 | _points && 51 | _normals && 52 | normalsParameter && 53 | normalsParameter->normalLength() > 0.0f) { 54 | normalsParameter->applyGLParameter(); 55 | float normalLength = normalsParameter->normalLength(); 56 | glBegin(GL_LINES); 57 | for(size_t i = 0; i < _normals->size(); i += normalsParameter->step()) { 58 | const Point &p = _points->at(i); 59 | const Normal &n = _normals->at(i); 60 | glVertex3f(p[0], p[1], p[2]); 61 | glVertex3f(p[0] + n[0]*normalLength, 62 | p[1] + n[1]*normalLength, 63 | p[2] + n[2]*normalLength); 64 | } 65 | glEnd(); 66 | } 67 | glEndList(); 68 | } 69 | 70 | } 71 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_normals.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp/homogeneousvector4f.h" 4 | #include "gl_parameter_normals.h" 5 | #include "drawable.h" 6 | 7 | using namespace nicp; 8 | 9 | namespace nicp_viewer { 10 | 11 | class DrawableNormals : public Drawable { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | DrawableNormals(); 16 | DrawableNormals(const Eigen::Isometry3f &transformation_, GLParameter *parameter_, PointVector *points_, NormalVector *normals_); 17 | virtual ~DrawableNormals() { glDeleteLists(_normalDrawList, 1); } 18 | 19 | virtual GLParameter* parameter() { return _parameter; }; 20 | virtual bool setParameter(GLParameter *parameter_); 21 | 22 | virtual PointVector* points() { return _points; } 23 | virtual void setPoints(PointVector *points_) { 24 | _points = points_; 25 | updateNormalDrawList(); 26 | } 27 | 28 | virtual NormalVector* normals() { return _normals; } 29 | virtual void setNormals(NormalVector *normals_) { 30 | _normals = normals_; 31 | updateNormalDrawList(); 32 | } 33 | 34 | void setStep(int step_) { 35 | _parameter->setStep(step_); 36 | updateNormalDrawList(); 37 | } 38 | void setNormalLength(float normalLength_) { 39 | _parameter->setNormalLength(normalLength_); 40 | updateNormalDrawList(); 41 | } 42 | void setNormalWidth(float lineWidth_) { 43 | _parameter->setLineWidth(lineWidth_); 44 | updateNormalDrawList(); 45 | } 46 | 47 | inline GLuint normalDrawList() { return _normalDrawList; } 48 | 49 | virtual void draw(); 50 | void updateNormalDrawList(); 51 | 52 | protected: 53 | GLParameterNormals *_parameter; 54 | PointVector *_points; 55 | NormalVector *_normals; 56 | GLuint _normalDrawList; 57 | }; 58 | 59 | } 60 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_points.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_points.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | DrawablePoints::DrawablePoints() : Drawable() { 6 | _parameter = 0; 7 | _points = 0; 8 | _normals = 0; 9 | _rgbs = 0; 10 | _pointDrawList = 0; 11 | _pointDrawList = glGenLists(1); 12 | updatePointDrawList(); 13 | } 14 | 15 | DrawablePoints::DrawablePoints(const Eigen::Isometry3f& transformation_, GLParameter *parameter_, PointVector *points_, 16 | NormalVector *normals_, RGBVector *rgbs_) : Drawable(transformation_) { 17 | setParameter(parameter_); 18 | _points = points_; 19 | _normals = normals_; 20 | _rgbs = rgbs_; 21 | _pointDrawList = 0; 22 | _pointDrawList = glGenLists(1); 23 | updatePointDrawList(); 24 | } 25 | 26 | bool DrawablePoints::setParameter(GLParameter *parameter_) { 27 | GLParameterPoints *pointsParameter = (GLParameterPoints*)parameter_; 28 | if(pointsParameter == 0) { 29 | _parameter = 0; 30 | return false; 31 | } 32 | _parameter = pointsParameter; 33 | return true; 34 | } 35 | 36 | void DrawablePoints::draw() { 37 | GLParameterPoints *pointsParameter = dynamic_cast(_parameter); 38 | if(_pointDrawList && 39 | pointsParameter && 40 | pointsParameter->show() && 41 | pointsParameter->pointSize() > 0.0f) { 42 | glPushMatrix(); 43 | glMultMatrixf(_transformation.data()); 44 | glCallList(_pointDrawList); 45 | glPopMatrix(); 46 | } 47 | } 48 | 49 | void DrawablePoints::updatePointDrawList() { 50 | GLParameterPoints *pointsParameter = dynamic_cast(_parameter); 51 | glNewList(_pointDrawList, GL_COMPILE); 52 | float scale = 1./255; 53 | if(_pointDrawList && 54 | _points && 55 | _normals && 56 | pointsParameter && 57 | pointsParameter->pointSize() > 0.0f) { 58 | pointsParameter->applyGLParameter(); 59 | glBegin(GL_POINTS); 60 | for(size_t i = 0; i < _points->size(); i += pointsParameter->step()) { 61 | const Point &p = _points->at(i); 62 | const Normal &n = _normals->at(i); 63 | if(_rgbs && _rgbs->size() > 0) { 64 | cv::Vec3b c=_rgbs->at(i); 65 | glColor3f(scale*c[0], scale*c[1], scale*c[2]); 66 | } 67 | // glColor4f(fabs(n[0]), fabs(n[1]), fabs(n[2]), 1.0f); 68 | glNormal3f(n[0], n[1], n[2]); 69 | glVertex3f(p[0], p[1], p[2]); 70 | } 71 | glEnd(); 72 | } 73 | glEndList(); 74 | } 75 | 76 | } 77 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_points.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp/homogeneousvector4f.h" 4 | #include "nicp/definitions.h" 5 | #include "drawable.h" 6 | #include "gl_parameter_points.h" 7 | 8 | using namespace nicp; 9 | 10 | namespace nicp_viewer { 11 | 12 | class DrawablePoints : public Drawable { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 15 | 16 | DrawablePoints(); 17 | DrawablePoints(const Eigen::Isometry3f& transformation_, GLParameter *parameter_, PointVector *points_, NormalVector *normals_, RGBVector* rgbs_=0); 18 | virtual ~DrawablePoints() { glDeleteLists(_pointDrawList, 1); } 19 | 20 | virtual GLParameter* parameter() { return _parameter; }; 21 | virtual PointVector* points() { return _points; } 22 | virtual RGBVector* rgbs() { return _rgbs; } 23 | virtual NormalVector* normals() { return _normals; } 24 | inline GLuint pointDrawList() { return _pointDrawList; } 25 | 26 | virtual bool setParameter(GLParameter *parameter_); 27 | virtual void setPoints(PointVector *points_) { 28 | _points = points_; 29 | updatePointDrawList(); 30 | } 31 | virtual void setNormals(NormalVector *normals_) { 32 | _normals = normals_; 33 | updatePointDrawList(); 34 | } 35 | 36 | virtual void setRGBs(RGBVector *rgbs_) { 37 | _rgbs = rgbs_; 38 | updatePointDrawList(); 39 | } 40 | 41 | virtual void setPointsAndNormals(PointVector *points_, NormalVector *normals_) { 42 | _points = points_; 43 | _normals = normals_; 44 | updatePointDrawList(); 45 | } 46 | 47 | void setStep(int step_) { 48 | _parameter->setStep(step_); 49 | updatePointDrawList(); 50 | } 51 | void setPointSize(float pointSize_) { 52 | _parameter->setPointSize(pointSize_); 53 | updatePointDrawList(); 54 | } 55 | 56 | virtual void draw(); 57 | virtual void updatePointDrawList(); 58 | 59 | protected: 60 | GLParameterPoints *_parameter; 61 | PointVector *_points; 62 | NormalVector *_normals; 63 | RGBVector *_rgbs; 64 | GLuint _pointDrawList; 65 | }; 66 | 67 | } 68 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_trajectory.h" 2 | #include "opengl_primitives.h" 3 | 4 | namespace nicp_viewer { 5 | 6 | DrawableTrajectory::DrawableTrajectory() : Drawable() { 7 | _parameter = 0; 8 | _trajectory = 0; 9 | _pyramidDrawList = glGenLists(1); 10 | _trajectoryDrawList = glGenLists(1); 11 | glNewList(_pyramidDrawList, GL_COMPILE); 12 | drawPyramid(1.0f, 1.0f); 13 | glEndList(); 14 | _trajectorySize = 0; 15 | updateTrajectoryDrawList(); 16 | } 17 | 18 | DrawableTrajectory::DrawableTrajectory(const Eigen::Isometry3f &transformation_, GLParameter *parameter_, 19 | std::vector* trajectory_, 20 | std::vector >* trajectoryColors_) : Drawable(transformation_) { 21 | setParameter(parameter_); 22 | _trajectory = trajectory_; 23 | _trajectoryColors = trajectoryColors_; 24 | _pyramidDrawList = glGenLists(1); 25 | _trajectoryDrawList = glGenLists(1); 26 | glNewList(_pyramidDrawList, GL_COMPILE); 27 | drawPyramid(1.0f, 1.0f); 28 | glEndList(); 29 | if(_trajectory != 0) 30 | _trajectorySize = _trajectory->size(); 31 | else 32 | _trajectorySize = 0; 33 | updateTrajectoryDrawList(); 34 | } 35 | 36 | bool DrawableTrajectory::setParameter(GLParameter *parameter_) { 37 | GLParameterTrajectory *trajectoryParameter = (GLParameterTrajectory*)parameter_; 38 | if(trajectoryParameter == 0) { 39 | _parameter = 0; 40 | return false; 41 | } 42 | _parameter = trajectoryParameter; 43 | return true; 44 | } 45 | 46 | void DrawableTrajectory::draw() { 47 | GLParameterTrajectory *trajectoryParameter = dynamic_cast(_parameter); 48 | if(_trajectory && 49 | trajectoryParameter && 50 | trajectoryParameter->show() && 51 | trajectoryParameter->pyramidScale() > 0.0f) { 52 | if(_trajectorySize != _trajectory->size()) 53 | updateTrajectoryDrawList(); 54 | glPushMatrix(); 55 | glMultMatrixf(_transformation.data()); 56 | trajectoryParameter->applyGLParameter(); 57 | glCallList(_trajectoryDrawList); 58 | glPopMatrix(); 59 | } 60 | } 61 | 62 | void DrawableTrajectory::updateTrajectoryDrawList() { 63 | GLParameterTrajectory *trajectoryParameter = dynamic_cast(_parameter); 64 | glNewList(_trajectoryDrawList, GL_COMPILE); 65 | if(_trajectory && 66 | trajectoryParameter && 67 | trajectoryParameter->show() && 68 | trajectoryParameter->pyramidScale() > 0.0f) { 69 | for(size_t i = 0; i < _trajectory->size(); i += trajectoryParameter->step()) { 70 | if (i>0) { 71 | Eigen::Vector3f p1=_trajectory->at(i).translation(); 72 | Eigen::Vector3f p2=_trajectory->at(i-1).translation(); 73 | 74 | glBegin(GL_LINES); 75 | glVertex3f(p1.x(), p1.y(), p1.z()); 76 | glVertex3f(p2.x(), p2.y(), p2.z()); 77 | glEnd(); 78 | } 79 | glPushMatrix(); 80 | glMultMatrixf(_trajectory->at(i).data()); 81 | glRotatef(90.0f, 0.0f, 1.0f, 0.0f); 82 | glScalef(trajectoryParameter->pyramidScale() ,trajectoryParameter->pyramidScale(), trajectoryParameter->pyramidScale() * 2.0f); 83 | glColor4f(_trajectoryColors->at(i).x(),_trajectoryColors->at(i).y(), _trajectoryColors->at(i).z(), _trajectoryColors->at(i)[3]); 84 | glCallList(_pyramidDrawList); 85 | glPopMatrix(); 86 | } 87 | } 88 | glEndList(); 89 | } 90 | 91 | } 92 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "drawable.h" 7 | #include "gl_parameter_trajectory.h" 8 | 9 | namespace nicp_viewer { 10 | 11 | class DrawableTrajectory : public Drawable { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | DrawableTrajectory(); 16 | DrawableTrajectory(const Eigen::Isometry3f &transformation_, GLParameter *parameter_, 17 | std::vector *trajectory_, std::vector >* trajectoryColors_); 18 | virtual ~DrawableTrajectory() { glDeleteLists(_trajectoryDrawList, 1); } 19 | 20 | virtual GLParameter* parameter() { return _parameter; }; 21 | virtual bool setParameter(GLParameter *parameter_); 22 | 23 | std::vector* trajectory() { return _trajectory; } 24 | void setTrajectory(std::vector *trajectory_) { 25 | _trajectory = trajectory_; 26 | updateTrajectoryDrawList(); 27 | } 28 | 29 | std::vector >* trajectoryColors() { return _trajectoryColors; } 30 | void setTrajectoryColors(std::vector >* trajectoryColors_) { 31 | _trajectoryColors = trajectoryColors_; 32 | updateTrajectoryDrawList(); 33 | } 34 | 35 | inline GLuint pyramidDrawList() { return _pyramidDrawList; } 36 | inline GLuint trajectoryDrawList() { return _trajectoryDrawList; } 37 | 38 | void setStep(int step_) { 39 | _parameter->setStep(step_); 40 | updateTrajectoryDrawList(); 41 | } 42 | void setPyramidScale(float pyramidScale_) { 43 | _parameter->setPyramidScale(pyramidScale_); 44 | updateTrajectoryDrawList(); 45 | } 46 | 47 | virtual void draw(); 48 | void updateTrajectoryDrawList(); 49 | 50 | protected: 51 | GLParameterTrajectory *_parameter; 52 | 53 | std::vector >* _trajectoryColors; 54 | std::vector* _trajectory; 55 | GLuint _pyramidDrawList; 56 | GLuint _trajectoryDrawList; 57 | size_t _trajectorySize; 58 | }; 59 | 60 | } 61 | 62 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_transform_covariance.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "drawable_transform_covariance.h" 4 | #include "gl_parameter_covariances.h" 5 | #include "nicp_qglviewer.h" 6 | #include "opengl_primitives.h" 7 | 8 | namespace nicp_viewer { 9 | 10 | DrawableTransformCovariance::DrawableTransformCovariance() : Drawable() { 11 | _parameter = 0; 12 | _covariance.setZero(); 13 | _mean.setZero(); 14 | _viewer = 0; 15 | _covarianceDrawList = glGenLists(1); 16 | _sphereDrawList = glGenLists(1); 17 | glNewList(_sphereDrawList, GL_COMPILE); 18 | drawSphere(1.0f); 19 | glEndList(); 20 | updateCovarianceDrawList(); 21 | } 22 | 23 | DrawableTransformCovariance::DrawableTransformCovariance(Eigen::Isometry3f transformation_, GLParameter *parameter_, Eigen::Matrix3f covariance_, Eigen::Vector3f mean_) : Drawable(transformation_) { 24 | setParameter(parameter_); 25 | _covariance = covariance_; 26 | _mean = mean_; 27 | _covarianceDrawList = glGenLists(1); 28 | _sphereDrawList = glGenLists(1); 29 | glNewList(_sphereDrawList, GL_COMPILE); 30 | drawSphere(1.0f); 31 | glEndList(); 32 | updateCovarianceDrawList(); 33 | } 34 | 35 | bool DrawableTransformCovariance::setParameter(GLParameter *parameter_) { 36 | GLParameterTransformCovariance *covarianceParameter = dynamic_cast(parameter_); 37 | if (covarianceParameter == 0) { 38 | _parameter = 0; 39 | return false; 40 | } 41 | _parameter = covarianceParameter; 42 | updateCovarianceDrawList(); 43 | return true; 44 | } 45 | 46 | void DrawableTransformCovariance::draw() { 47 | GLParameterTransformCovariance *covarianceParameter = dynamic_cast(_parameter); 48 | if(covarianceParameter && 49 | covarianceParameter->show() && 50 | covarianceParameter->scale() > 0.0f) { 51 | glPushMatrix(); 52 | glMultMatrixf(_transformation.data()); 53 | covarianceParameter->applyGLParameter(); 54 | glCallList(_covarianceDrawList); 55 | glPopMatrix(); 56 | } 57 | } 58 | 59 | void DrawableTransformCovariance::updateCovarianceDrawList() { 60 | GLParameterTransformCovariance *covarianceParameter = dynamic_cast(_parameter); 61 | glNewList(_covarianceDrawList, GL_COMPILE); 62 | if(_covariance != Eigen::Matrix3f::Zero() && 63 | covarianceParameter && 64 | covarianceParameter->show() && 65 | covarianceParameter->scale() > 0.0f) { 66 | float scale = covarianceParameter->scale(); 67 | Eigen::Vector4f color = covarianceParameter->color(); 68 | 69 | Eigen::SelfAdjointEigenSolver eigenSolver; 70 | eigenSolver.computeDirect(_covariance, Eigen::ComputeEigenvectors); 71 | 72 | Eigen::Vector3f lambda = eigenSolver.eigenvalues(); 73 | Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); 74 | I.linear() = eigenSolver.eigenvectors(); 75 | I.translation() = Eigen::Vector3f(_mean.x(), _mean.y(), _mean.z()); 76 | 77 | float sx = sqrt(lambda[0]) * scale; 78 | float sy = sqrt(lambda[1]) * scale; 79 | float sz = sqrt(lambda[2]) * scale; 80 | 81 | glPushMatrix(); 82 | glMultMatrixf(I.data()); 83 | glColor4f(color[0], color[1], color[2], color[3]); 84 | glScalef(sx, sy, sz); 85 | glCallList(_sphereDrawList); 86 | glPopMatrix(); 87 | } 88 | glEndList(); 89 | } 90 | 91 | } 92 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_transform_covariance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "gl_parameter_transform_covariance.h" 4 | #include "drawable.h" 5 | 6 | namespace nicp_viewer { 7 | 8 | class DrawableTransformCovariance : public Drawable { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 11 | 12 | DrawableTransformCovariance(); 13 | DrawableTransformCovariance(Eigen::Isometry3f transformation_, GLParameter *parameter_, Eigen::Matrix3f covariance_, Eigen::Vector3f mean_); 14 | virtual ~DrawableTransformCovariance() { 15 | glDeleteLists(_covarianceDrawList, 1); 16 | glDeleteLists(_sphereDrawList, 1); 17 | } 18 | 19 | virtual GLParameter* parameter() { return _parameter; } 20 | virtual bool setParameter(GLParameter *parameter_); 21 | 22 | virtual Eigen::Matrix3f covariance() { return _covariance; } 23 | virtual void setCovariances(Eigen::Matrix3f covariance_) { 24 | _covariance = covariance_; 25 | updateCovarianceDrawList(); 26 | } 27 | 28 | virtual Eigen::Vector3f mean() { return _mean; } 29 | virtual void setMean(Eigen::Vector3f mean_) { 30 | _mean = mean_; 31 | updateCovarianceDrawList(); 32 | } 33 | 34 | inline GLuint covarianceDrawList() { return _covarianceDrawList; } 35 | inline GLuint sphereDrawList() { return _sphereDrawList; } 36 | 37 | virtual void draw(); 38 | void updateCovarianceDrawList(); 39 | 40 | protected: 41 | GLParameterTransformCovariance *_parameter; 42 | Eigen::Matrix3f _covariance; 43 | Eigen::Vector3f _mean; 44 | GLuint _covarianceDrawList; 45 | GLuint _sphereDrawList; 46 | }; 47 | 48 | } 49 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_uncertainty.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_uncertainty.h" 2 | #include "nicp_qglviewer.h" 3 | #include "opengl_primitives.h" 4 | 5 | #include 6 | 7 | namespace nicp_viewer { 8 | 9 | DrawableUncertainty::DrawableUncertainty() : Drawable() { 10 | _parameter = 0; 11 | _covariances = 0; 12 | _covarianceDrawList = glGenLists(1); 13 | _sphereDrawList = glGenLists(1); 14 | glNewList(_sphereDrawList, GL_COMPILE); 15 | drawSphere(1.0f); 16 | glEndList(); 17 | updateCovarianceDrawList(); 18 | } 19 | 20 | DrawableUncertainty::DrawableUncertainty(Eigen::Isometry3f transformation_, GLParameter *parameter_, Gaussian3fVector *covariances_) : Drawable(transformation_) { 21 | setParameter(parameter_); 22 | _covariances = covariances_; 23 | _covarianceDrawList = glGenLists(1); 24 | _sphereDrawList = glGenLists(1); 25 | glNewList(_sphereDrawList, GL_COMPILE); 26 | drawSphere(1.0f); 27 | glEndList(); 28 | updateCovarianceDrawList(); 29 | } 30 | 31 | bool DrawableUncertainty::setParameter(GLParameter *parameter_) { 32 | GLParameterUncertainty *uncertaintyParameter = (GLParameterUncertainty*)parameter_; 33 | if(uncertaintyParameter == 0) { 34 | _parameter = 0; 35 | return false; 36 | } 37 | _parameter = uncertaintyParameter; 38 | return true; 39 | } 40 | 41 | void DrawableUncertainty::draw() { 42 | GLParameterUncertainty *uncertaintyParameter = dynamic_cast(_parameter); 43 | if(_covarianceDrawList && 44 | uncertaintyParameter && 45 | uncertaintyParameter->show() && 46 | uncertaintyParameter->ellipsoidScale() > 0.0f) { 47 | glPushMatrix(); 48 | glMultMatrixf(_transformation.data()); 49 | glCallList(_covarianceDrawList); 50 | glPopMatrix(); 51 | } 52 | } 53 | 54 | void DrawableUncertainty::updateCovarianceDrawList() { 55 | GLParameterUncertainty *uncertaintyParameter = dynamic_cast(_parameter); 56 | glNewList(_covarianceDrawList, GL_COMPILE); 57 | if(_covarianceDrawList && 58 | _covariances && 59 | uncertaintyParameter && 60 | uncertaintyParameter->ellipsoidScale() > 0.0f) { 61 | uncertaintyParameter->applyGLParameter(); 62 | Eigen::SelfAdjointEigenSolver eigenSolver; 63 | float ellipsoidScale = uncertaintyParameter->ellipsoidScale(); 64 | for(size_t i = 0; i < _covariances->size(); i += uncertaintyParameter->step()) { 65 | Gaussian3f &gaussian3f = _covariances->at(i); 66 | Eigen::Matrix3f covariance = gaussian3f.covarianceMatrix(); 67 | Eigen::Vector3f mean = gaussian3f.mean(); 68 | eigenSolver.computeDirect(covariance, Eigen::ComputeEigenvectors); 69 | Eigen::Vector3f eigenValues = eigenSolver.eigenvalues(); 70 | Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); 71 | I.linear() = eigenSolver.eigenvectors(); 72 | I.translation() = mean; 73 | float sx = sqrt(eigenValues[0]) * ellipsoidScale; 74 | float sy = sqrt(eigenValues[1]) * ellipsoidScale; 75 | float sz = sqrt(eigenValues[2]) * ellipsoidScale; 76 | glPushMatrix(); 77 | glMultMatrixf(I.data()); 78 | sx = sx; 79 | sy = sy; 80 | sz = sz; 81 | glScalef(sx, sy, sz); 82 | glCallList(_sphereDrawList); 83 | glPopMatrix(); 84 | } 85 | } 86 | glEndList(); 87 | } 88 | 89 | } 90 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/drawable_uncertainty.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nicp/gaussian3.h" 4 | #include "gl_parameter_uncertainty.h" 5 | #include "drawable.h" 6 | 7 | using namespace nicp; 8 | 9 | namespace nicp_viewer { 10 | 11 | class DrawableUncertainty : public Drawable { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | DrawableUncertainty(); 16 | DrawableUncertainty(Eigen::Isometry3f transformation_, GLParameter *parameter_, Gaussian3fVector *covariances_); 17 | virtual ~DrawableUncertainty() { 18 | glDeleteLists(_covarianceDrawList, 1); 19 | glDeleteLists(_sphereDrawList, 1); 20 | } 21 | 22 | virtual GLParameter* parameter() { return _parameter; } 23 | virtual bool setParameter(GLParameter *parameter_); 24 | 25 | virtual Gaussian3fVector* covariances() { return _covariances; } 26 | virtual void setCovariances(Gaussian3fVector *covariances_) { 27 | _covariances = covariances_; 28 | updateCovarianceDrawList(); 29 | } 30 | 31 | inline GLuint covarianceDrawList() { return _covarianceDrawList; } 32 | inline GLuint sphereDrawList() { return _sphereDrawList; } 33 | 34 | void setStep(int step_) { 35 | _parameter->setStep(step_); 36 | updateCovarianceDrawList(); 37 | } 38 | void setEllipsoidScale(float ellipsoidScale_) { 39 | _parameter->setEllipsoidScale(ellipsoidScale_); 40 | updateCovarianceDrawList(); 41 | } 42 | 43 | virtual void draw(); 44 | void updateCovarianceDrawList(); 45 | 46 | protected: 47 | GLParameterUncertainty *_parameter; 48 | Gaussian3fVector *_covariances; 49 | GLuint _covarianceDrawList; 50 | GLuint _sphereDrawList; 51 | }; 52 | 53 | } 54 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameter::GLParameter() { 6 | _step = 1; 7 | _show = true; 8 | } 9 | 10 | } 11 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace nicp_viewer { 7 | 8 | class GLParameter : public QObject { 9 | Q_OBJECT; 10 | 11 | public: 12 | GLParameter(); 13 | virtual ~GLParameter() {} 14 | 15 | virtual void applyGLParameter() = 0; 16 | 17 | inline int step() { return _step; } 18 | inline void setStep(int step_) { _step = step_; } 19 | 20 | inline bool show() const { return _show; } 21 | inline void setShow(bool show_) { _show = show_; } 22 | 23 | protected: 24 | int _step; 25 | bool _show; 26 | }; 27 | 28 | } 29 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_cloud.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterCloud::GLParameterCloud(int step) : GLParameter() { 6 | _parameterPoints = new GLParameterPoints(1.0f, Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f)); 7 | _parameterNormals = new GLParameterNormals(1.0f, Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f), 0.0f); 8 | _parameterCovariances = new GLParameterCovariances(1.0f, 9 | Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f), Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f), 10 | 0.02f, 0.0f); 11 | _parameterCorrespondences = new GLParameterCorrespondences(1.0f, Eigen::Vector4f(1.0f, 0.0f, 1.0f, 1.0f), 0.0f); 12 | 13 | _parameterPoints->setStep(step); 14 | _parameterNormals->setStep(step); 15 | _parameterCovariances->setStep(step); 16 | _parameterCorrespondences->setStep(step); 17 | } 18 | 19 | GLParameterCloud::~GLParameterCloud() { 20 | delete _parameterPoints; 21 | delete _parameterNormals; 22 | delete _parameterCovariances; 23 | delete _parameterCorrespondences; 24 | } 25 | 26 | void GLParameterCloud::setStep(int step_) { 27 | _step = step_; 28 | _parameterPoints->setStep(step_); 29 | _parameterNormals->setStep(step_); 30 | _parameterCovariances->setStep(step_); 31 | _parameterCorrespondences->setStep(step_); 32 | } 33 | 34 | void GLParameterCloud::setPointSize(float pointSize) { 35 | _parameterPoints->setPointSize(pointSize); 36 | _parameterNormals->setPointSize(pointSize); 37 | _parameterCovariances->setPointSize(pointSize); 38 | _parameterCorrespondences->setPointSize(pointSize); 39 | } 40 | 41 | void GLParameterCloud::setShow(bool show_) { 42 | _show = show_; 43 | _parameterPoints->setShow(show_); 44 | _parameterNormals->setShow(show_); 45 | _parameterCovariances->setShow(show_); 46 | _parameterCorrespondences->setShow(show_); 47 | } 48 | 49 | } 50 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_cloud.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "gl_parameter.h" 4 | #include "gl_parameter_points.h" 5 | #include "gl_parameter_normals.h" 6 | #include "gl_parameter_covariances.h" 7 | #include "gl_parameter_correspondences.h" 8 | 9 | namespace nicp_viewer { 10 | 11 | class GLParameterCloud : public GLParameter { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | GLParameterCloud(int step = 1); 16 | virtual ~GLParameterCloud(); 17 | 18 | virtual void applyGLParameter() {} 19 | 20 | virtual void setStep(int step_); 21 | virtual void setPointSize(float pointSize); 22 | virtual void setShow(bool show_); 23 | 24 | GLParameterPoints* parameterPoints() { return _parameterPoints; } 25 | void setParameterPoints(GLParameterPoints *parameterPoints_) { _parameterPoints = parameterPoints_; } 26 | 27 | GLParameterNormals* parameterNormals() { return _parameterNormals; } 28 | void setParameterNormals(GLParameterNormals *parameterNormals_) { _parameterNormals = parameterNormals_; } 29 | 30 | GLParameterCovariances* parameterCovariances() { return _parameterCovariances; } 31 | void setParameterCovariances(GLParameterCovariances *parameterCovariances_) { _parameterCovariances = parameterCovariances_; } 32 | 33 | GLParameterCorrespondences* parameterCorrespondences() { return _parameterCorrespondences; } 34 | void setParameterCorrespondences(GLParameterCorrespondences *parameterCorrespondences_) { _parameterCorrespondences = parameterCorrespondences_; } 35 | 36 | protected: 37 | GLParameterPoints *_parameterPoints; 38 | GLParameterNormals *_parameterNormals; 39 | GLParameterCovariances *_parameterCovariances; 40 | GLParameterCorrespondences *_parameterCorrespondences; 41 | }; 42 | 43 | } 44 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_correspondences.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_correspondences.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterCorrespondences::GLParameterCorrespondences() : GLParameter() { 6 | _pointSize = 0.1f; 7 | _color = Eigen::Vector4f(0.0f, 0.0f, 1.0f, 0.5f); 8 | _lineWidth = 3.0f; 9 | } 10 | 11 | GLParameterCorrespondences::GLParameterCorrespondences(float pointSize_, Eigen::Vector4f color_, float lineWidth_) : GLParameter() { 12 | _pointSize = pointSize_; 13 | _color = color_; 14 | _lineWidth = lineWidth_; 15 | } 16 | 17 | void GLParameterCorrespondences::applyGLParameter() { 18 | glColor4f(_color[0], _color[1], _color[2], _color[3]); 19 | glPointSize(_pointSize); 20 | glLineWidth(_lineWidth); 21 | } 22 | 23 | } 24 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_correspondences.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterCorrespondences : public GLParameter { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterCorrespondences(); 14 | GLParameterCorrespondences(float pointSize_, Eigen::Vector4f color_, float lineWidth_); 15 | virtual ~GLParameterCorrespondences() {} 16 | 17 | virtual void applyGLParameter(); 18 | 19 | float pointSize() { return _pointSize; } 20 | void setPointSize(float pointSize_) { _pointSize = pointSize_; } 21 | 22 | Eigen::Vector4f color() { return _color; } 23 | void setColor(Eigen::Vector4f color_) { _color = color_; } 24 | 25 | float lineWidth() { return _lineWidth; } 26 | void setLineWidth(float lineWidth_) { _lineWidth = lineWidth_; } 27 | 28 | protected: 29 | float _pointSize; 30 | Eigen::Vector4f _color; 31 | float _lineWidth; 32 | }; 33 | 34 | } 35 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_covariances.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_covariances.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterCovariances::GLParameterCovariances() : GLParameter() { 6 | _pointSize = 1.0f; 7 | _colorLowCurvature = Eigen::Vector4f(0.0f, 1.0f, 0.0f, 0.5f); 8 | _colorHighCurvature = Eigen::Vector4f(1.0f, 0.0f, 0.0f, 0.5f); 9 | _curvatureThreshold = 0.02f; 10 | _ellipsoidScale = 0.05f; 11 | } 12 | 13 | GLParameterCovariances::GLParameterCovariances(float pointSize_, 14 | Eigen::Vector4f colorLowCurvature_, Eigen::Vector4f colorHighCurvature_, 15 | float curvatureThreshold_, float ellipsoidScale_) : GLParameter() { 16 | _pointSize = pointSize_; 17 | _colorLowCurvature = colorLowCurvature_; 18 | _colorHighCurvature = colorHighCurvature_; 19 | _curvatureThreshold = curvatureThreshold_; 20 | _ellipsoidScale = ellipsoidScale_; 21 | } 22 | 23 | void GLParameterCovariances::applyGLParameter() { 24 | glPointSize(_pointSize); 25 | } 26 | 27 | } 28 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_covariances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterCovariances : public GLParameter { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterCovariances(); 14 | GLParameterCovariances(float pointSize_, 15 | Eigen::Vector4f colorLowCurvature_, Eigen::Vector4f colorHighCurvature_, 16 | float curvatureThreshold_, float ellipsoidScale_); 17 | virtual ~GLParameterCovariances() {} 18 | 19 | virtual void applyGLParameter(); 20 | 21 | float pointSize() { return _pointSize; } 22 | void setPointSize(float pointSize_) { _pointSize = pointSize_; } 23 | 24 | Eigen::Vector4f colorLowCurvature() { return _colorLowCurvature; } 25 | void setColorLowCurvature(Eigen::Vector4f colorLowCurvature_) { _colorLowCurvature = colorLowCurvature_; } 26 | 27 | Eigen::Vector4f colorHighCurvature() { return _colorHighCurvature; } 28 | void setColorHighCurvature(Eigen::Vector4f colorHighCurvature_) { _colorHighCurvature = colorHighCurvature_; } 29 | 30 | float curvatureThreshold() { return _curvatureThreshold; } 31 | void setCurvatureThreshold(float curvatureThreshold_) { _curvatureThreshold = curvatureThreshold_; } 32 | 33 | float ellipsoidScale() { return _ellipsoidScale; } 34 | void setEllipsoidScale(float ellipsoidScale_) { _ellipsoidScale = ellipsoidScale_; } 35 | 36 | protected: 37 | float _pointSize; 38 | Eigen::Vector4f _colorLowCurvature; 39 | Eigen::Vector4f _colorHighCurvature; 40 | float _curvatureThreshold; 41 | float _ellipsoidScale; 42 | }; 43 | 44 | } 45 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_normals.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_normals.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterNormals::GLParameterNormals() : GLParameter() { 6 | _pointSize = 0.1f; 7 | _color = Eigen::Vector4f(0.0f, 0.0f, 1.0f, 0.5f); 8 | _normalLength = 0.02f; 9 | _lineWidth = 1.0; 10 | } 11 | 12 | GLParameterNormals::GLParameterNormals(float pointSize_, Eigen::Vector4f color_, float normalLength_, float lineWidth_) : GLParameter() { 13 | _pointSize = pointSize_; 14 | _color = color_; 15 | _normalLength = normalLength_; 16 | _lineWidth = lineWidth_; 17 | } 18 | 19 | void GLParameterNormals::applyGLParameter() { 20 | glColor4f(_color[0], _color[1], _color[2], _color[3]); 21 | glPointSize(_pointSize); 22 | glLineWidth(_lineWidth); 23 | } 24 | 25 | } 26 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_normals.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterNormals: public GLParameter { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterNormals(); 14 | GLParameterNormals(float pointSize_, Eigen::Vector4f color_, float normalLength_, float lineWidth_ = 1.0f); 15 | virtual ~GLParameterNormals() {} 16 | 17 | virtual void applyGLParameter(); 18 | 19 | float pointSize() { return _pointSize; } 20 | void setPointSize(float pointSize_) { _pointSize = pointSize_; } 21 | 22 | Eigen::Vector4f color() { return _color; } 23 | void setColor(Eigen::Vector4f color_) { _color = color_; } 24 | 25 | float normalLength() { return _normalLength; } 26 | void setNormalLength(float normalLength_) { _normalLength = normalLength_; } 27 | 28 | float lineWidth() { return _lineWidth; } 29 | void setLineWidth(float lineWidth_) { _lineWidth = lineWidth_; } 30 | 31 | protected: 32 | float _pointSize; 33 | Eigen::Vector4f _color; 34 | float _normalLength; 35 | float _lineWidth; 36 | }; 37 | 38 | } 39 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_points.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_points.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterPoints::GLParameterPoints() : GLParameter() { 6 | _pointSize = 1.0f; 7 | _color = Eigen::Vector4f(1.0f, 1.0f, 0.0f, 0.5f); 8 | } 9 | 10 | GLParameterPoints::GLParameterPoints(float pointSize_, const Eigen::Vector4f color_) : GLParameter() { 11 | _pointSize = pointSize_; 12 | _color = color_; 13 | } 14 | 15 | void GLParameterPoints::applyGLParameter() { 16 | glColor4f(_color[0], _color[1], _color[2], _color[3]); 17 | glPointSize(_pointSize); 18 | } 19 | 20 | } 21 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_points.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterPoints : public GLParameter { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterPoints(); 14 | GLParameterPoints(float pointSize_, const Eigen::Vector4f color_); 15 | virtual ~GLParameterPoints() {} 16 | 17 | virtual void applyGLParameter(); 18 | 19 | float pointSize() { return _pointSize; } 20 | void setPointSize(float pointSize_) { _pointSize = pointSize_; } 21 | 22 | Eigen::Vector4f color() { return _color; } 23 | void setColor(Eigen::Vector4f color_) { _color = color_; } 24 | 25 | protected: 26 | float _pointSize; 27 | Eigen::Vector4f _color; 28 | }; 29 | 30 | } 31 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_trajectory.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterTrajectory::GLParameterTrajectory() : GLParameter() { 6 | _pyramidScale = 1.0f; 7 | _color = Eigen::Vector4f(1.0f, 1.0f, 0.0f, 0.5f); 8 | } 9 | 10 | GLParameterTrajectory::GLParameterTrajectory(float pyramidScale_, const Eigen::Vector4f color_) : GLParameter() { 11 | _pyramidScale = pyramidScale_; 12 | _color = color_; 13 | } 14 | 15 | } 16 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterTrajectory : public GLParameter { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterTrajectory(); 14 | GLParameterTrajectory(float pyramidScale_, const Eigen::Vector4f color_); 15 | virtual ~GLParameterTrajectory() {} 16 | 17 | virtual void applyGLParameter() { glColor4f(_color[0], _color[1], _color[2], _color[3]); } 18 | 19 | float pyramidScale() { return _pyramidScale; } 20 | void setPyramidScale(float pyramidScale_) { _pyramidScale = pyramidScale_; } 21 | 22 | Eigen::Vector4f color() { return _color; } 23 | void setColor(Eigen::Vector4f color_) { _color = color_; } 24 | 25 | protected: 26 | float _pyramidScale; 27 | Eigen::Vector4f _color; 28 | }; 29 | 30 | } 31 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_transform_covariance.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_transform_covariance.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterTransformCovariance::GLParameterTransformCovariance() : GLParameter() { 6 | _color = Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f); 7 | _scale = 0.05f; 8 | } 9 | 10 | GLParameterTransformCovariance::GLParameterTransformCovariance(Eigen::Vector4f color_, float scale_) : GLParameter() { 11 | _color = color_; 12 | _scale = scale_; 13 | } 14 | 15 | void GLParameterTransformCovariance::applyGLParameter() {} 16 | 17 | } 18 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_transform_covariance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterTransformCovariance : public GLParameter{ 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterTransformCovariance(); 14 | GLParameterTransformCovariance(Eigen::Vector4f color_, float scale_); 15 | virtual ~GLParameterTransformCovariance() {} 16 | 17 | virtual void applyGLParameter(); 18 | 19 | Eigen::Vector4f color() { return _color; } 20 | void setColor(Eigen::Vector4f color_) { _color = color_; } 21 | 22 | float scale() { return _scale; } 23 | void setScale(float scale_) { _scale = scale_; } 24 | 25 | protected: 26 | Eigen::Vector4f _color; 27 | float _scale; 28 | }; 29 | 30 | } 31 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_uncertainty.cpp: -------------------------------------------------------------------------------- 1 | #include "gl_parameter_uncertainty.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | GLParameterUncertainty::GLParameterUncertainty() : GLParameter() { 6 | _pointSize = 1.0f; 7 | _color = Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f); 8 | _ellipsoidScale = 0.05f; 9 | } 10 | 11 | GLParameterUncertainty::GLParameterUncertainty(float pointSize_, 12 | Eigen::Vector4f color_, 13 | float ellipsoidScale_) : GLParameter() { 14 | _pointSize = pointSize_; 15 | _color = color_; 16 | _ellipsoidScale = ellipsoidScale_; 17 | } 18 | 19 | void GLParameterUncertainty::applyGLParameter() { 20 | glColor4f(_color[0], _color[1], _color[2], _color[3]); 21 | glPointSize(_pointSize); 22 | } 23 | 24 | } 25 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/gl_parameter_uncertainty.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "gl_parameter.h" 6 | 7 | namespace nicp_viewer { 8 | 9 | class GLParameterUncertainty : public GLParameter { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | 13 | GLParameterUncertainty(); 14 | GLParameterUncertainty(float pointSize_, 15 | Eigen::Vector4f color_, 16 | float ellipsoidScale_); 17 | virtual ~GLParameterUncertainty() {} 18 | 19 | virtual void applyGLParameter(); 20 | 21 | float pointSize() { return _pointSize; } 22 | void setPointSize(float pointSize_) { _pointSize = pointSize_; } 23 | 24 | Eigen::Vector4f color() { return _color; } 25 | void setColor(Eigen::Vector4f color_) { _color = color_; } 26 | 27 | float ellipsoidScale() { return _ellipsoidScale; } 28 | void setEllipsoidScale(float ellipsoidScale_) { _ellipsoidScale = ellipsoidScale_; } 29 | 30 | protected: 31 | float _pointSize; 32 | Eigen::Vector4f _color; 33 | float _ellipsoidScale; 34 | }; 35 | 36 | } 37 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/imageview.cpp: -------------------------------------------------------------------------------- 1 | #include "imageview.h" 2 | 3 | namespace nicp_viewer { 4 | 5 | DepthImageView::DepthImageView() { 6 | computeColorMap (0, 255, 0xff); 7 | } 8 | 9 | void DepthImageView::computeColorMap(int cmin, int cmax, unsigned char alpha){ 10 | unsigned int* color = _colorMap; 11 | float scale = 255.0f / (float)(cmax - cmin); 12 | for(int i = 0; i < 0xffff; i++) { 13 | unsigned char c; 14 | if(i < cmin) { 15 | c = 0x00; 16 | } 17 | else if(i > cmax) { 18 | c = 0xff; 19 | } 20 | else { 21 | c = (unsigned char) (scale*(i - cmin)); 22 | } 23 | *color++ = ((unsigned int) alpha << 24) | c << 16 | c << 8 | c; 24 | } 25 | } 26 | 27 | void DepthImageView::convertToQImage(QImage &img, const RawDepthImage &m) const { 28 | if(img.size().height() != m.rows || img.size().width() != m.cols || img.format() != QImage::Format_ARGB32) 29 | img = QImage(m.cols, m.rows, QImage::Format_ARGB32); 30 | for(int r = 0; r < m.rows; ++r) { 31 | for(int c = 0; c < m.cols; ++c) { 32 | img.setPixel(c, r, color(m(r, c))); 33 | } 34 | } 35 | } 36 | 37 | void DepthImageView::convertToQImage(QImage &img, const DepthImage &m) const { 38 | if(img.size().height() != m.rows || img.size().width() != m.cols || img.format() != QImage::Format_ARGB32) 39 | img = QImage(m.cols, m.rows, QImage::Format_ARGB32); 40 | for(int r = 0; r < m.rows; ++r) { 41 | for(int c = 0; c < m.cols; ++c) { 42 | img.setPixel(c, r, color(1000.0f * m(r, c))); 43 | } 44 | } 45 | } 46 | 47 | 48 | RGBImageView::RGBImageView() { 49 | } 50 | 51 | 52 | void RGBImageView::convertToQImage(QImage &img, const RGBImage &m) const { 53 | if(img.size().height() != m.cols || img.size().width() != m.rows || img.format() != QImage::Format_ARGB32) 54 | img = QImage(m.rows, m.cols, QImage::Format_ARGB32); 55 | for(int j = 0; j < m.cols; j++) { 56 | for(int i = 0; i < m.rows; i++) { 57 | const cv::Vec3b& c = m(i,j); 58 | unsigned int color = c[0] << 16 | c[1] << 8 | c[2]; 59 | img.setPixel(i, j, color); 60 | } 61 | } 62 | } 63 | 64 | 65 | } 66 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/imageview.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "nicp/definitions.h" 7 | 8 | using namespace nicp; 9 | 10 | namespace nicp_viewer { 11 | 12 | struct DepthImageView { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 14 | 15 | DepthImageView(); 16 | ~DepthImageView() {} 17 | 18 | void computeColorMap(int cmin, int cmax, unsigned char alpha); 19 | 20 | inline unsigned int color(unsigned short idx) const { return _colorMap[idx]; } 21 | 22 | void convertToQImage(QImage &img, const RawDepthImage &m) const; 23 | void convertToQImage(QImage &img, const DepthImage &m) const; 24 | 25 | protected: 26 | unsigned int _colorMap[0xffff]; 27 | }; 28 | 29 | struct RGBImageView { 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 31 | RGBImageView(); 32 | ~RGBImageView() {} 33 | void convertToQImage(QImage &img, const RGBImage &m) const; 34 | }; 35 | 36 | 37 | } 38 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/nicp_aligner_gui.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "nicp_aligner_gui_main_window.h" 6 | 7 | int main(int argc, char **argv) { 8 | if(argc < 2 || 9 | std::string(argv[1]) == "-h" || std::string(argv[1]) == "-help" || 10 | std::string(argv[1]) == "--h" || std::string(argv[1]) == "--help") { 11 | std::cout << "USAGE: nicp_aligner_gui working_directory" << std::endl; 12 | std::cout << "working_directory \n " 13 | << "\t directory where to search for .pgm depth images" 14 | << std::endl; 15 | return 0; 16 | } 17 | std::string working_directory = std::string(argv[1]); 18 | 19 | QApplication qApplication(argc, argv); 20 | nicp_viewer::NICPAlignerGuiMainWindow mainWindow(working_directory); 21 | mainWindow.viewer->setKinectFrameCameraPosition(); 22 | mainWindow.showMaximized(); 23 | mainWindow.show(); 24 | 25 | /* Standard Kinect */ 26 | mainWindow.pinhole_projector_radioButton->setChecked(true); 27 | mainWindow.fx_doubleSpinBox->setValue(525.0f); 28 | mainWindow.fy_doubleSpinBox->setValue(525.0f); 29 | mainWindow.cx_doubleSpinBox->setValue(319.5); 30 | mainWindow.cy_doubleSpinBox->setValue(239.5); 31 | mainWindow.rows_spinBox->setValue(480); 32 | mainWindow.cols_spinBox->setValue(640); 33 | mainWindow.min_distance_doubleSpinBox->setValue(0.5f); 34 | mainWindow.max_distance_doubleSpinBox->setValue(3.0f); 35 | mainWindow.min_image_radius_spinBox->setValue(20); 36 | mainWindow.max_image_radius_spinBox->setValue(40); 37 | mainWindow.min_points_spinBox->setValue(40); 38 | mainWindow.world_radius_doubleSpinBox->setValue(0.1f); 39 | mainWindow.curv_threshold_doubleSpinBox->setValue(0.2f); 40 | mainWindow.normal_angle_doubleSpinBox->setValue(0.9f); 41 | mainWindow.point_distance_doubleSpinBox->setValue(0.5f); 42 | mainWindow.max_chi2_doubleSpinBox->setValue(100.0f); 43 | mainWindow.curv_flatness_doubleSpinBox->setValue(0.02f); 44 | mainWindow.statsUpdate(); 45 | mainWindow.correspondencesUpdate(); 46 | mainWindow.alignerUpdate(); 47 | mainWindow.projectorsUpdate(); 48 | 49 | /* ETH Laser */ 50 | // mainWindow.spherical_projector_radioButton->setChecked(true); 51 | // mainWindow.rows_spinBox->setValue(1000); 52 | // mainWindow.cols_spinBox->setValue(1500); 53 | // mainWindow.min_distance_doubleSpinBox->setValue(0.5f); 54 | // mainWindow.max_distance_doubleSpinBox->setValue(30.0f); 55 | // mainWindow.min_image_radius_spinBox->setValue(10); 56 | // mainWindow.max_image_radius_spinBox->setValue(100); 57 | // mainWindow.min_points_spinBox->setValue(30); 58 | // mainWindow.world_radius_doubleSpinBox->setValue(0.25f); 59 | // mainWindow.curv_threshold_doubleSpinBox->setValue(0.1f); 60 | // mainWindow.normal_angle_doubleSpinBox->setValue(0.78f); 61 | // mainWindow.point_distance_doubleSpinBox->setValue(1.5f); 62 | // mainWindow.statsUpdate(); 63 | // mainWindow.correspondencesUpdate(); 64 | // mainWindow.alignerUpdate(); 65 | // mainWindow.projectorsUpdate(); 66 | 67 | /* ETH Kinect */ 68 | // mainWindow.pinhole_projector_radioButton->setChecked(true); 69 | // mainWindow.fx_doubleSpinBox->setValue(131.25); 70 | // mainWindow.fy_doubleSpinBox->setValue(131.25); 71 | // mainWindow.cx_doubleSpinBox->setValue(79.875); 72 | // mainWindow.cy_doubleSpinBox->setValue(59.875); 73 | // mainWindow.rows_spinBox->setValue(120); 74 | // mainWindow.cols_spinBox->setValue(160); 75 | // mainWindow.min_distance_doubleSpinBox->setValue(0.01f); 76 | // mainWindow.max_distance_doubleSpinBox->setValue(5.0f); 77 | // mainWindow.min_image_radius_spinBox->setValue(5); 78 | // mainWindow.max_image_radius_spinBox->setValue(10); 79 | // mainWindow.min_points_spinBox->setValue(20); 80 | // mainWindow.world_radius_doubleSpinBox->setValue(0.2f); 81 | // mainWindow.curv_threshold_doubleSpinBox->setValue(0.3f); 82 | // mainWindow.normal_angle_doubleSpinBox->setValue(0.8f); 83 | // mainWindow.point_distance_doubleSpinBox->setValue(0.25f); 84 | // mainWindow.max_chi2_doubleSpinBox->setValue(1.0f); 85 | // mainWindow.curv_flatness_doubleSpinBox->setValue(0.3f); 86 | // mainWindow.statsUpdate(); 87 | // mainWindow.correspondencesUpdate(); 88 | // mainWindow.alignerUpdate(); 89 | // mainWindow.projectorsUpdate(); 90 | 91 | while(mainWindow.isVisible()) { qApplication.processEvents(); } 92 | 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/nicp_aligner_gui_main_window.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "nicp_aligner_gui_ui_main_window.h" 6 | 7 | #include "nicp/pinholepointprojector.h" 8 | #include "nicp/sphericalpointprojector.h" 9 | #include "nicp/statscalculatorintegralimage.h" 10 | #include "nicp/informationmatrixcalculator.h" 11 | #include "nicp/depthimageconverterintegralimage.h" 12 | #include "nicp/linearizer.h" 13 | #include "nicp/alignerprojective.h" 14 | #include "nicp/alignernn.h" 15 | 16 | namespace nicp_viewer { 17 | 18 | class NICPAlignerGuiMainWindow : public QMainWindow, public Ui::MainWindow { 19 | Q_OBJECT 20 | public: 21 | NICPAlignerGuiMainWindow(std::string directory, QWidget *parent = 0, Qt::WindowFlags flags = 0); 22 | ~NICPAlignerGuiMainWindow(); 23 | 24 | void setSensorOffset(const Eigen::Isometry3f &sensorOffset_) { _sensorOffset = sensorOffset_; } 25 | const Eigen::Isometry3f& sensorOffset() { return _sensorOffset; } 26 | 27 | public slots: 28 | virtual void visualizationUpdate(); 29 | virtual void statsUpdate(); 30 | virtual void correspondencesUpdate(); 31 | virtual void alignerUpdate(); 32 | virtual void projectorsUpdate(); 33 | virtual void addCloud(); 34 | virtual void nicpSnapshot(); 35 | virtual void jpgSnapshot(); 36 | virtual void clearLast(); 37 | virtual void clearAll(); 38 | virtual void correspondences(); 39 | virtual void initialGuess(); 40 | virtual void optimize(); 41 | virtual void merge(); 42 | 43 | protected: 44 | std::set _readDirectory(std::string directory); 45 | void _selectProjector(); 46 | 47 | unsigned int _pngCounter, _nicpCounter; 48 | 49 | Eigen::Isometry3f _sensorOffset; 50 | 51 | nicp::PointProjector *_projector; 52 | nicp::PinholePointProjector *_pinholeProjector; 53 | nicp::SphericalPointProjector *_sphericalProjector; 54 | nicp::StatsCalculatorIntegralImage *_statsCalculatorIntegralImage; 55 | nicp::PointInformationMatrixCalculator *_pointInformationMatrixCalculator; 56 | nicp::NormalInformationMatrixCalculator *_normalInformationMatrixCalculator; 57 | nicp::DepthImageConverterIntegralImage *_depthImageConverterIntegralImage; 58 | nicp::CorrespondenceFinderProjective *_correspondenceFinderProjective; 59 | nicp::CorrespondenceFinderNN *_correspondenceFinderNN; 60 | nicp::CorrespondenceFinder *_correspondenceFinder; 61 | nicp::Linearizer *_linearizer; 62 | nicp::AlignerProjective *_alignerProjective; 63 | nicp::AlignerNN *_alignerNN; 64 | nicp::Aligner *_aligner; 65 | 66 | nicp::RawDepthImage _rawDepth; 67 | nicp::DepthImage _depth, _scaledDepth, _referenceDepth, _currentDepth; 68 | 69 | std::vector _clouds; 70 | std::vector _poses; 71 | std::vector _depths; 72 | 73 | QGraphicsScene *_referenceScene, *_currentScene; 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/nicp_qglviewer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "nicp_qglviewer.h" 4 | 5 | #include "opengl_primitives.h" 6 | 7 | using namespace Eigen; 8 | 9 | namespace nicp_viewer { 10 | 11 | class StandardCamera : public qglviewer::Camera { 12 | public: 13 | StandardCamera() : _standard(true) {} 14 | 15 | qreal zNear() const { 16 | if(_standard) 17 | return qreal(0.001f); 18 | else 19 | return Camera::zNear(); 20 | } 21 | 22 | qreal zFar() const { 23 | if(_standard) 24 | return qreal(10000.0f); 25 | else 26 | return Camera::zFar(); 27 | } 28 | 29 | bool standard() const { return _standard; } 30 | 31 | void setStandard(bool s) { _standard = s; } 32 | 33 | protected: 34 | bool _standard; 35 | }; 36 | 37 | NICPQGLViewer::NICPQGLViewer(QWidget *parent, const QGLWidget *shareWidget, Qt::WindowFlags flags) : QGLViewer(parent, shareWidget, flags), _last_key_event(QEvent::None, 0, Qt::NoModifier) { 38 | _last_key_event_processed = true; 39 | 40 | _ellipsoidDrawList = 0; 41 | _numDrawLists = 2; 42 | } 43 | 44 | void NICPQGLViewer::updateCameraPosition(Eigen::Isometry3f pose) { 45 | qglviewer::Camera *oldcam = camera(); 46 | qglviewer::Camera *cam = new StandardCamera(); 47 | setCamera(cam); 48 | 49 | Eigen::Vector3f position = pose*Vector3f(-2.0f, 0.0f, 1.0f); 50 | cam->setPosition(qglviewer::Vec(position[0], position[1], position[2])); 51 | Eigen::Vector3f upVector = pose.linear()*Vector3f(0.0f, 0.0f, 0.5f); 52 | upVector.normalize(); 53 | cam->setUpVector(qglviewer::Vec(upVector[0], upVector[1], upVector[2]), true); 54 | 55 | Eigen::Vector3f lookAt = pose*Vector3f(4.0f, 0.0f, 0.0f); 56 | cam->lookAt(qglviewer::Vec(lookAt[0], lookAt[1], lookAt[2])); 57 | 58 | delete oldcam; 59 | } 60 | 61 | void NICPQGLViewer::setKinectFrameCameraPosition() { 62 | qglviewer::Camera *oldcam = camera(); 63 | qglviewer::Camera *cam = new StandardCamera(); 64 | setCamera(cam); 65 | 66 | cam->setPosition(qglviewer::Vec(0.0f, 0.0f, -1.0f)); 67 | cam->setUpVector(qglviewer::Vec(0.0f, -1.0f, 0.0f)); 68 | cam->lookAt(qglviewer::Vec(0.0f, 0.0f, 0.0f)); 69 | 70 | delete oldcam; 71 | } 72 | 73 | void NICPQGLViewer::init() { 74 | // Init QGLViewer. 75 | QGLViewer::init(); 76 | 77 | // Set some default settings. 78 | glEnable(GL_LINE_SMOOTH); 79 | glEnable(GL_BLEND); 80 | glEnable(GL_DEPTH_TEST); 81 | glEnable(GL_NORMALIZE); 82 | glShadeModel(GL_FLAT); 83 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 84 | 85 | // Don't save state. 86 | setStateFileName(QString::null); 87 | 88 | // Mouse bindings. 89 | setMouseBinding(Qt::RightButton, CAMERA, ZOOM); 90 | setMouseBinding(Qt::MidButton, CAMERA, TRANSLATE); 91 | 92 | // Replace camera. 93 | qglviewer::Camera *oldcam = camera(); 94 | qglviewer::Camera *cam = new StandardCamera(); 95 | setCamera(cam); 96 | cam->setPosition(qglviewer::Vec(-1.0f, 0.0f, 0.0f)); 97 | cam->setUpVector(qglviewer::Vec(0.0f, 0.0f, 1.0f)); 98 | cam->lookAt(qglviewer::Vec(0.0f, 0.0f, 0.0f)); 99 | delete oldcam; 100 | 101 | // Create draw lists. 102 | _ellipsoidDrawList = glGenLists(_numDrawLists); 103 | _pyramidDrawList = glGenLists(_numDrawLists); 104 | 105 | // Compile draw lists. 106 | // Ellipsoid. 107 | glNewList(_ellipsoidDrawList, GL_COMPILE); 108 | drawSphere(1.0f); 109 | glEndList(); 110 | // Pyramid. 111 | glNewList(_pyramidDrawList, GL_COMPILE); 112 | drawPyramid(0.5f, 0.5f); 113 | glEndList(); 114 | } 115 | 116 | // Function containing the draw commands. 117 | void NICPQGLViewer::draw() { 118 | QGLViewer::draw(); 119 | 120 | // Draw the vector of drawable objects. 121 | for(size_t i = 0; i < _drawableList.size(); i++) { 122 | if(_drawableList[i]) { 123 | _drawableList[i]->draw(); 124 | } 125 | } 126 | } 127 | 128 | // Function to add a drawable objects to the viewer. 129 | void NICPQGLViewer::addDrawable(Drawable *d) { 130 | // Set the viewer to the input drawable object. 131 | d->setViewer(this); 132 | // Add the input object to the vector. 133 | _drawableList.push_back(d); 134 | } 135 | 136 | QKeyEvent* NICPQGLViewer::lastKeyEvent() { 137 | if(_last_key_event_processed) { return 0; } 138 | return &_last_key_event; 139 | } 140 | 141 | void NICPQGLViewer::keyEventProcessed() { _last_key_event_processed = true; } 142 | 143 | void NICPQGLViewer::keyPressEvent(QKeyEvent* e) { 144 | QGLViewer::keyPressEvent(e); 145 | _last_key_event = *e; 146 | _last_key_event_processed = false; 147 | } 148 | 149 | } 150 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/nicp_qglviewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "drawable.h" 10 | 11 | using namespace std; 12 | 13 | namespace nicp_viewer { 14 | 15 | class NICPQGLViewer : public QGLViewer { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 18 | 19 | NICPQGLViewer(QWidget *parent = 0, const QGLWidget *shareWidget = 0, Qt::WindowFlags flags = 0); 20 | virtual ~NICPQGLViewer() {} 21 | 22 | virtual void init(); 23 | virtual void draw(); 24 | 25 | virtual void keyPressEvent(QKeyEvent *e); 26 | QKeyEvent* lastKeyEvent(); 27 | void keyEventProcessed(); 28 | 29 | virtual void addDrawable(Drawable *d); 30 | inline void popFront() { _drawableList.erase(_drawableList.begin()); } 31 | inline void popBack() { _drawableList.pop_back(); } 32 | inline void erase(int index) { _drawableList.erase(_drawableList.begin() + index); } 33 | inline void clearDrawableList() { _drawableList.clear(); } 34 | 35 | inline GLuint ellipsoidDrawList() { return _ellipsoidDrawList; } 36 | inline GLuint pyramidDrawList() { return _ellipsoidDrawList; } 37 | inline std::vector& drawableList() { return _drawableList; } 38 | 39 | void updateCameraPosition(Eigen::Isometry3f pose); 40 | void setKinectFrameCameraPosition(); 41 | 42 | inline const std::vector& drawableList() const { return _drawableList; } 43 | 44 | protected: 45 | QKeyEvent _last_key_event; 46 | bool _last_key_event_processed; 47 | 48 | int _numDrawLists; 49 | GLuint _ellipsoidDrawList; 50 | GLuint _pyramidDrawList; 51 | std::vector _drawableList; 52 | }; 53 | 54 | } 55 | -------------------------------------------------------------------------------- /nicp/nicp_viewer/opengl_primitives.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace nicp_viewer { 7 | 8 | /** 9 | * Draw a box that is centered in the current coordinate frame 10 | * @param l length of the box (x dimension) 11 | * @param w width of the box (y dimension) 12 | * @param h height of the box (z dimension) 13 | */ 14 | void drawBox(GLfloat l, GLfloat w, GLfloat h); 15 | 16 | /** 17 | * Draw a plane in x-y dimension with a height of zero 18 | * @param l length in x 19 | * @param w width in y 20 | */ 21 | void drawPlane(GLfloat l, GLfloat w); 22 | 23 | /** 24 | * Draw a sphere whose center is in the origin of the current coordinate frame 25 | * @param radius the radius of the sphere 26 | */ 27 | void drawSphere(GLfloat radius); 28 | 29 | /** 30 | * Draw a ellipsoid whose center is in the origin of the current coordinate frame 31 | * @param r1 radius along x axis 32 | * @param r2 radius along y axis 33 | * @param r3 radius along z axis 34 | */ 35 | void drawEllipsoid(GLfloat r1, GLfloat r2, GLfloat r3); 36 | 37 | /** 38 | * Draw a cone 39 | */ 40 | void drawCone(GLfloat radius, GLfloat height); 41 | 42 | /** 43 | * Draw a disk 44 | */ 45 | void drawDisk(GLfloat radius); 46 | 47 | /** 48 | * Draw a (closed) cylinder 49 | * @param radius the radius of the cylinder 50 | * @param height the height of the cylinder 51 | */ 52 | void drawCylinder(GLfloat radius, GLfloat height); 53 | 54 | /** 55 | * Draw a pyramid 56 | */ 57 | void drawPyramid(GLfloat length, GLfloat height); 58 | 59 | /** 60 | * Draw a range ring 61 | * @param range the range (radius) of the partial ring 62 | * @param fov Field Of View of the range sensor 63 | * @param range_width specify how thick the ring should be drawn 64 | */ 65 | void drawRangeRing(GLfloat range, GLfloat fov, GLfloat range_width = 0.05); 66 | 67 | /** 68 | * Draw a slice of a cylinder (approximated with slices_per_circle triangles for the complete circle) 69 | * @param radius the radius of the cylinder 70 | * @param height the height of the cylinder 71 | * @param fov the "fov" of the slice (om degree) 72 | * @param slices_per_circle the number of triangle used to approximate the fulle circle 73 | */ 74 | void drawSlice(GLfloat radius, GLfloat height, GLfloat fov, int slices_per_circle = 32); 75 | 76 | /** 77 | * Draws a box used to represent a 6d pose 78 | */ 79 | void drawPoseBox(); 80 | 81 | /** 82 | * Draw a 2D arrow along the x axis with the given len 83 | */ 84 | void drawArrow2D(float len, float head_width, float head_len); 85 | 86 | /** 87 | * Draw a point in the origin, having a size of pointSize 88 | */ 89 | void drawPoint(float pointSize); 90 | 91 | } 92 | --------------------------------------------------------------------------------