├── CMakeLists.txt ├── cmake └── find │ ├── FindFreenect.cmake │ ├── FindGLUT.cmake │ ├── FindOpenGL.cmake │ └── FindOpenCV.cmake ├── README.txt └── kinReg.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8) 2 | 3 | project(KinReg C CXX) 4 | 5 | set(DEPENDENCIES OpenCV GLUT OpenGL Freenect) 6 | 7 | message("\n") 8 | foreach( DEP ${DEPENDENCIES} ) 9 | 10 | find_package( ${DEP} ) 11 | 12 | string( TOUPPER ${DEP} UDEP ) # Capitalize 13 | if( ${DEP}_FOUND OR ${UDEP}_FOUND ) 14 | message("\n${DEP}_Found = TRUE\n") 15 | else() 16 | message("Find${DEP}.cmake not found... Looking for ${DEP} 17 | ourselves.\n") 18 | include( "cmake/find/Find${DEP}.cmake" ) # Try our Find files 19 | if( ${DEP}_FOUND OR ${UDEP}_FOUND ) 20 | message("\n${DEP}_Found = TRUE\n") 21 | else() 22 | message("\n${DEP}_Found = FALSE\n") 23 | endif() 24 | endif() 25 | 26 | endforeach() 27 | 28 | 29 | include_directories( 30 | 31 | ${FREENECT_INCLUDE_DIR} 32 | ${GLUT_INCLUDE_DIR} 33 | ${OPENGL_INCLUDE_DIR} 34 | ${OpenCV_INCLUDE_DIRS} 35 | ) 36 | 37 | add_executable(kinect_reg kinReg.cpp) 38 | 39 | target_link_libraries(kinect_reg 40 | 41 | ${FREENECT_LIBRARIES} 42 | ${GLUT_LIBRARY} 43 | ${OPENGL_LIBRARIES} 44 | ${OpenCV_LIBS} 45 | ) 46 | -------------------------------------------------------------------------------- /cmake/find/FindFreenect.cmake: -------------------------------------------------------------------------------- 1 | # - Find the libfreenect includes and library 2 | # This module defines 3 | # FREENECT_INCLUDE_DIR, path to libfreenect/libfreenect.h, etc. 4 | # FREENECT_LIBRARIES, the libraries required to use FREENECT. 5 | # FREENECT_FOUND, If false, do not try to use FREENECT. 6 | # also defined, but not for general use are 7 | # FREENECT_freenect_LIBRARY, where to find the FREENECT library. 8 | 9 | FIND_PATH(FREENECT_INCLUDE_DIR libfreenect.h 10 | /usr/include 11 | /usr/include/libfreenect 12 | /usr/local/include 13 | /usr/local/include/libfreenect 14 | ) 15 | 16 | message("FREENECT_INCLUDE_DIR = ${FREENECT_INCLUDE_DIR} " ) 17 | 18 | set( LIB_SEARCH_PATHS /usr/local/lib64 /usr/local/lib /usr/lib NO_DEFAULT_PATH ) 19 | 20 | FIND_LIBRARY(FREENECT_LIB freenect 21 | ${LIB_SEARCH_PATHS} 22 | ) 23 | 24 | FIND_LIBRARY(FREENECT_SYNC_LIB freenect_sync 25 | ${LIB_SEARCH_PATHS} 26 | ) 27 | 28 | FIND_LIBRARY(FREENECT_CV_LIB freenect_cv 29 | ${LIB_SEARCH_PATHS} 30 | ) 31 | 32 | set( FREENECT_LIBRARIES ${FREENECT_LIB} ${FREENECT_SYNC_LIB} ${FREENECT_CV_LIB} ) 33 | 34 | message("FREENECT_LIBRARIES = ${FREENECT_LIBRARIES} ") 35 | 36 | MARK_AS_ADVANCED( 37 | FREENECT_INCLUDE_DIR 38 | FREENECT_LIBRARIES) 39 | 40 | SET( FREENECT_FOUND "NO" ) 41 | IF(FREENECT_INCLUDE_DIR) 42 | IF(FREENECT_LIBRARIES) 43 | SET( FREENECT_FOUND "YES" ) 44 | ENDIF(FREENECT_LIBRARIES) 45 | ENDIF(FREENECT_INCLUDE_DIR) 46 | 47 | IF(FREENECT_FOUND) 48 | MESSAGE(STATUS "Found freenect library") 49 | ELSE(FREENECT_FOUND) 50 | IF(FREENECT_FIND_REQUIRED) 51 | MESSAGE(FATAL_ERROR "Could not find libfreenect 52 | -- please give some paths to CMake or make sure libfreenect is installed in your system") 53 | ELSE(FREENECT_FIND_REQUIRED) 54 | MESSAGE(STATUS "Could not find libfreenect 55 | -- please give some paths to CMake or make sure libfreenect is installed in your system") 56 | ENDIF(FREENECT_FIND_REQUIRED) 57 | ENDIF(FREENECT_FOUND) 58 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | Author: Nathan Crock 2 | mathnathan@gmail.com 3 | http://www.mathnathan.com 4 | 5 | 6 | Date: 03/23/11 14:53 7 | 8 | 9 | ----------- Kinect Camera Registraion ------------ 10 | 11 | 12 | OS SUPPORT: Only tested on Linux so far 13 | 14 | Ubuntu 12.04, 12.10 15 | 16 | FILES: 17 | KinReg.cpp - The registraion program 18 | CMakeLists.txt - Cmake file with build commands 19 | cmake | 20 | | find | 21 | | Find*.cmake all of the cmake files to find the 22 | | dependencies 23 | 24 | 25 | DEPENDENCIES: Freenect 26 | OpenCV 27 | OpenGL 28 | GLUT 29 | 30 | COMPILATION INSTRUCTIONS: 31 | 32 | -If you don't have cmake you can get it with aptitude 33 | 34 | sudo apt-get install cmake 35 | 36 | -For a breif introduction to CMake follow my tutorial 37 | 38 | http://mathnathan.com/2010/07/11/getting-started-with-cmake/ 39 | 40 | -Make a build directory and from it run 41 | 42 | cmake .. 43 | make 44 | 45 | NOTE: If you have the dependencies installed (to your root directories, or 46 | their paths in your ldpaths), then it should "just work" in UBUNTU 12.04 and 47 | 12.10. Please let me know otherwise 48 | 49 | 50 | ---------------------------Simple Project Manual-------------------------------------- 51 | 52 | 53 | This program is intended to register multiple point clouds 54 | into one coordinate system. The point clouds are obtained 55 | from Microsoft's Kinect sensors. 56 | 57 | NOTE: Only support for 2 cameras for now 58 | 59 | It begins by finding correspondences between the two cameras. 60 | The process is interactive. 61 | 62 | Once KinReg is running, a window pops up containing both images side by side. 63 | 64 | All you do is click on pixels in both cameras which correspond to the same 65 | point in world coordinates. Currently the process is rough, it can be 66 | improved in the future by drawing circles around the points you click on 67 | and adding colors to signify if the correspondence is accepted or not. If 68 | both depth measurements for your clicks are found it stores the 69 | correspondence. 70 | 71 | If at least one of the corespondences is bad (no depth) it deletes the match. 72 | 73 | Once you've collected around 10 or so correspondences pushing 'p' applies 74 | procrustes analysis on the correspondences and calculates the transformation 75 | matrices. 76 | 77 | Press 't' to see the translation of the centroids to the origin 78 | 79 | Press 'a' to see the translation and rotation applied to both point clouds 80 | 81 | ====================================================================================== 82 | 83 | "Believing in the way, makes the way!" 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /cmake/find/FindGLUT.cmake: -------------------------------------------------------------------------------- 1 | # - try to find glut library and include files 2 | # GLUT_INCLUDE_DIR, where to find GL/glut.h, etc. 3 | # GLUT_LIBRARIES, the libraries to link against 4 | # GLUT_FOUND, If false, do not try to use GLUT. 5 | # Also defined, but not for general use are: 6 | # GLUT_glut_LIBRARY = the full path to the glut library. 7 | # GLUT_Xmu_LIBRARY = the full path to the Xmu library. 8 | # GLUT_Xi_LIBRARY = the full path to the Xi Library. 9 | 10 | #============================================================================= 11 | # Copyright 2001-2009 Kitware, Inc. 12 | # 13 | # Distributed under the OSI-approved BSD License (the "License"); 14 | # see accompanying file Copyright.txt for details. 15 | # 16 | # This software is distributed WITHOUT ANY WARRANTY; without even the 17 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 18 | # See the License for more information. 19 | #============================================================================= 20 | # (To distribute this file outside of CMake, substitute the full 21 | # License text for the above reference.) 22 | 23 | IF (WIN32) 24 | FIND_PATH( GLUT_INCLUDE_DIR NAMES GL/glut.h 25 | PATHS ${GLUT_ROOT_PATH}/include ) 26 | FIND_LIBRARY( GLUT_glut_LIBRARY NAMES glut glut32 freeglut 27 | PATHS 28 | ${OPENGL_LIBRARY_DIR} 29 | ${GLUT_ROOT_PATH}/Release 30 | ) 31 | ELSE (WIN32) 32 | 33 | IF (APPLE) 34 | # These values for Apple could probably do with improvement. 35 | FIND_PATH( GLUT_INCLUDE_DIR glut.h 36 | /System/Library/Frameworks/GLUT.framework/Versions/A/Headers 37 | ${OPENGL_LIBRARY_DIR} 38 | ) 39 | SET(GLUT_glut_LIBRARY "-framework GLUT" CACHE STRING "GLUT library for OSX") 40 | SET(GLUT_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX") 41 | ELSE (APPLE) 42 | 43 | FIND_PATH( GLUT_INCLUDE_DIR GL/glut.h 44 | /usr/include/GL 45 | /usr/openwin/share/include 46 | /usr/openwin/include 47 | /opt/graphics/OpenGL/include 48 | /opt/graphics/OpenGL/contrib/libglut 49 | ) 50 | 51 | FIND_LIBRARY( GLUT_glut_LIBRARY glut 52 | /usr/openwin/lib 53 | ) 54 | 55 | FIND_LIBRARY( GLUT_Xi_LIBRARY Xi 56 | /usr/openwin/lib 57 | ) 58 | 59 | FIND_LIBRARY( GLUT_Xmu_LIBRARY Xmu 60 | /usr/openwin/lib 61 | ) 62 | 63 | ENDIF (APPLE) 64 | 65 | ENDIF (WIN32) 66 | 67 | INCLUDE(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) 68 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GLUT REQUIRED_VARS GLUT_glut_LIBRARY GLUT_INCLUDE_DIR) 69 | 70 | IF (GLUT_FOUND) 71 | # Is -lXi and -lXmu required on all platforms that have it? 72 | # If not, we need some way to figure out what platform we are on. 73 | SET( GLUT_LIBRARIES 74 | ${GLUT_glut_LIBRARY} 75 | ${GLUT_Xmu_LIBRARY} 76 | ${GLUT_Xi_LIBRARY} 77 | ${GLUT_cocoa_LIBRARY} 78 | ) 79 | 80 | #The following deprecated settings are for backwards compatibility with CMake1.4 81 | SET (GLUT_LIBRARY ${GLUT_LIBRARIES}) 82 | SET (GLUT_INCLUDE_PATH ${GLUT_INCLUDE_DIR}) 83 | ENDIF(GLUT_FOUND) 84 | 85 | MARK_AS_ADVANCED( 86 | GLUT_INCLUDE_DIR 87 | GLUT_glut_LIBRARY 88 | GLUT_Xmu_LIBRARY 89 | GLUT_Xi_LIBRARY 90 | ) 91 | -------------------------------------------------------------------------------- /cmake/find/FindOpenGL.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find OpenGL 2 | # Once done this will define 3 | # 4 | # OPENGL_FOUND - system has OpenGL 5 | # OPENGL_XMESA_FOUND - system has XMESA 6 | # OPENGL_GLU_FOUND - system has GLU 7 | # OPENGL_INCLUDE_DIR - the GL include directory 8 | # OPENGL_LIBRARIES - Link these to use OpenGL and GLU 9 | # 10 | # If you want to use just GL you can use these values 11 | # OPENGL_gl_LIBRARY - Path to OpenGL Library 12 | # OPENGL_glu_LIBRARY - Path to GLU Library 13 | # 14 | # On OSX default to using the framework version of opengl 15 | # People will have to change the cache values of OPENGL_glu_LIBRARY 16 | # and OPENGL_gl_LIBRARY to use OpenGL with X11 on OSX 17 | 18 | #============================================================================= 19 | # Copyright 2001-2009 Kitware, Inc. 20 | # 21 | # Distributed under the OSI-approved BSD License (the "License"); 22 | # see accompanying file Copyright.txt for details. 23 | # 24 | # This software is distributed WITHOUT ANY WARRANTY; without even the 25 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 26 | # See the License for more information. 27 | #============================================================================= 28 | # (To distribute this file outside of CMake, substitute the full 29 | # License text for the above reference.) 30 | 31 | IF (WIN32) 32 | IF (CYGWIN) 33 | 34 | FIND_PATH(OPENGL_INCLUDE_DIR GL/gl.h ) 35 | 36 | FIND_LIBRARY(OPENGL_gl_LIBRARY opengl32 ) 37 | 38 | FIND_LIBRARY(OPENGL_glu_LIBRARY glu32 ) 39 | 40 | ELSE (CYGWIN) 41 | 42 | IF(BORLAND) 43 | SET (OPENGL_gl_LIBRARY import32 CACHE STRING "OpenGL library for win32") 44 | SET (OPENGL_glu_LIBRARY import32 CACHE STRING "GLU library for win32") 45 | ELSE(BORLAND) 46 | SET (OPENGL_gl_LIBRARY opengl32 CACHE STRING "OpenGL library for win32") 47 | SET (OPENGL_glu_LIBRARY glu32 CACHE STRING "GLU library for win32") 48 | ENDIF(BORLAND) 49 | 50 | ENDIF (CYGWIN) 51 | 52 | ELSE (WIN32) 53 | 54 | IF (APPLE) 55 | 56 | FIND_LIBRARY(OPENGL_gl_LIBRARY OpenGL DOC "OpenGL lib for OSX") 57 | FIND_LIBRARY(OPENGL_glu_LIBRARY AGL DOC "AGL lib for OSX") 58 | FIND_PATH(OPENGL_INCLUDE_DIR OpenGL/gl.h DOC "Include for OpenGL on OSX") 59 | 60 | ELSE(APPLE) 61 | # Handle HP-UX cases where we only want to find OpenGL in either hpux64 62 | # or hpux32 depending on if we're doing a 64 bit build. 63 | IF(CMAKE_SIZEOF_VOID_P EQUAL 4) 64 | SET(HPUX_IA_OPENGL_LIB_PATH /opt/graphics/OpenGL/lib/hpux32/) 65 | ELSE(CMAKE_SIZEOF_VOID_P EQUAL 4) 66 | SET(HPUX_IA_OPENGL_LIB_PATH 67 | /opt/graphics/OpenGL/lib/hpux64/ 68 | /opt/graphics/OpenGL/lib/pa20_64) 69 | ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 4) 70 | 71 | # The first line below is to make sure that the proper headers 72 | # are used on a Linux machine with the NVidia drivers installed. 73 | # They replace Mesa with NVidia's own library but normally do not 74 | # install headers and that causes the linking to 75 | # fail since the compiler finds the Mesa headers but NVidia's library. 76 | # Make sure the NVIDIA directory comes BEFORE the others. 77 | # - Atanas Georgiev 78 | 79 | FIND_PATH(OPENGL_INCLUDE_DIR GL/gl.h 80 | /usr/share/doc/NVIDIA_GLX-1.0/include 81 | /usr/openwin/share/include 82 | /opt/graphics/OpenGL/include /usr/X11R6/include 83 | ) 84 | 85 | FIND_PATH(OPENGL_xmesa_INCLUDE_DIR GL/xmesa.h 86 | /usr/share/doc/NVIDIA_GLX-1.0/include 87 | /usr/openwin/share/include 88 | /opt/graphics/OpenGL/include /usr/X11R6/include 89 | ) 90 | 91 | FIND_LIBRARY(OPENGL_gl_LIBRARY 92 | NAMES GL MesaGL 93 | PATHS /opt/graphics/OpenGL/lib 94 | /usr/openwin/lib 95 | /usr/shlib /usr/X11R6/lib 96 | ${HPUX_IA_OPENGL_LIB_PATH} 97 | ) 98 | 99 | # On Unix OpenGL most certainly always requires X11. 100 | # Feel free to tighten up these conditions if you don't 101 | # think this is always true. 102 | # It's not true on OSX. 103 | 104 | IF (OPENGL_gl_LIBRARY) 105 | IF(NOT X11_FOUND) 106 | INCLUDE(FindX11) 107 | ENDIF(NOT X11_FOUND) 108 | IF (X11_FOUND) 109 | IF (NOT APPLE) 110 | SET (OPENGL_LIBRARIES ${X11_LIBRARIES}) 111 | ENDIF (NOT APPLE) 112 | ENDIF (X11_FOUND) 113 | ENDIF (OPENGL_gl_LIBRARY) 114 | 115 | FIND_LIBRARY(OPENGL_glu_LIBRARY 116 | NAMES GLU MesaGLU 117 | PATHS ${OPENGL_gl_LIBRARY} 118 | /opt/graphics/OpenGL/lib 119 | /usr/openwin/lib 120 | /usr/shlib /usr/X11R6/lib 121 | ) 122 | 123 | ENDIF(APPLE) 124 | ENDIF (WIN32) 125 | 126 | IF(OPENGL_gl_LIBRARY) 127 | 128 | IF(OPENGL_xmesa_INCLUDE_DIR) 129 | SET( OPENGL_XMESA_FOUND "YES" ) 130 | ELSE(OPENGL_xmesa_INCLUDE_DIR) 131 | SET( OPENGL_XMESA_FOUND "NO" ) 132 | ENDIF(OPENGL_xmesa_INCLUDE_DIR) 133 | 134 | SET( OPENGL_LIBRARIES ${OPENGL_gl_LIBRARY} ${OPENGL_LIBRARIES}) 135 | IF(OPENGL_glu_LIBRARY) 136 | SET( OPENGL_GLU_FOUND "YES" ) 137 | SET( OPENGL_LIBRARIES ${OPENGL_glu_LIBRARY} ${OPENGL_LIBRARIES} ) 138 | ELSE(OPENGL_glu_LIBRARY) 139 | SET( OPENGL_GLU_FOUND "NO" ) 140 | ENDIF(OPENGL_glu_LIBRARY) 141 | 142 | # This deprecated setting is for backward compatibility with CMake1.4 143 | SET (OPENGL_LIBRARY ${OPENGL_LIBRARIES}) 144 | 145 | ENDIF(OPENGL_gl_LIBRARY) 146 | 147 | # This deprecated setting is for backward compatibility with CMake1.4 148 | SET(OPENGL_INCLUDE_PATH ${OPENGL_INCLUDE_DIR}) 149 | 150 | # handle the QUIETLY and REQUIRED arguments and set OPENGL_FOUND to TRUE if 151 | # all listed variables are TRUE 152 | INCLUDE(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) 153 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(OpenGL DEFAULT_MSG OPENGL_gl_LIBRARY) 154 | 155 | MARK_AS_ADVANCED( 156 | OPENGL_INCLUDE_DIR 157 | OPENGL_xmesa_INCLUDE_DIR 158 | OPENGL_glu_LIBRARY 159 | OPENGL_gl_LIBRARY 160 | ) 161 | -------------------------------------------------------------------------------- /cmake/find/FindOpenCV.cmake: -------------------------------------------------------------------------------- 1 | ########################################################### 2 | # Find OpenCV Library 3 | # See http://sourceforge.net/projects/opencvlibrary/ 4 | #---------------------------------------------------------- 5 | # 6 | ## 1: Setup: 7 | # The following variables are optionally searched for defaults 8 | # OpenCV_DIR: Base directory of OpenCv tree to use. 9 | # 10 | ## 2: Variable 11 | # The following are set after configuration is done: 12 | # 13 | # OpenCV_FOUND 14 | # OpenCV_LIBS 15 | # OpenCV_INCLUDE_DIR 16 | # OpenCV_VERSION (OpenCV_VERSION_MAJOR, OpenCV_VERSION_MINOR, OpenCV_VERSION_PATCH) 17 | # 18 | # 19 | # Deprecated variable are used to maintain backward compatibility with 20 | # the script of Jan Woetzel (2006/09): www.mip.informatik.uni-kiel.de/~jw 21 | # OpenCV_INCLUDE_DIRS 22 | # OpenCV_LIBRARIES 23 | # OpenCV_LINK_DIRECTORIES 24 | # 25 | ## 3: Version 26 | # 27 | # 2010/04/07 Benoit Rat, Correct a bug when OpenCVConfig.cmake is not found. 28 | # 2010/03/24 Benoit Rat, Add compatibility for when OpenCVConfig.cmake is not found. 29 | # 2010/03/22 Benoit Rat, Creation of the script. 30 | # 31 | # 32 | # tested with: 33 | # - OpenCV 2.1: MinGW, MSVC2008 34 | # - OpenCV 2.0: MinGW, MSVC2008, GCC4 35 | # 36 | # 37 | ## 4: Licence: 38 | # 39 | # LGPL 2.1 : GNU Lesser General Public License Usage 40 | # Alternatively, this file may be used under the terms of the GNU Lesser 41 | 42 | # General Public License version 2.1 as published by the Free Software 43 | # Foundation and appearing in the file LICENSE.LGPL included in the 44 | # packaging of this file. Please review the following information to 45 | # ensure the GNU Lesser General Public License version 2.1 requirements 46 | # will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html. 47 | # 48 | #---------------------------------------------------------- 49 | 50 | 51 | find_path(OpenCV_DIR "OpenCVConfig.cmake" DOC "Root directory of OpenCV") 52 | 53 | ### Nathan, my OpenCV_DIR on my mac should be: "/Users/erlebach/Documents/src/OpenCV-2.4.3" 54 | ### How to set this up in CMake? I don't have the time to figure this out. 55 | 56 | message("GE, OPENCV_DIR: ${OpenCV_DIR}") 57 | 58 | ##==================================================== 59 | ## Find OpenCV libraries 60 | ##---------------------------------------------------- 61 | if(EXISTS "${OpenCV_DIR}") 62 | 63 | #When its possible to use the Config script use it. 64 | if(EXISTS "${OpenCV_DIR}/OpenCVConfig.cmake") 65 | 66 | ## Include the standard CMake script 67 | include("${OpenCV_DIR}/OpenCVConfig.cmake") 68 | 69 | ## Search for a specific version 70 | set(CVLIB_SUFFIX "${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}") 71 | message("CVLIB_SUFFIX: ${CVLIB_SUFFIX}") 72 | 73 | #Otherwise it try to guess it. 74 | else(EXISTS "${OpenCV_DIR}/OpenCVConfig.cmake") 75 | 76 | set(OPENCV_LIB_COMPONENTS cxcore cv ml highgui cvaux) 77 | find_path(OpenCV_INCLUDE_DIR "cv.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "") 78 | if(EXISTS ${OpenCV_INCLUDE_DIR}) 79 | include_directories(${OpenCV_INCLUDE_DIR}) 80 | endif(EXISTS ${OpenCV_INCLUDE_DIR}) 81 | 82 | #Find OpenCV version by looking at cvver.h 83 | file(STRINGS ${OpenCV_INCLUDE_DIR}/cvver.h OpenCV_VERSIONS_TMP REGEX "^#define CV_[A-Z]+_VERSION[ \t]+[0-9]+$") 84 | string(REGEX REPLACE ".*#define CV_MAJOR_VERSION[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_MAJOR ${OpenCV_VERSIONS_TMP}) 85 | string(REGEX REPLACE ".*#define CV_MINOR_VERSION[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_MINOR ${OpenCV_VERSIONS_TMP}) 86 | string(REGEX REPLACE ".*#define CV_SUBMINOR_VERSION[ \t]+([0-9]+).*" "\\1" OpenCV_VERSION_PATCH ${OpenCV_VERSIONS_TMP}) 87 | set(OpenCV_VERSION ${OpenCV_VERSION_MAJOR}.${OpenCV_VERSION_MINOR}.${OpenCV_VERSION_PATCH} CACHE STRING "" FORCE) 88 | set(CVLIB_SUFFIX "${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}") 89 | 90 | endif(EXISTS "${OpenCV_DIR}/OpenCVConfig.cmake") 91 | 92 | 93 | 94 | 95 | ## Initiate the variable before the loop 96 | set(GLOBAL OpenCV_LIBS "") 97 | set(OpenCV_FOUND_TMP true) 98 | 99 | ## Loop over each components 100 | foreach(__CVLIB ${OPENCV_LIB_COMPONENTS}) 101 | 102 | find_library(OpenCV_${__CVLIB}_LIBRARY_DEBUG NAMES "${__CVLIB}${CVLIB_SUFFIX}d" "lib${__CVLIB}${CVLIB_SUFFIX}d" PATHS "${OpenCV_DIR}/lib" NO_DEFAULT_PATH) 103 | find_library(OpenCV_${__CVLIB}_LIBRARY_RELEASE NAMES "${__CVLIB}${CVLIB_SUFFIX}" "lib${__CVLIB}${CVLIB_SUFFIX}" PATHS "${OpenCV_DIR}/lib" NO_DEFAULT_PATH) 104 | 105 | #Remove the cache value 106 | set(OpenCV_${__CVLIB}_LIBRARY "" CACHE STRING "" FORCE) 107 | 108 | #both debug/release 109 | if(OpenCV_${__CVLIB}_LIBRARY_DEBUG AND OpenCV_${__CVLIB}_LIBRARY_RELEASE) 110 | set(OpenCV_${__CVLIB}_LIBRARY debug ${OpenCV_${__CVLIB}_LIBRARY_DEBUG} optimized ${OpenCV_${__CVLIB}_LIBRARY_RELEASE} CACHE STRING "" FORCE) 111 | #only debug 112 | elseif(OpenCV_${__CVLIB}_LIBRARY_DEBUG) 113 | set(OpenCV_${__CVLIB}_LIBRARY ${OpenCV_${__CVLIB}_LIBRARY_DEBUG} CACHE STRING "" FORCE) 114 | #only release 115 | elseif(OpenCV_${__CVLIB}_LIBRARY_RELEASE) 116 | set(OpenCV_${__CVLIB}_LIBRARY ${OpenCV_${__CVLIB}_LIBRARY_RELEASE} CACHE STRING "" FORCE) 117 | #no library found 118 | else() 119 | set(OpenCV_FOUND_TMP false) 120 | endif() 121 | 122 | #Add to the general list 123 | if(OpenCV_${__CVLIB}_LIBRARY) 124 | set(OpenCV_LIBS ${OpenCV_LIBS} ${OpenCV_${__CVLIB}_LIBRARY}) 125 | endif(OpenCV_${__CVLIB}_LIBRARY) 126 | 127 | endforeach(__CVLIB) 128 | 129 | 130 | set(OpenCV_FOUND ${OpenCV_FOUND_TMP} CACHE BOOL "" FORCE) 131 | 132 | 133 | else(EXISTS "${OpenCV_DIR}") 134 | set(ERR_MSG "Please specify OpenCV directory using OpenCV_DIR env. variable") 135 | endif(EXISTS "${OpenCV_DIR}") 136 | ##==================================================== 137 | 138 | 139 | ##==================================================== 140 | ## Print message 141 | ##---------------------------------------------------- 142 | if(NOT OpenCV_FOUND) 143 | # make FIND_PACKAGE friendly 144 | if(NOT OpenCV_FIND_QUIETLY) 145 | if(OpenCV_FIND_REQUIRED) 146 | message(FATAL_ERROR "OpenCV required but some headers or libs not found. ${ERR_MSG}") 147 | else(OpenCV_FIND_REQUIRED) 148 | message(STATUS "WARNING: OpenCV was not found. ${ERR_MSG}") 149 | endif(OpenCV_FIND_REQUIRED) 150 | endif(NOT OpenCV_FIND_QUIETLY) 151 | endif(NOT OpenCV_FOUND) 152 | ##==================================================== 153 | 154 | 155 | ##==================================================== 156 | ## Backward compatibility 157 | ##---------------------------------------------------- 158 | if(OpenCV_FOUND) 159 | option(OpenCV_BACKWARD_COMPA "Add some variable to make this script compatible with the other version of FindOpenCV.cmake" false) 160 | if(OpenCV_BACKWARD_COMPA) 161 | find_path(OpenCV_INCLUDE_DIRS "cv.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "Include directory") 162 | find_path(OpenCV_INCLUDE_DIR "cv.h" PATHS "${OpenCV_DIR}" PATH_SUFFIXES "include" "include/opencv" DOC "Include directory") 163 | set(OpenCV_LIBRARIES "${OpenCV_LIBS}" CACHE STRING "" FORCE) 164 | endif(OpenCV_BACKWARD_COMPA) 165 | endif(OpenCV_FOUND) 166 | ##==================================================== 167 | -------------------------------------------------------------------------------- /kinReg.cpp: -------------------------------------------------------------------------------- 1 | // ---- OpenGL ----- 2 | #include 3 | // ---- OpenCV ----- 4 | #include 5 | #include 6 | // -- libfreenect -- 7 | #include 8 | #include 9 | #include 10 | // --- C++ --- 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | /* 17 | * Kinect registration program 18 | * 19 | * All of this code has been commented for use with the Viz Lab at FSU. 20 | * Eventually this entire project will be refactored, modularized, and 21 | * compressed into something like two function calls getCorrespondences(), and 22 | * register() (currently depends on OpenCV. Should change this in the 23 | * future). All of the procrustes analysis should be in their own libraries 24 | * and the OpenGL visualization can just be an optional driver on top, 25 | * but if the registration is separated and just returns the transformations 26 | * then it can be more easily incorporated into other projects. 27 | */ 28 | 29 | using namespace cv; 30 | 31 | // Store the correspondences and centroids 32 | // Vec3f is an OpenCV data structure 33 | typedef std::pair< Vec3f, Vec3f > match; 34 | 35 | // Used for interactive transformations 36 | // see transformation() and KeyPressed() 37 | enum Transform_Mode{ rotation, translation, full_transform, none }; 38 | Transform_Mode transform_mode = none; 39 | 40 | // Variables for Calculations and Loops 41 | const int NUM_CAMS = 2; 42 | int GLwindow; 43 | int mx=-1,my=-1; // Prevous mouse coordinates 44 | int rotangles[2] = {0}; // Panning angles 45 | float zoom = 1; // zoom factor 46 | match centroids; 47 | Mat P, Q, trans, rot; // P and Q store the points for procrustes analysis 48 | vector< Vec3f > P_pts, Q_pts; // This is how they're collected 49 | // NOTE: Simplify correspondence containers above 50 | 51 | // This is used for collecting Correspondences. It keeps track of where the points were just collected from. 52 | enum Points{ P_POINTS, Q_POINTS, NONE }; 53 | Points POINTS = NONE; 54 | 55 | // Window Size and Position 56 | const int window_width = 640, window_height = 480; 57 | int window_xpos = 1000, window_ypos = 100; 58 | 59 | // OpenGL Callback Functions 60 | void cbRender(); 61 | void cbReSizeGLScene( int Width, int Height); 62 | void cbMouseMoved( int x, int y); 63 | void cbMousePress( int button, int state, int x, int y); 64 | void cbTimer( int ms ); 65 | void cbKeyPressed( unsigned char key, int x, int y); 66 | 67 | // OpenCV Callback Functions 68 | void cbMouseEvent( int event, int x, int y, int flags, void* param ); 69 | 70 | // Handy functions to call in the render function 71 | void transformation( int cam ); // This applies the procrustes transformations 72 | void loadVertexMatrix(); // This applies the projection transformation 73 | void noKinectQuit(); 74 | void draw_axes(); 75 | void draw_line(Vec3b v1, Vec3b v2); 76 | 77 | // Computer Vision functions 78 | void displayCVcams(); // Calls joinFrames and displays the RGB images 79 | Mat joinFrames( const Mat& img1, const Mat& img2 ); // Puts images side by side 80 | Mat convert_vector2Mat( const vector< Vec3f > vec ); 81 | Vec3f transformPoint( const Vec3f& pt ); // Transforms pt from image space 82 | // to Kinect space 83 | match calculateCentroids( const Mat& verts1, const Mat& verts2 ); 84 | void procrustes( const vector< Vec3f >&, const vector< Vec3f >&, Mat&, Mat& ); 85 | 86 | // Collects the information from a (cameraIndx) Kinect 87 | void loadBuffers( int cameraIndx, 88 | unsigned int indices[window_height][window_width], 89 | short xyz[window_height][window_width][3], 90 | unsigned char rgb[window_height][window_width][3] ); 91 | 92 | // getDepth (poorly) attempts to ameliorate the bad depth measurements 93 | // by checking the neighbors in a 3x3 grid around a pixel which got a 94 | // depth measurement > 2047 95 | float getDepth( int cam, int x, int y ); 96 | void printMat( const Mat& A ); 97 | 98 | // Store the matrices from all cameras here 99 | vector rgbCV; 100 | vector depthCV; 101 | 102 | int main( int argc, char** argv ) { 103 | 104 | // load the first frames (OpenCV gets upset otherwise) 105 | for( int cam = 0; cam < NUM_CAMS; cam++ ) { 106 | rgbCV.push_back( freenect_sync_get_rgb_cv(cam) ); 107 | depthCV.push_back( freenect_sync_get_depth_cv(cam) ); 108 | } 109 | 110 | // Initialize Display Mode 111 | glutInit( &argc, argv ); 112 | glutInitDisplayMode( GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH ); 113 | 114 | // Initialize OpenGL Window 115 | glutInitWindowSize( window_width, window_height ); 116 | glutInitWindowPosition( window_xpos, window_ypos ); 117 | GLwindow = glutCreateWindow("Kinect Registration"); 118 | glClearColor( 0.0f, 0.0f, 0.0f, 0.0f ); 119 | 120 | // Initialize OpenCV Window 121 | namedWindow( "Camera 0 | Camera 1", CV_WINDOW_AUTOSIZE ); 122 | 123 | // Setup The GL Callbacks 124 | glutDisplayFunc( cbRender ); 125 | glutReshapeFunc( cbReSizeGLScene ); 126 | glutKeyboardFunc( cbKeyPressed ); 127 | glutMotionFunc( cbMouseMoved ); 128 | glutMouseFunc( cbMousePress ); 129 | glutTimerFunc( 10, cbTimer, 10 ); 130 | 131 | // Setup The CV Callbacks 132 | cvSetMouseCallback( "Camera 0 | Camera 1", cbMouseEvent ); 133 | 134 | glutMainLoop(); 135 | 136 | return 0; 137 | } 138 | 139 | void cbRender() { 140 | short xyz[window_height][window_width][3]; 141 | unsigned char rgb[window_height][window_width][3]; 142 | unsigned int indices[window_height][window_width]; 143 | 144 | // Flush the OpenCV Mat's from last frame 145 | rgbCV.clear(); 146 | depthCV.clear(); 147 | glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); 148 | glEnable( GL_DEPTH_TEST ); 149 | glPushMatrix(); 150 | glScalef( zoom,zoom,1 ); 151 | gluLookAt( 0, 0, 3.5, 0, 0, 0, 0, 1.0, 0 ); 152 | glRotatef( rotangles[0], 1,0,0 ); 153 | glRotatef( rotangles[1], 0,1,0 ); 154 | draw_axes(); 155 | 156 | glEnableClientState( GL_VERTEX_ARRAY ); 157 | glEnableClientState( GL_COLOR_ARRAY ); 158 | glPointSize( 2 ); 159 | //--------Camera 0 (P)----------- 160 | loadBuffers( 0, indices, xyz, rgb ); 161 | glVertexPointer( 3, GL_SHORT, 0, xyz ); 162 | glColorPointer( 3, GL_UNSIGNED_BYTE, 0, rgb ); 163 | glPushMatrix(); 164 | // transform centroid of P to origin and rotate 165 | transformation( 0 ); 166 | // projection matrix (camera specific - Can be improved) 167 | loadVertexMatrix(); 168 | glDrawArrays(GL_POINTS, 0, window_width*window_height); 169 | glPopMatrix(); 170 | //--------Camera 1 (Q)----------- 171 | loadBuffers( 1, indices, xyz, rgb ); 172 | glVertexPointer( 3, GL_SHORT, 0, xyz ); 173 | glColorPointer( 3, GL_UNSIGNED_BYTE, 0, rgb ); 174 | glPushMatrix(); 175 | // translate centroid of Q to origin 176 | transformation( 1 ); 177 | loadVertexMatrix(); 178 | glDrawArrays( GL_POINTS, 0, window_width*window_height ); 179 | glPopMatrix(); 180 | glPopMatrix(); 181 | 182 | displayCVcams(); 183 | glFlush(); 184 | glutSwapBuffers(); 185 | glDisable( GL_DEPTH_TEST ); 186 | } 187 | 188 | void cbKeyPressed( unsigned char key, int x, int y ) { 189 | 190 | // Press esc to exit 191 | if ( key == 27 ) { 192 | glutDestroyWindow( GLwindow ); 193 | exit( 0 ); 194 | } 195 | else if( key == 'p' ) { 196 | procrustes( P_pts, Q_pts, trans, rot ); 197 | // The correspondence arrays are flushed after the transformations 198 | // are calculated. Just in case the registration attempt is poor 199 | // and you want to try again, you can just start collecting 200 | // correspondences again without restarting the program 201 | P_pts.clear(); 202 | Q_pts.clear(); 203 | } 204 | else if( key == 'r' ) 205 | transform_mode = rotation; 206 | else if( key == 't' ) 207 | transform_mode = translation; 208 | else if( key == 'a' ) 209 | transform_mode = full_transform; 210 | else if( key == 'n' ) 211 | transform_mode = none; 212 | else if ( key == 'z' ) 213 | zoom *= 1.1f; 214 | else if ( key == 'x' ) 215 | zoom /= 1.1f; 216 | 217 | } 218 | 219 | void cbMouseMoved( int x, int y) { 220 | 221 | if ( mx>=0 && my>=0) { 222 | rotangles[0] += y-my; 223 | rotangles[1] += x-mx; 224 | } 225 | 226 | mx = x; 227 | my = y; 228 | 229 | } 230 | 231 | void cbMousePress( int button, int state, int x, int y) { 232 | 233 | if ( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) { 234 | mx = x; 235 | my = y; 236 | } 237 | 238 | if ( button == GLUT_LEFT_BUTTON && state == GLUT_UP) { 239 | mx = -1; 240 | my = -1; 241 | } 242 | 243 | } 244 | 245 | // This ensures that OpenGL and OpenCV play nicely together 246 | // ms needs to be the same time as OpenCV's waitKey( ms ) 247 | void cbTimer( int ms ) { 248 | 249 | glutTimerFunc( ms, cbTimer, ms ); 250 | glutPostRedisplay(); 251 | 252 | } 253 | 254 | void cbReSizeGLScene( int Width, int Height) { 255 | 256 | glViewport( 0,0,Width,Height ); 257 | glMatrixMode( GL_PROJECTION ); 258 | glLoadIdentity(); 259 | gluPerspective( 60, 4/3., 0.3, 200 ); 260 | glMatrixMode( GL_MODELVIEW ); 261 | } 262 | 263 | void noKinectQuit() { 264 | printf( "Error: Kinect not connected?\n" ); 265 | exit( 1 ); 266 | } 267 | 268 | void loadBuffers( int cameraIndx, 269 | unsigned int indices[window_height][window_width], 270 | short xyz[window_height][window_width][3], 271 | unsigned char rgb[window_height][window_width][3] ) { 272 | 273 | rgbCV.push_back( freenect_sync_get_rgb_cv(cameraIndx) ); 274 | depthCV.push_back( freenect_sync_get_depth_cv(cameraIndx) ); 275 | 276 | if( rgbCV[cameraIndx].empty() || depthCV[cameraIndx].empty() ) 277 | noKinectQuit(); 278 | 279 | for ( int row = 0; row < window_height; row++ ) { 280 | for ( int col = 0; col < window_width; col++ ) { 281 | xyz[row][col][0] = col; 282 | xyz[row][col][1] = row; 283 | xyz[row][col][2] = getDepth( cameraIndx, row, col ); 284 | indices[row][col] = (window_height + row)*window_width + col; 285 | Vec3b color = rgbCV[cameraIndx].at(row,col); 286 | rgb[row][col][0] = color[0]; 287 | rgb[row][col][1] = color[1]; 288 | rgb[row][col][2] = color[2]; 289 | } 290 | } 291 | } 292 | 293 | // Do the projection from u,v,depth to X,Y,Z directly in an opengl matrix 294 | // These numbers come from a combination of the ros kinect_node wiki, and 295 | // nicolas burrus' posts. 296 | void loadVertexMatrix() { 297 | 298 | float fx = 594.21f; 299 | float fy = 591.04f; 300 | float a = -0.0030711f; 301 | float b = 3.3309495f; 302 | float cx = 339.5f; 303 | float cy = 242.7f; 304 | GLfloat mat[16] = { 305 | 1/fx, 0, 0, 0, 306 | 0, -1/fy, 0, 0, 307 | 0, 0, 0, a, 308 | -cx/fx, cy/fy, -1, b 309 | }; 310 | glMultMatrixf( mat); 311 | } 312 | 313 | void printMat( const Mat& A ) { 314 | 315 | printf("| "); 316 | for(int i = 0; i < A.rows; i++) { 317 | if(i > 0 && i < A.rows) { 318 | printf("|\n"); 319 | printf("| "); 320 | } 321 | for(int j = 0; j < A.cols; j++) { 322 | printf( "%f ", A.at(i,j) ); 323 | } 324 | } 325 | printf("|\n"); 326 | } 327 | 328 | // Definitely need to come up with a better 329 | // way to find good depth measurements... 330 | float getDepth( int cam, int x, int y ) { 331 | 332 | float d = (float)(depthCV[cam].at(x,y)); 333 | 334 | if( d >= 2047 ) 335 | d = (float)(depthCV[cam].at(x,y+1)); 336 | 337 | if( d >= 2047 ) 338 | d = (float)(depthCV[cam].at(x,y-1)); 339 | 340 | if( d >= 2047 ) 341 | d = (float)(depthCV[cam].at(x+1,y)); 342 | 343 | if( d >= 2047 ) 344 | d = (float)(depthCV[cam].at(x-1,y)); 345 | 346 | if( d >= 2047 ) 347 | d = (float)(depthCV[cam].at(x-1,y-1)); 348 | 349 | if( d >= 2047 ) 350 | d = (float)(depthCV[cam].at(x-1,y+1)); 351 | 352 | if( d >= 2047 ) 353 | d = (float)(depthCV[cam].at(x+1,y-1)); 354 | 355 | if( d >= 2047 ) 356 | d = (float)(depthCV[cam].at(x+1,y+1)); 357 | 358 | return d; 359 | } 360 | 361 | void draw_line( Vec3b v1, Vec3b v2) { 362 | 363 | glBegin(GL_LINES); 364 | glVertex3f(v1[0], v1[1], v1[2]); 365 | glVertex3f(v2[0], v2[1], v2[2]); 366 | glEnd(); 367 | 368 | } 369 | 370 | void draw_axes() { 371 | 372 | //X Axis 373 | 374 | glColor3f(1,0,0); //red 375 | Vec3b r1(0,0,0); 376 | Vec3b r2(1,0,0); 377 | draw_line(r1, r2); 378 | 379 | //Y Axis 380 | 381 | glColor3f(0,1,0); //green 382 | Vec3b g1(0,0,0); 383 | Vec3b g2(0,1,0); 384 | draw_line(g1, g2); 385 | 386 | //Z Axis 387 | 388 | glColor3f(0,0,1); //blue 389 | Vec3b b1(0,0,0); 390 | Vec3b b2(0,0,1); 391 | draw_line(b1, b2); 392 | 393 | } 394 | 395 | // Puts two RGB images side by side in one Mat 396 | Mat joinFrames( const Mat& img1, const Mat& img2 ) { 397 | 398 | Mat rslt = Mat::zeros( img1.rows, img1.cols*2, img1.type()); 399 | for( int i = 0; i < img1.rows; i++ ) 400 | for( int j = 0; j < img1.cols; j++ ) { 401 | Vec3b p = img1.at( i,j); 402 | Vec3b q = img2.at(i,j); 403 | rslt.at(i,j) = p; 404 | rslt.at(i,j+window_width) = q; 405 | } 406 | 407 | return rslt; 408 | } 409 | 410 | // Display the joined frames in one window 411 | void displayCVcams() { 412 | 413 | Mat tmp = Mat::zeros( window_width, window_height, CV_8U ); 414 | for( int cam = 0; cam < NUM_CAMS; cam++ ) { 415 | cvtColor( rgbCV[cam], tmp, CV_RGB2BGR ); 416 | rgbCV[cam] = tmp.clone(); 417 | } 418 | 419 | Mat rgb = joinFrames( rgbCV[0], rgbCV[1] ); 420 | imshow( "Camera 0 | Camera 1", rgb ); 421 | 422 | // Time here needs to be the same as cbTimer 423 | // returns -1 if no key pressed 424 | char key = waitKey( 10 ); 425 | 426 | // If someone presses a button while a cv window 427 | // is in the foreground we want the behavior to 428 | // be the same as for the OpenGL window, so call 429 | // OpenGL's cbKeyPressed callback function 430 | if( key != -1 ) 431 | cbKeyPressed( key, 0, 0 ); 432 | 433 | } 434 | 435 | match calculateCentroids( const Mat& verts1, const Mat& verts2 ) { 436 | 437 | Vec3f centroid1(0,0,0), centroid2(0,0,0); 438 | 439 | for( int i = 0; i < verts1.cols; i++ ) { 440 | 441 | // Sum of components of first point cloud 442 | centroid1[0] += verts1.at(0,i); 443 | centroid1[1] += verts1.at(1,i); 444 | centroid1[2] += verts1.at(2,i); 445 | 446 | // Sum of components of second point cloud 447 | centroid2[0] += verts2.at(0,i); 448 | centroid2[1] += verts2.at(1,i); 449 | centroid2[2] += verts2.at(2,i); 450 | } 451 | 452 | // First centroid 453 | centroid1[0] /= verts1.cols; 454 | centroid1[1] /= verts1.cols; 455 | centroid1[2] /= verts1.cols; 456 | printf(" First centroid = (%f,%f,%f)\n", centroid1[0], centroid1[1], centroid1[2] ); 457 | 458 | // Second centroid 459 | centroid2[0] /= verts1.cols; 460 | centroid2[1] /= verts1.cols; 461 | centroid2[2] /= verts1.cols; 462 | printf(" Second centroid = (%f,%f,%f)\n", centroid2[0], centroid2[1], centroid2[2] ); 463 | 464 | match centroids( centroid1, centroid2 ); 465 | 466 | return centroids; 467 | } 468 | 469 | void procrustes( const vector< Vec3f >& P_pts, 470 | const vector< Vec3f >& Q_pts, 471 | Mat& trans, Mat& rot ) { 472 | printf("\n\n------------ENTERING procrustes()------------\n"); 473 | 474 | if( P_pts.size() == 0 || Q_pts.size() == 0 ) { 475 | printf("There are no correspondences for Registration...\n"); 476 | return; 477 | } 478 | 479 | P = convert_vector2Mat( P_pts ); 480 | printf("P:\n"); 481 | printMat( P ); 482 | Q = convert_vector2Mat( Q_pts ); 483 | printf("Q:\n"); 484 | printMat( Q ); 485 | centroids = calculateCentroids( P, Q ); 486 | 487 | // To calculate the rotation we assume the centroids of their 488 | // correspondences are aligned. So move the centroids of each 489 | // collection of correspondences to the origin. 490 | for( int pt = 0; pt < P.cols; pt++ ) { 491 | P.at(0,pt) -= centroids.first[0]; 492 | P.at(1,pt) -= centroids.first[1]; 493 | P.at(2,pt) -= centroids.first[2]; 494 | } 495 | for( int pt = 0; pt < Q.cols; pt++ ) { 496 | Q.at(0,pt) -= centroids.second[0]; 497 | Q.at(1,pt) -= centroids.second[1]; 498 | Q.at(2,pt) -= centroids.second[2]; 499 | } 500 | // Procrustes in 3 lines baby 501 | Mat PQt = P*Q.t(); 502 | SVD svd( PQt ); 503 | Mat rotMat = svd.u*svd.vt; 504 | 505 | // change the rotMat to column major (OpenGL stores matrices in column 506 | // major order, while OpenCV row major, and add the extra column and row) 507 | float rotCpy[16] = { rotMat.at(0,0), rotMat.at(0,1), rotMat.at(0,2), 0, 508 | rotMat.at(1,0), rotMat.at(1,1), rotMat.at(1,2), 0, 509 | rotMat.at(2,0), rotMat.at(2,1), rotMat.at(2,2), 0, 510 | 0, 0, 0, 1 }; 511 | rot = Mat( 4, 4, CV_32F, rotCpy ).clone(); 512 | printf(" rot:\n"); 513 | printMat( rot ); 514 | 515 | printf("\n\n------------LEAVING procrustes()------------\n"); 516 | } 517 | 518 | // Apply certain transformations to each camera 519 | void transformation( int cam ) { 520 | 521 | if( transform_mode == none ) 522 | return; 523 | else if( transform_mode == rotation && cam == 0 ) { 524 | glMultMatrixf( (GLfloat*)rot.data ); 525 | } 526 | else if( transform_mode == translation && cam == 0 ) { 527 | glTranslatef( -centroids.first[0], -centroids.first[1], -centroids.first[2] ); 528 | } 529 | else if( transform_mode == translation && cam == 1 ) { 530 | glTranslatef( -centroids.second[0], -centroids.second[1], -centroids.second[2] ); 531 | } 532 | else if( transform_mode == full_transform && cam == 0 ) { 533 | glMultMatrixf( (GLfloat*)rot.data ); 534 | glTranslatef( -centroids.first[0], -centroids.first[1], -centroids.first[2] ); 535 | } 536 | else if( transform_mode == full_transform && cam == 1 ) { 537 | glTranslatef( -centroids.second[0], -centroids.second[1], -centroids.second[2] ); 538 | } 539 | 540 | } 541 | 542 | // Callback function 543 | void cbMouseEvent( int event, int col, int row, int flags, void* param ) { 544 | 545 | switch( event ) { 546 | case CV_EVENT_LBUTTONDOWN: 547 | if( col <= 640 ) { // This is how it know which points you're 548 | // collecting (images side by side) 549 | printf(" Click in P ( %d, %d, %f )\n", col, row, getDepth(0,row,col) ); 550 | if( POINTS == NONE ) { 551 | // If first click, grab the point and 552 | P_pts.push_back( Vec3f( col, row, getDepth(0,row,col) ) ); 553 | // state that the last point grabbed was a point in P 554 | POINTS = P_POINTS; 555 | break; 556 | } 557 | if( POINTS == P_POINTS ) { 558 | printf("Can't match to the same image! Erasing previous point...\n"); 559 | P_pts.pop_back(); 560 | POINTS = NONE; 561 | break; 562 | } 563 | // If not NONE and not P_POINTS must be Q_POINTS 564 | P_pts.push_back( Vec3f( col, row, getDepth(0,row,col) ) ); 565 | float Pz = P_pts.back()[2]; 566 | float Qz = Q_pts.back()[2]; 567 | // Check if the depth measurements are bad, if so remove them 568 | if( Pz >= 2047 || Pz <= 0 || Qz >= 2047 || Qz <= 0 ) { 569 | printf( "\nDepths are bad! Erasing correspondence...\n"); 570 | P_pts.pop_back(); 571 | Q_pts.pop_back(); 572 | POINTS = NONE; 573 | break; 574 | } 575 | else { 576 | // If the depths are good, transform the points and store them! 577 | Vec3f transP_pt = transformPoint( P_pts.back() ); 578 | P_pts.pop_back(); 579 | P_pts.push_back( transP_pt ); 580 | Vec3f transQ_pt = transformPoint( Q_pts.back() ); 581 | Q_pts.pop_back(); 582 | Q_pts.push_back( transQ_pt ); 583 | printf(" P_pts: \n" ); 584 | for( int i = 0; i < (int)P_pts.size(); i++ ) 585 | printf( "%d - (%f,%f,%f)\n",i,P_pts[i][0],P_pts[i][1],P_pts[i][2] ); 586 | printf(" Q_pts: \n" ); 587 | for( int i = 0; i < (int)Q_pts.size(); i++ ) 588 | printf( "%d - (%f,%f,%f)\n",i,Q_pts[i][0],Q_pts[i][1],Q_pts[i][2] ); 589 | } 590 | POINTS = NONE; 591 | } 592 | else { 593 | // SAME AS ABOVE JUST FOR OTHER SIDE OF WINDOW ( Q POINTS ) 594 | printf(" Click in Q ( %d, %d, %f )\n", col-640, row, getDepth(1,row,col-640) ); 595 | if( POINTS == NONE ) { 596 | Q_pts.push_back( Vec3f( col-640, row, getDepth(1,row,col-640) ) ); 597 | POINTS = Q_POINTS; 598 | break; 599 | } 600 | if( POINTS == Q_POINTS ) { 601 | printf("Can't match to the same image! Erasing previous point...\n"); 602 | Q_pts.pop_back(); 603 | POINTS = NONE; 604 | break; 605 | } 606 | Q_pts.push_back( Vec3f( col-640, row, getDepth(1,row,col-640) ) ); 607 | float Pz = P_pts.back()[2]; 608 | float Qz = Q_pts.back()[2]; 609 | if( Pz >= 2047 || Pz <= 0 || Qz >= 2047 || Qz <= 0 ) { 610 | P_pts.pop_back(); 611 | Q_pts.pop_back(); 612 | printf( "\nDepths are bad! Erasing correspondence...\n"); 613 | POINTS = NONE; 614 | break; 615 | } 616 | else { 617 | Vec3f transP_pt = transformPoint( P_pts.back() ); 618 | P_pts.pop_back(); 619 | P_pts.push_back( transP_pt ); 620 | Vec3f transQ_pt = transformPoint( Q_pts.back() ); 621 | Q_pts.pop_back(); 622 | Q_pts.push_back( transQ_pt ); 623 | printf("\n P_pts: \n" ); 624 | for( int i = 0; i < (int)P_pts.size(); i++ ) 625 | printf( "%d - (%f,%f,%f)\n",i,P_pts[i][0],P_pts[i][1],P_pts[i][2] ); 626 | printf(" Q_pts: \n" ); 627 | for( int i = 0; i < (int)Q_pts.size(); i++ ) 628 | printf( "%d - (%f,%f,%f)\n",i,Q_pts[i][0],Q_pts[i][1],Q_pts[i][2] ); 629 | } 630 | 631 | POINTS = NONE; 632 | } 633 | } 634 | } 635 | 636 | Mat convert_vector2Mat( const vector< Vec3f > vec ) { 637 | 638 | printf("\n\n---------- ENTERING convert_vector2Mat() ----------\n "); 639 | printf(" Incoming vector \n"); 640 | for( int i = 0; i < (int)vec.size(); i++ ) 641 | printf( " vec[%d] - ( %f, %f, %f )\n",i,vec[i][0],vec[i][1],vec[i][2] ); 642 | 643 | Mat m( 3, vec.size(), CV_32F ); 644 | 645 | for( int i = 0; i < (int)vec.size(); i++ ) { 646 | m.at(0,i) = vec[i][0]; 647 | m.at(1,i) = vec[i][1]; 648 | m.at(2,i) = vec[i][2]; 649 | } 650 | 651 | printf(" Mat version of vec:\n"); 652 | printMat( m ); 653 | 654 | printf("--------- LEAVING convert_vector2Mat() ---------\n\n"); 655 | return m; 656 | 657 | } 658 | 659 | Vec3f transformPoint( const Vec3f& pt ) { 660 | 661 | printf("\n\n----- ENTERING transformPoint() -------\n"); 662 | printf(" Incoming Point = ( %f, %f, %f )\n", pt[0], pt[1], pt[2] ); 663 | Mat point( pt ); 664 | point.resize( 4 ); 665 | point.at(0,3) = 1; 666 | printf(" Mat version of point\n"); 667 | printMat( point ); 668 | 669 | float fx = 594.21f; 670 | float fy = 591.04f; 671 | float a = -0.0030711f; 672 | float b = 3.3309495f; 673 | float cx = 339.5f; 674 | float cy = 242.7f; 675 | float data[16] = { 676 | 1/fx, 0, 0, -cx/fx, 677 | 0, -1/fy, 0, cy/fy, 678 | 0, 0, 0, -1, 679 | 0, 0, a, b 680 | }; 681 | 682 | Mat transform( 4, 4, CV_32F, data ); 683 | printf("Transformation Matrix:\n"); 684 | printMat( transform ); 685 | Mat tmp = transform*point; 686 | printf("Transformation Matrx * Point:\n"); 687 | printMat( tmp ); 688 | float w = tmp.at(0,3); 689 | float x = tmp.at(0,0) / w; 690 | float y = tmp.at(0,1) / w; 691 | float z = tmp.at(0,2) / w; 692 | Vec3f transformedPoint( x, y, z ); 693 | printf(" Perspective Division = ( %f, %f, %f )", transformedPoint[0], 694 | transformedPoint[1], 695 | transformedPoint[2] ); 696 | 697 | printf("\n----- LEAVING transformPoint() -------\n\n"); 698 | return transformedPoint; 699 | 700 | float flann_knn(Mat& m_destinations, Mat& m_object, vector& ptpairs, vector& dists = vector()) { 701 | // find nearest neighbors using FLANN 702 | Mat m_indices(m_object.rows, 1, CV_32S); 703 | Mat m_dists(m_object.rows, 1, CV_32F); 704 | 705 | Mat dest_32f; m_destinations.convertTo(dest_32f,CV_32FC2); 706 | Mat obj_32f; m_object.convertTo(obj_32f,CV_32FC2); 707 | 708 | assert(dest_32f.type() == CV_32F); 709 | 710 | cv::flann::Index flann_index(dest_32f, cv::flann::KDTreeIndexParams(2)); // using 2 randomized kdtrees 711 | flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, cv::flann::SearchParams(64) ); 712 | 713 | int* indices_ptr = m_indices.ptr(0); 714 | //float* dists_ptr = m_dists.ptr(0); 715 | for (int i=0;i(0); 752 | Scalar T(d_bar[0] - (m_bar[0]*_R[0] + m_bar[1]*_R[1]),d_bar[1] - (m_bar[0]*_R[2] + m_bar[1]*_R[3])); 753 | 754 | m = m.reshape(1); 755 | m = m * R; 756 | m = m.reshape(2); 757 | m = m + T;// + m_bar; 758 | m.convertTo(_m,CV_32S); 759 | } 760 | 761 | void icp() { 762 | vector pair; 763 | vector dists; 764 | 765 | while(true) { 766 | pair.clear(); dists.clear(); 767 | double dist = flann_knn(destination, X, pair, dists); 768 | 769 | if(lastDist <= dist) { 770 | X = lastGood; 771 | break; //converged? 772 | } 773 | lastDist = dist; 774 | X.copyTo(lastGood); 775 | 776 | cout << "distance: " << dist << endl; 777 | 778 | Mat X_bar(X.size(),X.type()); 779 | for(int i=0;i(pair[i],0); 781 | X_bar.at(i,0) = p; 782 | } 783 | 784 | ShowQuery(destination,X,X_bar); 785 | 786 | X = X.reshape(2); 787 | X_bar = X_bar.reshape(2); 788 | findBestReansformSVD(X,X_bar); 789 | X = X.reshape(1); // back to 1-channel 790 | } 791 | } 792 | --------------------------------------------------------------------------------