├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake_uninstall.cmake.in └── src ├── SL_PtrVec.h ├── SL_error.cpp ├── SL_error.h ├── calibration ├── SL_CalibTwoCam.cpp └── SL_CalibTwoCam.h ├── extern ├── arpack │ ├── README │ ├── arbgcomp.h │ ├── arbgnsym.h │ ├── arbgsym.h │ ├── arbnsmat.h │ ├── arbnspen.h │ ├── arbscomp.h │ ├── arbsmat.h │ ├── arbsnsym.h │ ├── arbspen.h │ ├── arbssym.h │ ├── arch.h │ ├── arcomp.h │ ├── ardfmat.h │ ├── ardgcomp.h │ ├── ardgnsym.h │ ├── ardgsym.h │ ├── ardnsmat.h │ ├── ardnspen.h │ ├── ardscomp.h │ ├── ardsmat.h │ ├── ardsnsym.h │ ├── ardspen.h │ ├── ardssym.h │ ├── areig.h │ ├── arerror.h │ ├── argcomp.h │ ├── argeig.h │ ├── argnsym.h │ ├── argsym.h │ ├── arhbmat.h │ ├── arlcomp.h │ ├── arlgcomp.h │ ├── arlgnsym.h │ ├── arlgsym.h │ ├── arlnames.h │ ├── arlnsmat.h │ ├── arlnspen.h │ ├── arlscomp.h │ ├── arlsmat.h │ ├── arlsnsym.h │ ├── arlspdef.h │ ├── arlspen.h │ ├── arlssym.h │ ├── arlsupm.h │ ├── arlutil.h │ ├── armat.h │ ├── arpackf.h │ ├── arrgcomp.h │ ├── arrgeig.h │ ├── arrgnsym.h │ ├── arrgsym.h │ ├── arrscomp.h │ ├── arrseig.h │ ├── arrsnsym.h │ ├── arrssym.h │ ├── arscomp.h │ ├── arseig.h │ ├── arsnsym.h │ ├── arssym.h │ ├── arugcomp.h │ ├── arugnsym.h │ ├── arugsym.h │ ├── arunsmat.h │ ├── arunspen.h │ ├── aruscomp.h │ ├── arusmat.h │ ├── arusnsym.h │ ├── aruspen.h │ ├── arussym.h │ ├── blas1c.h │ ├── blas1f.h │ ├── caupp.h │ ├── ceupp.h │ ├── debug.h │ ├── lapackc.h │ ├── lapackf.h │ ├── naupp.h │ ├── neupp.h │ ├── saupp.h │ ├── seupp.h │ ├── superluc.h │ ├── umfpackc.h │ └── umfpackf.h ├── csparse │ ├── README.txt │ ├── cs.h │ ├── cs_add.c │ ├── cs_amd.c │ ├── cs_chol.c │ ├── cs_cholsol.c │ ├── cs_compress.c │ ├── cs_counts.c │ ├── cs_cumsum.c │ ├── cs_dfs.c │ ├── cs_dmperm.c │ ├── cs_droptol.c │ ├── cs_dropzeros.c │ ├── cs_dupl.c │ ├── cs_entry.c │ ├── cs_ereach.c │ ├── cs_etree.c │ ├── cs_fkeep.c │ ├── cs_gaxpy.c │ ├── cs_happly.c │ ├── cs_house.c │ ├── cs_ipvec.c │ ├── cs_leaf.c │ ├── cs_load.c │ ├── cs_lsolve.c │ ├── cs_ltsolve.c │ ├── cs_lu.c │ ├── cs_lusol.c │ ├── cs_malloc.c │ ├── cs_maxtrans.c │ ├── cs_multiply.c │ ├── cs_norm.c │ ├── cs_permute.c │ ├── cs_pinv.c │ ├── cs_post.c │ ├── cs_print.c │ ├── cs_pvec.c │ ├── cs_qr.c │ ├── cs_qrsol.c │ ├── cs_randperm.c │ ├── cs_reach.c │ ├── cs_scatter.c │ ├── cs_scc.c │ ├── cs_schol.c │ ├── cs_spsolve.c │ ├── cs_sqr.c │ ├── cs_symperm.c │ ├── cs_tdfs.c │ ├── cs_transpose.c │ ├── cs_updown.c │ ├── cs_usolve.c │ ├── cs_util.c │ └── cs_utsolve.c ├── lapack.h ├── levmar │ ├── Axb.c │ ├── Axb_core.c │ ├── compiler.h │ ├── levmar.h │ ├── lm.c │ ├── lm.h │ ├── lm_core.c │ ├── lmbc.c │ ├── lmbc_core.c │ ├── lmblec.c │ ├── lmblec_core.c │ ├── lmbleic.c │ ├── lmbleic_core.c │ ├── lmlec.c │ ├── lmlec_core.c │ ├── misc.c │ ├── misc.h │ └── misc_core.c └── sba-1.6 │ ├── compiler.h │ ├── sba.h │ ├── sba_chkjac.cpp │ ├── sba_chkjac.h │ ├── sba_crsm.cpp │ ├── sba_lapack.cpp │ ├── sba_levmar.cpp │ └── sba_levmar_wrap.cpp ├── geometry ├── SL_5point.cpp ├── SL_5point.h ├── SL_8point.cpp ├── SL_8point.h ├── SL_AbsoluteOrientation.cpp ├── SL_AbsoluteOrientation.h ├── SL_BundleAdjust.cpp ├── SL_BundleAdjust.h ├── SL_BundleHelper.cpp ├── SL_BundleHelper.h ├── SL_ConvexHull2D.cpp ├── SL_ConvexHull2D.h ├── SL_Depth.cpp ├── SL_Depth.h ├── SL_Distortion.cpp ├── SL_Distortion.h ├── SL_EMatWrapper.cpp ├── SL_EMatWrapper.h ├── SL_FundamentalMatrix.cpp ├── SL_FundamentalMatrix.h ├── SL_Geometry.cpp ├── SL_Geometry.h ├── SL_Point.h ├── SL_PoseEst.cpp ├── SL_PoseEst.h ├── SL_Quaternion.cpp ├── SL_Quaternion.h ├── SL_RigidTransform.cpp ├── SL_RigidTransform.h ├── SL_Triangulate.cpp ├── SL_Triangulate.h ├── SL_p3p.cpp ├── SL_p3p.h ├── epnp.cpp ├── epnp.h ├── quartSolver.cpp └── quartSolver.h ├── imgproc ├── SL_BoxFilter.cpp ├── SL_BoxFilter.h ├── SL_FloatImage.h ├── SL_Gradient.h ├── SL_GuidedFilter.h ├── SL_HarrisMatrix.cpp ├── SL_HarrisMatrix.h ├── SL_Image.cpp ├── SL_Image.h ├── SL_ImageIO.cpp ├── SL_ImageIO.h ├── SL_ImageOp.cpp └── SL_ImageOp.h ├── lap ├── LAP.cpp └── LAP.h ├── matching ├── SL_BlockDescriptorExtractor.cpp ├── SL_BlockDescriptorExtractor.h ├── SL_GuidedNCCMatcher.cpp ├── SL_GuidedNCCMatcher.h ├── SL_GuidedSSDMatcher.cpp ├── SL_GuidedSSDMatcher.h ├── SL_Matching.h ├── SL_StereoMatcher.cpp ├── SL_StereoMatcher.h ├── SL_StereoMatcherHelper.cpp ├── SL_StereoMatcherHelper.h ├── SL_SurfMatching.cpp └── SL_SurfMatching.h ├── math ├── SL_Array3D.cpp ├── SL_Array3D.h ├── SL_LinAlg.cpp ├── SL_LinAlg.h ├── SL_LinAlgWarper.h ├── SL_Math.cpp ├── SL_Math.h ├── SL_Matrix.cpp ├── SL_Matrix.h ├── SL_ProbFuncs.cpp ├── SL_ProbFuncs.h ├── SL_SparseLinearSystem.cpp ├── SL_SparseLinearSystem.h ├── SL_SparseMat.cpp ├── SL_SparseMat.h ├── lsqr.cpp └── lsqr.h ├── tools ├── GUI_ImageViewer.cpp ├── GUI_ImageViewer.h ├── GraphUtils.cpp ├── GraphUtils.h ├── SL_AVIReader.cpp ├── SL_AVIReader.h ├── SL_Debug.cpp ├── SL_Debug.h ├── SL_DrawCorners.cpp ├── SL_DrawCorners.h ├── SL_Print.cpp ├── SL_Print.h ├── SL_Tictoc.cpp ├── SL_Tictoc.h ├── SL_Timing.cpp ├── SL_Timing.h ├── SL_TypeConversion.cpp ├── SL_TypeConversion.h ├── SL_USBCamReader.cpp ├── SL_USBCamReader.h ├── SL_VideoReader.cpp ├── SL_VideoReader.h ├── SL_WriteRead.cpp ├── SL_WriteRead.h ├── cvHelper.cpp └── cvHelper.h └── util ├── SL_Graph.h ├── SL_Utility.h └── testSparse.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(libVisualSLAM) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | #The version number 5 | set (libVisualSLAM_VERSION_MAJOR 1) 6 | set (libVisualSLAM_VERSION_MINOR 0) 7 | 8 | find_package(OpenCV REQUIRED) 9 | 10 | aux_source_directory(src SRC) 11 | aux_source_directory(src/calibration SRC_CALIBRATION) 12 | aux_source_directory(src/extern SRC_EXTERN) 13 | aux_source_directory(src/extern/sba-1.6 SRC_SBA) 14 | aux_source_directory(src/extern/csparse SRC_CSPARSE) 15 | 16 | FILE(GLOB SRC_LEVMAR "src/extern/levmar/*.[hc]") 17 | FILE(GLOB levmar_cores "src/extern/levmar/*_core.c") 18 | list(REMOVE_ITEM SRC_LEVMAR ${levmar_cores}) 19 | 20 | aux_source_directory(src/geometry SRC_GEOMETRY) 21 | aux_source_directory(src/imgproc SRC_IMGPROC) 22 | aux_source_directory(src/lap SRC_LAP) 23 | aux_source_directory(src/matching SRC_MATCHING) 24 | aux_source_directory(src/math SRC_MATH) 25 | aux_source_directory(src/tools SRC_TOOLS) 26 | aux_source_directory(src/util SRC_UTIL) 27 | 28 | include_directories(${OpenCV_INCLUDE_DIRS}) 29 | include_directories(src) 30 | 31 | add_library(VisualSLAM ${SRC} ${SRC_CALIBRATION} ${SRC_EXTERN} ${SRC_SBA} ${SRC_CSPARSE} ${SRC_LEVMAR} ${SRC_GEOMETRY} ${SRC_IMGPROC} ${SRC_LAP} ${SRC_MATCHING} ${SRC_MATH} ${SRC_TOOLS} ${SRC_UTIL}) 32 | 33 | install (TARGETS VisualSLAM DESTINATION lib) 34 | FILE(GLOB headers_calibration "src/calibration/*.h") 35 | FILE(GLOB headers_extern "src/extern/*.h") 36 | FILE(GLOB headers_sba "src/extern/sba-1.6/*.h") 37 | FILE(GLOB headers_levmar "src/extern/levmar/*.h") 38 | FILE(GLOB headers_geometry "src/geometry/*.h") 39 | FILE(GLOB headers_imgproc "src/imgproc/*.h") 40 | FILE(GLOB headers_lap "src/lap/*.h") 41 | FILE(GLOB headers_matching "src/matching/*.h") 42 | FILE(GLOB headers_math "src/math/*.h") 43 | FILE(GLOB headers_tools "src/tools/*.h") 44 | FILE(GLOB headers_util "src/util/*.h") 45 | FILE(GLOB headers "src/*.h") 46 | 47 | install(FILES 48 | ${headers_calibration} 49 | DESTINATION 50 | include/VisualSLAM/calibration 51 | ) 52 | 53 | install(FILES 54 | ${headers_extern} 55 | DESTINATION 56 | include/VisualSLAM/extern 57 | ) 58 | 59 | install(FILES 60 | ${headers_sba} 61 | DESTINATION 62 | include/VisualSLAM/extern/sba-1.6 63 | ) 64 | 65 | install(FILES 66 | ${headers_geometry} 67 | DESTINATION 68 | include/VisualSLAM/geometry 69 | ) 70 | install(FILES 71 | ${headers_imgproc} 72 | DESTINATION 73 | include/VisualSLAM/imgproc 74 | ) 75 | install(FILES 76 | ${headers_lap} 77 | DESTINATION 78 | include/VisualSLAM/lap 79 | ) 80 | 81 | install(FILES 82 | ${headers_matching} 83 | DESTINATION 84 | include/VisualSLAM/matching 85 | ) 86 | 87 | install(FILES 88 | ${headers_math} 89 | DESTINATION 90 | include/VisualSLAM/math 91 | ) 92 | 93 | install(FILES 94 | ${headers_tools} 95 | DESTINATION 96 | include/VisualSLAM/tools 97 | ) 98 | 99 | install(FILES 100 | ${headers_util} 101 | DESTINATION 102 | include/VisualSLAM/util 103 | ) 104 | 105 | install(FILES 106 | ${headers} 107 | DESTINATION 108 | include/VisualSLAM 109 | ) 110 | 111 | 112 | configure_file( 113 | "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" 114 | "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" 115 | IMMEDIATE @ONLY) 116 | 117 | add_custom_target(uninstall 118 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) 119 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | LibVisualSLAM 2 | ============= 3 | LibVisualSLAM is an algorithm library accompany with [CoSLAM](https://github.com/danping/CoSLAM). 4 | 5 | System requirements 6 | ------------- 7 | 8 | * Linux Unbuntu or Linux Mint (64 bit). 9 | 10 | Dependent packages: 11 | ------------- 12 | * OpenCV 13 | * libatlas-dev 14 | 15 | 16 | Download & Installation 17 | ------------- 18 | 19 | git clone https://github.com/danping/LibVisualSLAM 20 | cd LibVisualSLAM 21 | cmake . 22 | make 23 | sudo make install 24 | 25 | 26 | Liciense 27 | ------------- 28 | LibVisualSLAM is released under [GPL v2.0](http://www.gnu.org/licenses/gpl-2.0.html). 29 | -------------------------------------------------------------------------------- /cmake_uninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if (NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") 3 | endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | list(REVERSE files) 8 | foreach (file ${files}) 9 | message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") 10 | if (EXISTS "$ENV{DESTDIR}${file}") 11 | execute_process( 12 | COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}" 13 | OUTPUT_VARIABLE rm_out 14 | RESULT_VARIABLE rm_retval 15 | ) 16 | if(NOT ${rm_retval} EQUAL 0) 17 | message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") 18 | endif (NOT ${rm_retval} EQUAL 0) 19 | else (EXISTS "$ENV{DESTDIR}${file}") 20 | message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") 21 | endif (EXISTS "$ENV{DESTDIR}${file}") 22 | endforeach(file) 23 | -------------------------------------------------------------------------------- /src/SL_PtrVec.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_PtrVec.h 3 | * 4 | * Created on: 2010-11-6 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_PTRVEC_H_ 10 | #define SL_PTRVEC_H_ 11 | 12 | /* class for automatically release the allocated pointer memories*/ 13 | template 14 | class PtrVec { 15 | public: 16 | T** data; 17 | int len; 18 | public: 19 | typedef T* PT; 20 | PtrVec() : 21 | data(0), len(0) { 22 | } 23 | ; 24 | virtual ~PtrVec() { 25 | clear(); 26 | } 27 | public: 28 | int size() const { 29 | return len; 30 | } 31 | void reserve(int num) { 32 | clear(); 33 | data = new T*[num]; 34 | } 35 | void push_back(T* ptr) { 36 | data[len++] = ptr; 37 | } 38 | bool empty() const { 39 | return len <= 0; 40 | } 41 | void clear() { 42 | if (empty()) 43 | return; 44 | else { /* release the element memory */ 45 | int i; 46 | for (i = 0; i < len; ++i) { 47 | delete data[i]; 48 | } 49 | } 50 | /*release the pointer's memory*/ 51 | delete[] data; 52 | data = 0; 53 | len = 0; 54 | } 55 | inline T* operator[](int i) const { 56 | return data[i]; 57 | } 58 | inline PT& operator[](int i) { 59 | return data[i]; 60 | } 61 | }; 62 | #endif /* SL_PTRVEC_H_ */ 63 | -------------------------------------------------------------------------------- /src/SL_error.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_error.cpp 3 | * 4 | * Created on: 2010-11-5 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #include "SL_error.h" 10 | SL_Exception::SL_Exception(void) { 11 | } 12 | SL_Exception::SL_Exception(const char* str) { 13 | strcpy(err_str, str); 14 | } 15 | SL_Exception::~SL_Exception(void) { 16 | 17 | } 18 | void warn(const char* fmtstr, ...) { 19 | char buf[1024]; 20 | GET_FMT_STR(fmtstr, buf) 21 | fprintf(stderr, buf); 22 | } 23 | void repErr(const char* fmtstr, ...) { 24 | char buf[1024]; 25 | GET_FMT_STR(fmtstr, buf) 26 | logInfo("%s\n", buf); 27 | throw SL_Exception(buf); 28 | } 29 | 30 | void logInfo(const char* fmtstr, ...) { 31 | char buf[1024]; 32 | GET_FMT_STR(fmtstr, buf) 33 | //can be replaced by other methods to output the information 34 | #ifdef WIN32 35 | //TRACE1("%s",buf); 36 | printf("%s", buf); 37 | #else 38 | printf("%s", buf); 39 | #endif 40 | } 41 | void LogFile::open(const char* fmtstr, ...){ 42 | char buf[1024]; 43 | GET_FMT_STR(fmtstr, buf); 44 | fp = fopen(buf, "w+"); 45 | } 46 | LogFile::LogFile(const char* fmtstr, ...) { 47 | char buf[1024]; 48 | GET_FMT_STR(fmtstr, buf); 49 | fp = fopen(buf, "w+"); 50 | } 51 | void LogFile::print(const char* fmtstr, ...) { 52 | char buf[1024]; 53 | GET_FMT_STR(fmtstr, buf); 54 | fprintf(fp, "%s", buf); 55 | } 56 | -------------------------------------------------------------------------------- /src/SL_error.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_error.h 3 | * 4 | * Created on: 2010-11-5 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_ERROR_H_ 10 | #define SL_ERROR_H_ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define GET_FMT_STR(fmstr,buf) \ 19 | va_list args;\ 20 | va_start(args,fmtstr);\ 21 | vsprintf(buf,fmstr,args);\ 22 | va_end(args); 23 | 24 | class SL_Exception { 25 | protected: 26 | char err_str[1024]; 27 | public: 28 | SL_Exception(void); 29 | SL_Exception(const char* str); 30 | ~SL_Exception(void); 31 | const char* what() const { 32 | return err_str; 33 | } 34 | }; 35 | //report a warning without interrupting the execution of the program 36 | void warn(const char* fmtstr, ...); 37 | //report an error exception 38 | void repErr(const char* fmtstr, ...); 39 | //just log out information 40 | void logInfo(const char* fmtstr, ...); 41 | 42 | 43 | class LogFile { 44 | public: 45 | FILE* fp; 46 | public: 47 | LogFile(){} 48 | LogFile(const char* fmtstr, ...); 49 | ~LogFile() { 50 | if (fp) 51 | fclose(fp); 52 | } 53 | void print(const char* fmtstr, ...); 54 | void open(const char* fmtstr, ...); 55 | void close() { 56 | fclose(fp); 57 | fp = 0; 58 | } 59 | }; 60 | #endif /* SL_ERROR_H_ */ 61 | -------------------------------------------------------------------------------- /src/calibration/SL_CalibTwoCam.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_EMatEst.h 3 | * 4 | * Created on: 2011-7-22 5 | * Author: zou 6 | */ 7 | 8 | #ifndef SL_EMATEST_H_ 9 | #define SL_EMATEST_H_ 10 | 11 | #include "math/SL_LinAlg.h" 12 | #include "math/SL_Matrix.h" 13 | #include "imgproc/SL_Image.h" 14 | #include "geometry/SL_Distortion.h" 15 | 16 | #include 17 | #include 18 | 19 | #include "opencv2/opencv.hpp" 20 | #define FF_RANSAC CV_FM_RANSAC 21 | #define FF_LMEDS CV_FM_LMEDS 22 | /** 23 | * find a fundamental matrix that 24 | * pts1^T F pts2 = 0 25 | */ 26 | bool findFMatrix(const Mat_d& pts1, const Mat_d& pts2, Mat_d& F, double maxEpiErr, int method); 27 | int getInlierFMatches(const Mat_d& pts1, const Mat_d& pts2, const double F[9], double maxEpiErr, Mat_uc& inlierFlags) ; 28 | bool refineFMatrix(const Mat_d& pts1, const Mat_d& pts2, const Mat_uc& inlierFlags, Mat_d& F); 29 | 30 | class CalibTwoCamParam { 31 | public: 32 | double hessianThreshold; 33 | double surfRatio; 34 | double surfMaxDiff; 35 | double maxEpiErr; 36 | int nRansac; 37 | int minInlierNum; 38 | public: 39 | CalibTwoCamParam() : 40 | hessianThreshold(100), surfRatio(0.83), surfMaxDiff(0.6), maxEpiErr(6.0), nRansac(1000), minInlierNum(20) { 41 | } 42 | }; 43 | class CalibTwoCam: public CalibTwoCamParam { 44 | public: 45 | CalibTwoCam(); 46 | virtual ~CalibTwoCam(); 47 | public: 48 | //intrinsic matrices of two cameras 49 | const double* K1, *K2; 50 | //distortion parameters 51 | const double* kc1, *kc2; 52 | 53 | //inverses of intrinsic matrices and distortion parameters 54 | double iK1[9], iK2[9]; 55 | double ikc1[7], ikc2[7]; 56 | 57 | //matched feature points 58 | Mat_d orgPts1, orgPts2; 59 | Mat_d normPts1, normPts2; 60 | Mat_uc inlierFlag; 61 | 62 | //estimated fundamental matrix 63 | double F[9]; 64 | //estimated essential matrix 65 | double E[9]; 66 | 67 | void setIntrinParam(const double* _K1, const double* _K2) { 68 | K1 = _K1; 69 | K2 = _K2; 70 | mat33Inv(K1, iK1); 71 | mat33Inv(K2, iK2); 72 | } 73 | 74 | void setDistorParam(int IW, int IH, const double* _kc1, const double* _kc2) { 75 | assert(K1 && K2); 76 | kc1 = _kc1; 77 | kc2 = _kc2; 78 | invDistorParam(IW, IH, iK1, kc1, ikc1); 79 | invDistorParam(IW, IH, iK2, kc2, ikc2); 80 | } 81 | void matchSURFPoints(const ImgG& img1, const ImgG& img2); 82 | void setMatchedPoints(const Mat_d& pts1, const Mat_d& pts2); 83 | void getInlierInd(std::vector& ind); 84 | void getInlierFlag(Mat_uc& inFlag){ 85 | inFlag.cloneFrom(inlierFlag); 86 | } 87 | 88 | int estimateEMatOld(double maxEpiErr = 3.0, int method = FF_RANSAC); 89 | int estimateEMat(double maxEpiErr = 6.0); 90 | void outputInlierPoints(Mat_d& pts1, Mat_d& pts2); 91 | void outputInlierNormPoints(Mat_d& normPts1, Mat_d& normPts2); 92 | void outputRTs(Mat_d& R1, Mat_d& t1, Mat_d& R2, Mat_d& t2, bool all = false); 93 | }; 94 | #endif /* SL_EMATEST_H_ */ 95 | -------------------------------------------------------------------------------- /src/extern/arpack/arch.h: -------------------------------------------------------------------------------- 1 | /* 2 | ARPACK++ v1.2 2/20/2000 3 | c++ interface to ARPACK code. 4 | 5 | MODULE arch.h 6 | Modified version of arch.h (from LAPACK++ 1.1). 7 | Machine dependent functions and variable types. 8 | 9 | ARPACK Authors 10 | Richard Lehoucq 11 | Danny Sorensen 12 | Chao Yang 13 | Dept. of Computational & Applied Mathematics 14 | Rice University 15 | Houston, Texas 16 | */ 17 | 18 | 19 | #ifndef ARCH_H 20 | #define ARCH_H 21 | 22 | // ARPACK++ arcomplex type definition. 23 | // If you are not using g++ (or CC) and also are not intending 24 | // use complex variables, comment out the following line. 25 | 26 | #include "arcomp.h" 27 | 28 | // STL vector class. 29 | // If the Standard Template Library is not available at your system 30 | // and you do not want to install it, comment out the following line. 31 | 32 | #include 33 | 34 | // If your STL vector class defines a variable other than 35 | // __SGI_STL_VECTOR_H, please change this variable name 36 | // in the ifdef command below. 37 | 38 | #ifdef __SGI_STL_VECTOR_H 39 | #define STL_VECTOR_H 40 | #endif 41 | 42 | // UMFPACK parameters. 43 | // These parameters are used by UMFPACK library functions. Normally 44 | // they are not modified by the user. To use the default value, set 45 | // the parameter to zero. For a complete description of all UMFPACK 46 | // parameters, see the library documentation. 47 | 48 | #define UICNTL7 0 // icntl(7). Block size for the blas (machine-dependent). 49 | #define UICNTL5 0 // icntl(5). Number of columns to examine during pivot search. 50 | #define UCNTL2 0 // cntl(2). Amalgamation parameter. 51 | #define UKEEP7 0 // keep(7). Absolute number of elements a column must have 52 | // to be considered "dense". 53 | #define UKEEP8 0 // keep(8). Relative number of elements a column must have 54 | // to be considered "dense". Dense columns have more 55 | // than max{0,UMFABDEN,UMFREDEN*sqrt(n)} elements. 56 | 57 | // Line length used when reading a dense matrix from a file. 58 | 59 | #define LINELEN 256 60 | 61 | // Linkage names between C, C++, and Fortran (platform dependent) 62 | 63 | #if defined(RIOS) && !defined(CLAPACK) 64 | #define F77NAME(x) x 65 | #else 66 | // #include 67 | // #define F77NAME(x) name2(x,_) 68 | #define F77NAME(x) x ## _ 69 | #endif 70 | 71 | #if defined(SGI) && !defined(SGI_DEC) 72 | #define SGI_DEC 73 | 74 | extern "C" { 75 | void mkidxname() {} 76 | void mkdatname() {} 77 | } 78 | #endif 79 | 80 | 81 | // Type conversion. 82 | 83 | typedef int ARint; 84 | typedef int ARlogical; 85 | 86 | #ifdef __SUNPRO_CC 87 | 88 | typedef int bool; 89 | int true = 1; 90 | int false = 0; 91 | 92 | #endif 93 | 94 | 95 | #endif // ARCH_H 96 | -------------------------------------------------------------------------------- /src/extern/arpack/arcomp.h: -------------------------------------------------------------------------------- 1 | /* 2 | ARPACK++ v1.2 2/20/2000 3 | c++ interface to ARPACK code. 4 | 5 | MODULE arcomp.h 6 | arcomplex complex type definition. 7 | 8 | ARPACK Authors 9 | Richard Lehoucq 10 | Danny Sorensen 11 | Chao Yang 12 | Dept. of Computational & Applied Mathematics 13 | Rice University 14 | Houston, Texas 15 | */ 16 | 17 | #ifndef ARCOMP_H 18 | #define ARCOMP_H 19 | 20 | #include 21 | 22 | #ifdef __GNUG__ 23 | 24 | #define arcomplex std::complex 25 | 26 | #endif 27 | 28 | #if defined(__SUNPRO_CC) || defined(__sgi) 29 | 30 | template 31 | class arcomplex: public complex 32 | { 33 | public: 34 | 35 | arcomplex(ARFLOAT x, ARFLOAT y): complex(x,y) { } 36 | arcomplex(): complex() { } 37 | arcomplex(complex x): complex(x) { } 38 | 39 | }; 40 | 41 | #endif 42 | 43 | #endif // ARCOMP_H 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/extern/arpack/armat.h: -------------------------------------------------------------------------------- 1 | /* 2 | ARPACK++ v1.2 2/20/2000 3 | c++ interface to ARPACK code. 4 | 5 | MODULE ARMat.h 6 | Generic matrix template with a matrix-vector product. 7 | 8 | ARPACK Authors 9 | Richard Lehoucq 10 | Danny Sorensen 11 | Chao Yang 12 | Dept. of Computational & Applied Mathematics 13 | Rice University 14 | Houston, Texas 15 | */ 16 | 17 | #ifndef ARMAT_H 18 | #define ARMAT_H 19 | 20 | template 21 | class ARMatrix { 22 | 23 | protected: 24 | 25 | int m, n; // Number of rows and columns. 26 | bool defined; 27 | 28 | public: 29 | 30 | ARMatrix() { defined = false; } 31 | // Short constructor. 32 | 33 | ARMatrix(int nrows, int ncols = 0) 34 | // Long constructor. 35 | { 36 | m = nrows; 37 | n = (ncols?ncols:nrows); 38 | defined = false; 39 | } // Constructor. 40 | 41 | virtual ~ARMatrix() { } 42 | // Destructor. 43 | 44 | int nrows() { return m; } 45 | 46 | int ncols() { return n; } 47 | 48 | bool IsDefined() { return defined; } 49 | 50 | virtual void MultMv(ARTYPE* v, ARTYPE* w) = 0; 51 | // Matrix-vector product: w = A*v. 52 | 53 | }; // ARMatrix. 54 | 55 | #endif // ARMAT_H 56 | 57 | -------------------------------------------------------------------------------- /src/extern/arpack/arrgcomp.h: -------------------------------------------------------------------------------- 1 | /* 2 | ARPACK++ v1.2 2/20/2000 3 | c++ interface to ARPACK code. 4 | 5 | MODULE ARRGComp.h. 6 | Arpack++ class ARrcCompGenEig definition. 7 | 8 | ARPACK Authors 9 | Richard Lehoucq 10 | Danny Sorensen 11 | Chao Yang 12 | Dept. of Computational & Applied Mathematics 13 | Rice University 14 | Houston, Texas 15 | */ 16 | 17 | #ifndef ARRGCOMP_H 18 | #define ARRGCOMP_H 19 | 20 | #include 21 | 22 | #include "arch.h" 23 | #include "arrscomp.h" 24 | #include "arrgeig.h" 25 | 26 | template 27 | class ARrcCompGenEig: 28 | virtual public ARrcGenEig >, 29 | virtual public ARrcCompStdEig { 30 | 31 | public: 32 | 33 | // a) Constructors and destructor. 34 | 35 | ARrcCompGenEig() { } 36 | // Short constructor (Does nothing but calling base classes constructors). 37 | 38 | ARrcCompGenEig(int np, int nevp, char* whichp = "LM", 39 | int ncvp = 0, ARFLOAT tolp = 0.0, int maxitp = 0, 40 | arcomplex* residp = NULL, bool ishiftp = true); 41 | // Long constructor (regular mode). 42 | 43 | ARrcCompGenEig(int np, int nevp, arcomplex sigmap, 44 | char* whichp = "LM", int ncvp = 0, ARFLOAT tolp = 0.0, 45 | int maxitp = 0, arcomplex* residp = NULL, 46 | bool ishiftp = true); 47 | // Long constructor (shift and invert mode). 48 | 49 | ARrcCompGenEig(const ARrcCompGenEig& other) { Copy(other); } 50 | // Copy constructor. 51 | 52 | virtual ~ARrcCompGenEig() { } 53 | // Destructor. 54 | 55 | // b) Operators. 56 | 57 | ARrcCompGenEig& operator=(const ARrcCompGenEig& other); 58 | // Assignment operator. 59 | 60 | }; // class ARrcCompGenEig. 61 | 62 | 63 | // ------------------------------------------------------------------------ // 64 | // ARrcCompGenEig member functions definition. // 65 | // ------------------------------------------------------------------------ // 66 | 67 | 68 | template 69 | inline ARrcCompGenEig:: 70 | ARrcCompGenEig(int np, int nevp, char* whichp, int ncvp, ARFLOAT tolp, 71 | int maxitp, arcomplex* residp, bool ishiftp) 72 | 73 | { 74 | 75 | this->NoShift(); 76 | DefineParameters(np, nevp, whichp, ncvp, tolp, maxitp, residp, ishiftp); 77 | 78 | } // Long constructor (regular mode). 79 | 80 | 81 | template 82 | inline ARrcCompGenEig:: 83 | ARrcCompGenEig(int np, int nevp, arcomplex sigmap, char* whichp, 84 | int ncvp, ARFLOAT tolp, int maxitp, arcomplex* residp, 85 | bool ishiftp) 86 | 87 | { 88 | 89 | ChangeShift(sigmap); 90 | DefineParameters(np, nevp, whichp, ncvp, tolp, maxitp, residp, ishiftp); 91 | 92 | } // Long constructor (shif and invert mode). 93 | 94 | 95 | template 96 | ARrcCompGenEig& ARrcCompGenEig:: 97 | operator=(const ARrcCompGenEig& other) 98 | { 99 | 100 | if (this != &other) { // Stroustrup suggestion. 101 | this->ClearMem(); 102 | Copy(other); 103 | } 104 | return *this; 105 | 106 | } // operator=. 107 | 108 | 109 | #endif // ARRGCOMP_H 110 | 111 | -------------------------------------------------------------------------------- /src/extern/arpack/arrgeig.h: -------------------------------------------------------------------------------- 1 | /* 2 | ARPACK++ v1.2 2/20/2000 3 | c++ interface to ARPACK code. 4 | 5 | MODULE ARRGEig.h. 6 | Arpack++ class ARrcGenEig definition. 7 | Derived from ARrcStdEig, this class implements the 8 | reverse communication interface for generalized problems. 9 | 10 | ARPACK Authors 11 | Richard Lehoucq 12 | Danny Sorensen 13 | Chao Yang 14 | Dept. of Computational & Applied Mathematics 15 | Rice University 16 | Houston, Texas 17 | */ 18 | 19 | #ifndef ARRGEIG_H 20 | #define ARRGEIG_H 21 | 22 | #include "arch.h" 23 | #include "arerror.h" 24 | #include "arrseig.h" 25 | 26 | // ARrcGenEig class definition. 27 | 28 | template 29 | class ARrcGenEig: virtual public ARrcStdEig { 30 | 31 | public: 32 | 33 | // a) Public functions: 34 | 35 | // a.1) Functions that allow changes in problem parameters. 36 | 37 | void NoShift(); 38 | // Turns the problem to regular mode. 39 | 40 | 41 | // a.2) Constructors and destructor. 42 | 43 | ARrcGenEig(); 44 | // Short constructor that does almost nothing. 45 | 46 | ARrcGenEig(const ARrcGenEig& other) { Copy(other); } 47 | // Copy constructor. 48 | 49 | virtual ~ARrcGenEig() { } 50 | // Destructor (presently meaningless). 51 | 52 | // b) Operators. 53 | 54 | ARrcGenEig& operator=(const ARrcGenEig& other); 55 | // Assignment operator. 56 | 57 | }; // class ARrcGenEig. 58 | 59 | 60 | // ------------------------------------------------------------------------ // 61 | // ARrcGenEig member functions definition. // 62 | // ------------------------------------------------------------------------ // 63 | 64 | 65 | template 66 | inline void ARrcGenEig::NoShift() 67 | { 68 | 69 | this->sigmaR = (ARTYPE)0; 70 | this->sigmaI = 0.0; 71 | this->mode = 2; 72 | this->iparam[7] = this->mode; 73 | this->Restart(); 74 | 75 | } // NoShift. 76 | 77 | 78 | template 79 | inline ARrcGenEig::ARrcGenEig() 80 | { 81 | 82 | this->bmat = 'G'; // This is a generalized problem. 83 | this->NoShift(); 84 | 85 | } // Short constructor. 86 | 87 | 88 | template 89 | ARrcGenEig& ARrcGenEig:: 90 | operator=(const ARrcGenEig& other) 91 | { 92 | 93 | if (this != &other) { // Stroustrup suggestion. 94 | this->ClearMem(); 95 | Copy(other); 96 | } 97 | return *this; 98 | 99 | } // operator=. 100 | 101 | 102 | #endif // ARRGEIG_H 103 | 104 | -------------------------------------------------------------------------------- /src/extern/csparse/README.txt: -------------------------------------------------------------------------------- 1 | CSparse/Source directory: primary ANSI C source code files for CSparse. 2 | All of these files are printed verbatim in the book. To compile the 3 | libcsparse.a C-callable library, just type "make" in this directory. 4 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_add.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = alpha*A + beta*B */ 3 | cs *cs_add (const cs *A, const cs *B, double alpha, double beta) 4 | { 5 | csi p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values ; 6 | double *x, *Bx, *Cx ; 7 | cs *C ; 8 | if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ; /* check inputs */ 9 | if (A->m != B->m || A->n != B->n) return (NULL) ; 10 | m = A->m ; anz = A->p [A->n] ; 11 | n = B->n ; Bp = B->p ; Bx = B->x ; bnz = Bp [n] ; 12 | w = cs_calloc (m, sizeof (csi)) ; /* get workspace */ 13 | values = (A->x != NULL) && (Bx != NULL) ; 14 | x = values ? cs_malloc (m, sizeof (double)) : NULL ; /* get workspace */ 15 | C = cs_spalloc (m, n, anz + bnz, values, 0) ; /* allocate result*/ 16 | if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ; 17 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 18 | for (j = 0 ; j < n ; j++) 19 | { 20 | Cp [j] = nz ; /* column j of C starts here */ 21 | nz = cs_scatter (A, j, alpha, w, x, j+1, C, nz) ; /* alpha*A(:,j)*/ 22 | nz = cs_scatter (B, j, beta, w, x, j+1, C, nz) ; /* beta*B(:,j) */ 23 | if (values) for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ; 24 | } 25 | Cp [n] = nz ; /* finalize the last column of C */ 26 | cs_sprealloc (C, 0) ; /* remove extra space from C */ 27 | return (cs_done (C, w, x, 1)) ; /* success; free workspace, return C */ 28 | } 29 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_chol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* L = chol (A, [pinv parent cp]), pinv is optional */ 3 | csn *cs_chol (const cs *A, const css *S) 4 | { 5 | double d, lki, *Lx, *x, *Cx ; 6 | csi top, i, p, k, n, *Li, *Lp, *cp, *pinv, *s, *c, *parent, *Cp, *Ci ; 7 | cs *L, *C, *E ; 8 | csn *N ; 9 | if (!CS_CSC (A) || !S || !S->cp || !S->parent) return (NULL) ; 10 | n = A->n ; 11 | N = cs_calloc (1, sizeof (csn)) ; /* allocate result */ 12 | c = cs_malloc (2*n, sizeof (csi)) ; /* get csi workspace */ 13 | x = cs_malloc (n, sizeof (double)) ; /* get double workspace */ 14 | cp = S->cp ; pinv = S->pinv ; parent = S->parent ; 15 | C = pinv ? cs_symperm (A, pinv, 1) : ((cs *) A) ; 16 | E = pinv ? C : NULL ; /* E is alias for A, or a copy E=A(p,p) */ 17 | if (!N || !c || !x || !C) return (cs_ndone (N, E, c, x, 0)) ; 18 | s = c + n ; 19 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 20 | N->L = L = cs_spalloc (n, n, cp [n], 1, 0) ; /* allocate result */ 21 | if (!L) return (cs_ndone (N, E, c, x, 0)) ; 22 | Lp = L->p ; Li = L->i ; Lx = L->x ; 23 | for (k = 0 ; k < n ; k++) Lp [k] = c [k] = cp [k] ; 24 | for (k = 0 ; k < n ; k++) /* compute L(k,:) for L*L' = C */ 25 | { 26 | /* --- Nonzero pattern of L(k,:) ------------------------------------ */ 27 | top = cs_ereach (C, k, parent, s, c) ; /* find pattern of L(k,:) */ 28 | x [k] = 0 ; /* x (0:k) is now zero */ 29 | for (p = Cp [k] ; p < Cp [k+1] ; p++) /* x = full(triu(C(:,k))) */ 30 | { 31 | if (Ci [p] <= k) x [Ci [p]] = Cx [p] ; 32 | } 33 | d = x [k] ; /* d = C(k,k) */ 34 | x [k] = 0 ; /* clear x for k+1st iteration */ 35 | /* --- Triangular solve --------------------------------------------- */ 36 | for ( ; top < n ; top++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */ 37 | { 38 | i = s [top] ; /* s [top..n-1] is pattern of L(k,:) */ 39 | lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */ 40 | x [i] = 0 ; /* clear x for k+1st iteration */ 41 | for (p = Lp [i] + 1 ; p < c [i] ; p++) 42 | { 43 | x [Li [p]] -= Lx [p] * lki ; 44 | } 45 | d -= lki * lki ; /* d = d - L(k,i)*L(k,i) */ 46 | p = c [i]++ ; 47 | Li [p] = k ; /* store L(k,i) in column i */ 48 | Lx [p] = lki ; 49 | } 50 | /* --- Compute L(k,k) ----------------------------------------------- */ 51 | if (d <= 0) return (cs_ndone (N, E, c, x, 0)) ; /* not pos def */ 52 | p = c [k]++ ; 53 | Li [p] = k ; /* store L(k,k) = sqrt (d) in column k */ 54 | Lx [p] = sqrt (d) ; 55 | } 56 | Lp [n] = cp [n] ; /* finalize L */ 57 | return (cs_ndone (N, E, c, x, 1)) ; /* success: free E,s,x; return N */ 58 | } 59 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_cholsol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x=A\b where A is symmetric positive definite; b overwritten with solution */ 3 | csi cs_cholsol (csi order, const cs *A, double *b) 4 | { 5 | double *x ; 6 | css *S ; 7 | csn *N ; 8 | csi n, ok ; 9 | if (!CS_CSC (A) || !b) return (0) ; /* check inputs */ 10 | n = A->n ; 11 | S = cs_schol (order, A) ; /* ordering and symbolic analysis */ 12 | N = cs_chol (A, S) ; /* numeric Cholesky factorization */ 13 | x = cs_malloc (n, sizeof (double)) ; /* get workspace */ 14 | ok = (S && N && x) ; 15 | if (ok) 16 | { 17 | cs_ipvec (S->pinv, b, x, n) ; /* x = P*b */ 18 | cs_lsolve (N->L, x) ; /* x = L\x */ 19 | cs_ltsolve (N->L, x) ; /* x = L'\x */ 20 | cs_pvec (S->pinv, x, b, n) ; /* b = P'*x */ 21 | } 22 | cs_free (x) ; 23 | cs_sfree (S) ; 24 | cs_nfree (N) ; 25 | return (ok) ; 26 | } 27 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_compress.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = compressed-column form of a triplet matrix T */ 3 | cs *cs_compress (const cs *T) 4 | { 5 | csi m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj ; 6 | double *Cx, *Tx ; 7 | cs *C ; 8 | if (!CS_TRIPLET (T)) return (NULL) ; /* check inputs */ 9 | m = T->m ; n = T->n ; Ti = T->i ; Tj = T->p ; Tx = T->x ; nz = T->nz ; 10 | C = cs_spalloc (m, n, nz, Tx != NULL, 0) ; /* allocate result */ 11 | w = cs_calloc (n, sizeof (csi)) ; /* get workspace */ 12 | if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */ 13 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 14 | for (k = 0 ; k < nz ; k++) w [Tj [k]]++ ; /* column counts */ 15 | cs_cumsum (Cp, w, n) ; /* column pointers */ 16 | for (k = 0 ; k < nz ; k++) 17 | { 18 | Ci [p = w [Tj [k]]++] = Ti [k] ; /* A(i,j) is the pth entry in C */ 19 | if (Cx) Cx [p] = Tx [k] ; 20 | } 21 | return (cs_done (C, w, NULL, 1)) ; /* success; free w and return C */ 22 | } 23 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_counts.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* column counts of LL'=A or LL'=A'A, given parent & post ordering */ 3 | #define HEAD(k,j) (ata ? head [k] : j) 4 | #define NEXT(J) (ata ? next [J] : -1) 5 | static void init_ata (cs *AT, const csi *post, csi *w, csi **head, csi **next) 6 | { 7 | csi i, k, p, m = AT->n, n = AT->m, *ATp = AT->p, *ATi = AT->i ; 8 | *head = w+4*n, *next = w+5*n+1 ; 9 | for (k = 0 ; k < n ; k++) w [post [k]] = k ; /* invert post */ 10 | for (i = 0 ; i < m ; i++) 11 | { 12 | for (k = n, p = ATp[i] ; p < ATp[i+1] ; p++) k = CS_MIN (k, w [ATi[p]]); 13 | (*next) [i] = (*head) [k] ; /* place row i in linked list k */ 14 | (*head) [k] = i ; 15 | } 16 | } 17 | csi *cs_counts (const cs *A, const csi *parent, const csi *post, csi ata) 18 | { 19 | csi i, j, k, n, m, J, s, p, q, jleaf, *ATp, *ATi, *maxfirst, *prevleaf, 20 | *ancestor, *head = NULL, *next = NULL, *colcount, *w, *first, *delta ; 21 | cs *AT ; 22 | if (!CS_CSC (A) || !parent || !post) return (NULL) ; /* check inputs */ 23 | m = A->m ; n = A->n ; 24 | s = 4*n + (ata ? (n+m+1) : 0) ; 25 | delta = colcount = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 26 | w = cs_malloc (s, sizeof (csi)) ; /* get workspace */ 27 | AT = cs_transpose (A, 0) ; /* AT = A' */ 28 | if (!AT || !colcount || !w) return (cs_idone (colcount, AT, w, 0)) ; 29 | ancestor = w ; maxfirst = w+n ; prevleaf = w+2*n ; first = w+3*n ; 30 | for (k = 0 ; k < s ; k++) w [k] = -1 ; /* clear workspace w [0..s-1] */ 31 | for (k = 0 ; k < n ; k++) /* find first [j] */ 32 | { 33 | j = post [k] ; 34 | delta [j] = (first [j] == -1) ? 1 : 0 ; /* delta[j]=1 if j is a leaf */ 35 | for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ; 36 | } 37 | ATp = AT->p ; ATi = AT->i ; 38 | if (ata) init_ata (AT, post, w, &head, &next) ; 39 | for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */ 40 | for (k = 0 ; k < n ; k++) 41 | { 42 | j = post [k] ; /* j is the kth node in postordered etree */ 43 | if (parent [j] != -1) delta [parent [j]]-- ; /* j is not a root */ 44 | for (J = HEAD (k,j) ; J != -1 ; J = NEXT (J)) /* J=j for LL'=A case */ 45 | { 46 | for (p = ATp [J] ; p < ATp [J+1] ; p++) 47 | { 48 | i = ATi [p] ; 49 | q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf); 50 | if (jleaf >= 1) delta [j]++ ; /* A(i,j) is in skeleton */ 51 | if (jleaf == 2) delta [q]-- ; /* account for overlap in q */ 52 | } 53 | } 54 | if (parent [j] != -1) ancestor [j] = parent [j] ; 55 | } 56 | for (j = 0 ; j < n ; j++) /* sum up delta's of each child */ 57 | { 58 | if (parent [j] != -1) colcount [parent [j]] += colcount [j] ; 59 | } 60 | return (cs_idone (colcount, AT, w, 1)) ; /* success: free workspace */ 61 | } 62 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_cumsum.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* p [0..n] = cumulative sum of c [0..n-1], and then copy p [0..n-1] into c */ 3 | double cs_cumsum (csi *p, csi *c, csi n) 4 | { 5 | csi i, nz = 0 ; 6 | double nz2 = 0 ; 7 | if (!p || !c) return (-1) ; /* check inputs */ 8 | for (i = 0 ; i < n ; i++) 9 | { 10 | p [i] = nz ; 11 | nz += c [i] ; 12 | nz2 += c [i] ; /* also in double to avoid csi overflow */ 13 | c [i] = p [i] ; /* also copy p[0..n-1] back into c[0..n-1]*/ 14 | } 15 | p [n] = nz ; 16 | return (nz2) ; /* return sum (c [0..n-1]) */ 17 | } 18 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_dfs.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* depth-first-search of the graph of a matrix, starting at node j */ 3 | csi cs_dfs (csi j, cs *G, csi top, csi *xi, csi *pstack, const csi *pinv) 4 | { 5 | csi i, p, p2, done, jnew, head = 0, *Gp, *Gi ; 6 | if (!CS_CSC (G) || !xi || !pstack) return (-1) ; /* check inputs */ 7 | Gp = G->p ; Gi = G->i ; 8 | xi [0] = j ; /* initialize the recursion stack */ 9 | while (head >= 0) 10 | { 11 | j = xi [head] ; /* get j from the top of the recursion stack */ 12 | jnew = pinv ? (pinv [j]) : j ; 13 | if (!CS_MARKED (Gp, j)) 14 | { 15 | CS_MARK (Gp, j) ; /* mark node j as visited */ 16 | pstack [head] = (jnew < 0) ? 0 : CS_UNFLIP (Gp [jnew]) ; 17 | } 18 | done = 1 ; /* node j done if no unvisited neighbors */ 19 | p2 = (jnew < 0) ? 0 : CS_UNFLIP (Gp [jnew+1]) ; 20 | for (p = pstack [head] ; p < p2 ; p++) /* examine all neighbors of j */ 21 | { 22 | i = Gi [p] ; /* consider neighbor node i */ 23 | if (CS_MARKED (Gp, i)) continue ; /* skip visited node i */ 24 | pstack [head] = p ; /* pause depth-first search of node j */ 25 | xi [++head] = i ; /* start dfs at node i */ 26 | done = 0 ; /* node j is not done */ 27 | break ; /* break, to start dfs (i) */ 28 | } 29 | if (done) /* depth-first search at node j is done */ 30 | { 31 | head-- ; /* remove j from the recursion stack */ 32 | xi [--top] = j ; /* and place in the output stack */ 33 | } 34 | } 35 | return (top) ; 36 | } 37 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_droptol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | static csi cs_tol (csi i, csi j, double aij, void *tol) 3 | { 4 | return (fabs (aij) > *((double *) tol)) ; 5 | } 6 | csi cs_droptol (cs *A, double tol) 7 | { 8 | return (cs_fkeep (A, &cs_tol, &tol)) ; /* keep all large entries */ 9 | } 10 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_dropzeros.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | static csi cs_nonzero (csi i, csi j, double aij, void *other) 3 | { 4 | return (aij != 0) ; 5 | } 6 | csi cs_dropzeros (cs *A) 7 | { 8 | return (cs_fkeep (A, &cs_nonzero, NULL)) ; /* keep all nonzero entries */ 9 | } 10 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_dupl.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* remove duplicate entries from A */ 3 | csi cs_dupl (cs *A) 4 | { 5 | csi i, j, p, q, nz = 0, n, m, *Ap, *Ai, *w ; 6 | double *Ax ; 7 | if (!CS_CSC (A)) return (0) ; /* check inputs */ 8 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | w = cs_malloc (m, sizeof (csi)) ; /* get workspace */ 10 | if (!w) return (0) ; /* out of memory */ 11 | for (i = 0 ; i < m ; i++) w [i] = -1 ; /* row i not yet seen */ 12 | for (j = 0 ; j < n ; j++) 13 | { 14 | q = nz ; /* column j will start at q */ 15 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 16 | { 17 | i = Ai [p] ; /* A(i,j) is nonzero */ 18 | if (w [i] >= q) 19 | { 20 | Ax [w [i]] += Ax [p] ; /* A(i,j) is a duplicate */ 21 | } 22 | else 23 | { 24 | w [i] = nz ; /* record where row i occurs */ 25 | Ai [nz] = i ; /* keep A(i,j) */ 26 | Ax [nz++] = Ax [p] ; 27 | } 28 | } 29 | Ap [j] = q ; /* record start of column j */ 30 | } 31 | Ap [n] = nz ; /* finalize A */ 32 | cs_free (w) ; /* free workspace */ 33 | return (cs_sprealloc (A, 0)) ; /* remove extra space from A */ 34 | } 35 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_entry.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* add an entry to a triplet matrix; return 1 if ok, 0 otherwise */ 3 | csi cs_entry (cs *T, csi i, csi j, double x) 4 | { 5 | if (!CS_TRIPLET (T) || i < 0 || j < 0) return (0) ; /* check inputs */ 6 | if (T->nz >= T->nzmax && !cs_sprealloc (T,2*(T->nzmax))) return (0) ; 7 | if (T->x) T->x [T->nz] = x ; 8 | T->i [T->nz] = i ; 9 | T->p [T->nz++] = j ; 10 | T->m = CS_MAX (T->m, i+1) ; 11 | T->n = CS_MAX (T->n, j+1) ; 12 | return (1) ; 13 | } 14 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_ereach.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* find nonzero pattern of Cholesky L(k,1:k-1) using etree and triu(A(:,k)) */ 3 | csi cs_ereach (const cs *A, csi k, const csi *parent, csi *s, csi *w) 4 | { 5 | csi i, p, n, len, top, *Ap, *Ai ; 6 | if (!CS_CSC (A) || !parent || !s || !w) return (-1) ; /* check inputs */ 7 | top = n = A->n ; Ap = A->p ; Ai = A->i ; 8 | CS_MARK (w, k) ; /* mark node k as visited */ 9 | for (p = Ap [k] ; p < Ap [k+1] ; p++) 10 | { 11 | i = Ai [p] ; /* A(i,k) is nonzero */ 12 | if (i > k) continue ; /* only use upper triangular part of A */ 13 | for (len = 0 ; !CS_MARKED (w,i) ; i = parent [i]) /* traverse up etree*/ 14 | { 15 | s [len++] = i ; /* L(k,i) is nonzero */ 16 | CS_MARK (w, i) ; /* mark i as visited */ 17 | } 18 | while (len > 0) s [--top] = s [--len] ; /* push path onto stack */ 19 | } 20 | for (p = top ; p < n ; p++) CS_MARK (w, s [p]) ; /* unmark all nodes */ 21 | CS_MARK (w, k) ; /* unmark node k */ 22 | return (top) ; /* s [top..n-1] contains pattern of L(k,:)*/ 23 | } 24 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_etree.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* compute the etree of A (using triu(A), or A'A without forming A'A */ 3 | csi *cs_etree (const cs *A, csi ata) 4 | { 5 | csi i, k, p, m, n, inext, *Ap, *Ai, *w, *parent, *ancestor, *prev ; 6 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 7 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; 8 | parent = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 9 | w = cs_malloc (n + (ata ? m : 0), sizeof (csi)) ; /* get workspace */ 10 | if (!w || !parent) return (cs_idone (parent, NULL, w, 0)) ; 11 | ancestor = w ; prev = w + n ; 12 | if (ata) for (i = 0 ; i < m ; i++) prev [i] = -1 ; 13 | for (k = 0 ; k < n ; k++) 14 | { 15 | parent [k] = -1 ; /* node k has no parent yet */ 16 | ancestor [k] = -1 ; /* nor does k have an ancestor */ 17 | for (p = Ap [k] ; p < Ap [k+1] ; p++) 18 | { 19 | i = ata ? (prev [Ai [p]]) : (Ai [p]) ; 20 | for ( ; i != -1 && i < k ; i = inext) /* traverse from i to k */ 21 | { 22 | inext = ancestor [i] ; /* inext = ancestor of i */ 23 | ancestor [i] = k ; /* path compression */ 24 | if (inext == -1) parent [i] = k ; /* no anc., parent is k */ 25 | } 26 | if (ata) prev [Ai [p]] = k ; 27 | } 28 | } 29 | return (cs_idone (parent, NULL, w, 1)) ; 30 | } 31 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_fkeep.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* drop entries for which fkeep(A(i,j)) is false; return nz if OK, else -1 */ 3 | csi cs_fkeep (cs *A, csi (*fkeep) (csi, csi, double, void *), void *other) 4 | { 5 | csi j, p, nz = 0, n, *Ap, *Ai ; 6 | double *Ax ; 7 | if (!CS_CSC (A) || !fkeep) return (-1) ; /* check inputs */ 8 | n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | p = Ap [j] ; /* get current location of col j */ 12 | Ap [j] = nz ; /* record new location of col j */ 13 | for ( ; p < Ap [j+1] ; p++) 14 | { 15 | if (fkeep (Ai [p], j, Ax ? Ax [p] : 1, other)) 16 | { 17 | if (Ax) Ax [nz] = Ax [p] ; /* keep A(i,j) */ 18 | Ai [nz++] = Ai [p] ; 19 | } 20 | } 21 | } 22 | Ap [n] = nz ; /* finalize A */ 23 | cs_sprealloc (A, 0) ; /* remove extra space from A */ 24 | return (nz) ; 25 | } 26 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_gaxpy.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* y = A*x+y */ 3 | csi cs_gaxpy (const cs *A, const double *x, double *y) 4 | { 5 | csi p, j, n, *Ap, *Ai ; 6 | double *Ax ; 7 | if (!CS_CSC (A) || !x || !y) return (0) ; /* check inputs */ 8 | n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 12 | { 13 | y [Ai [p]] += Ax [p] * x [j] ; 14 | } 15 | } 16 | return (1) ; 17 | } 18 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_happly.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* apply the ith Householder vector to x */ 3 | csi cs_happly (const cs *V, csi i, double beta, double *x) 4 | { 5 | csi p, *Vp, *Vi ; 6 | double *Vx, tau = 0 ; 7 | if (!CS_CSC (V) || !x) return (0) ; /* check inputs */ 8 | Vp = V->p ; Vi = V->i ; Vx = V->x ; 9 | for (p = Vp [i] ; p < Vp [i+1] ; p++) /* tau = v'*x */ 10 | { 11 | tau += Vx [p] * x [Vi [p]] ; 12 | } 13 | tau *= beta ; /* tau = beta*(v'*x) */ 14 | for (p = Vp [i] ; p < Vp [i+1] ; p++) /* x = x - v*tau */ 15 | { 16 | x [Vi [p]] -= Vx [p] * tau ; 17 | } 18 | return (1) ; 19 | } 20 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_house.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* create a Householder reflection [v,beta,s]=house(x), overwrite x with v, 3 | * where (I-beta*v*v')*x = s*e1. See Algo 5.1.1, Golub & Van Loan, 3rd ed. */ 4 | double cs_house (double *x, double *beta, csi n) 5 | { 6 | double s, sigma = 0 ; 7 | csi i ; 8 | if (!x || !beta) return (-1) ; /* check inputs */ 9 | for (i = 1 ; i < n ; i++) sigma += x [i] * x [i] ; 10 | if (sigma == 0) 11 | { 12 | s = fabs (x [0]) ; /* s = |x(0)| */ 13 | (*beta) = (x [0] <= 0) ? 2 : 0 ; 14 | x [0] = 1 ; 15 | } 16 | else 17 | { 18 | s = sqrt (x [0] * x [0] + sigma) ; /* s = norm (x) */ 19 | x [0] = (x [0] <= 0) ? (x [0] - s) : (-sigma / (x [0] + s)) ; 20 | (*beta) = -1. / (s * x [0]) ; 21 | } 22 | return (s) ; 23 | } 24 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_ipvec.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x(p) = b, for dense vectors x and b; p=NULL denotes identity */ 3 | csi cs_ipvec (const csi *p, const double *b, double *x, csi n) 4 | { 5 | csi k ; 6 | if (!x || !b) return (0) ; /* check inputs */ 7 | for (k = 0 ; k < n ; k++) x [p ? p [k] : k] = b [k] ; 8 | return (1) ; 9 | } 10 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_leaf.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* consider A(i,j), node j in ith row subtree and return lca(jprev,j) */ 3 | csi cs_leaf (csi i, csi j, const csi *first, csi *maxfirst, csi *prevleaf, 4 | csi *ancestor, csi *jleaf) 5 | { 6 | csi q, s, sparent, jprev ; 7 | if (!first || !maxfirst || !prevleaf || !ancestor || !jleaf) return (-1) ; 8 | *jleaf = 0 ; 9 | if (i <= j || first [j] <= maxfirst [i]) return (-1) ; /* j not a leaf */ 10 | maxfirst [i] = first [j] ; /* update max first[j] seen so far */ 11 | jprev = prevleaf [i] ; /* jprev = previous leaf of ith subtree */ 12 | prevleaf [i] = j ; 13 | *jleaf = (jprev == -1) ? 1: 2 ; /* j is first or subsequent leaf */ 14 | if (*jleaf == 1) return (i) ; /* if 1st leaf, q = root of ith subtree */ 15 | for (q = jprev ; q != ancestor [q] ; q = ancestor [q]) ; 16 | for (s = jprev ; s != q ; s = sparent) 17 | { 18 | sparent = ancestor [s] ; /* path compression */ 19 | ancestor [s] = q ; 20 | } 21 | return (q) ; /* q = least common ancester (jprev,j) */ 22 | } 23 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_load.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* load a triplet matrix from a file */ 3 | cs *cs_load (FILE *f) 4 | { 5 | double i, j ; /* use double for integers to avoid csi conflicts */ 6 | double x ; 7 | cs *T ; 8 | if (!f) return (NULL) ; /* check inputs */ 9 | T = cs_spalloc (0, 0, 1, 1, 1) ; /* allocate result */ 10 | while (fscanf (f, "%lg %lg %lg\n", &i, &j, &x) == 3) 11 | { 12 | if (!cs_entry (T, (csi) i, (csi) j, x)) return (cs_spfree (T)) ; 13 | } 14 | return (T) ; 15 | } 16 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_lsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve Lx=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_lsolve (const cs *L, double *x) 4 | { 5 | csi p, j, n, *Lp, *Li ; 6 | double *Lx ; 7 | if (!CS_CSC (L) || !x) return (0) ; /* check inputs */ 8 | n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | x [j] /= Lx [Lp [j]] ; 12 | for (p = Lp [j]+1 ; p < Lp [j+1] ; p++) 13 | { 14 | x [Li [p]] -= Lx [p] * x [j] ; 15 | } 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_ltsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve L'x=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_ltsolve (const cs *L, double *x) 4 | { 5 | csi p, j, n, *Lp, *Li ; 6 | double *Lx ; 7 | if (!CS_CSC (L) || !x) return (0) ; /* check inputs */ 8 | n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ; 9 | for (j = n-1 ; j >= 0 ; j--) 10 | { 11 | for (p = Lp [j]+1 ; p < Lp [j+1] ; p++) 12 | { 13 | x [j] -= Lx [p] * x [Li [p]] ; 14 | } 15 | x [j] /= Lx [Lp [j]] ; 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_lusol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x=A\b where A is unsymmetric; b overwritten with solution */ 3 | csi cs_lusol (csi order, const cs *A, double *b, double tol) 4 | { 5 | double *x ; 6 | css *S ; 7 | csn *N ; 8 | csi n, ok ; 9 | if (!CS_CSC (A) || !b) return (0) ; /* check inputs */ 10 | n = A->n ; 11 | S = cs_sqr (order, A, 0) ; /* ordering and symbolic analysis */ 12 | N = cs_lu (A, S, tol) ; /* numeric LU factorization */ 13 | x = cs_malloc (n, sizeof (double)) ; /* get workspace */ 14 | ok = (S && N && x) ; 15 | if (ok) 16 | { 17 | cs_ipvec (N->pinv, b, x, n) ; /* x = b(p) */ 18 | cs_lsolve (N->L, x) ; /* x = L\x */ 19 | cs_usolve (N->U, x) ; /* x = U\x */ 20 | cs_ipvec (S->q, x, b, n) ; /* b(q) = x */ 21 | } 22 | cs_free (x) ; 23 | cs_sfree (S) ; 24 | cs_nfree (N) ; 25 | return (ok) ; 26 | } 27 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_malloc.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | #ifdef MATLAB_MEX_FILE 3 | #define malloc mxMalloc 4 | #define free mxFree 5 | #define realloc mxRealloc 6 | #define calloc mxCalloc 7 | #endif 8 | 9 | /* wrapper for malloc */ 10 | void *cs_malloc (csi n, size_t size) 11 | { 12 | return (malloc (CS_MAX (n,1) * size)) ; 13 | } 14 | 15 | /* wrapper for calloc */ 16 | void *cs_calloc (csi n, size_t size) 17 | { 18 | return (calloc (CS_MAX (n,1), size)) ; 19 | } 20 | 21 | /* wrapper for free */ 22 | void *cs_free (void *p) 23 | { 24 | if (p) free (p) ; /* free p if it is not already NULL */ 25 | return (NULL) ; /* return NULL to simplify the use of cs_free */ 26 | } 27 | 28 | /* wrapper for realloc */ 29 | void *cs_realloc (void *p, csi n, size_t size, csi *ok) 30 | { 31 | void *pnew ; 32 | pnew = realloc (p, CS_MAX (n,1) * size) ; /* realloc the block */ 33 | *ok = (pnew != NULL) ; /* realloc fails if pnew is NULL */ 34 | return ((*ok) ? pnew : p) ; /* return original p if failure */ 35 | } 36 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_multiply.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A*B */ 3 | cs *cs_multiply (const cs *A, const cs *B) 4 | { 5 | csi p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values, *Bi ; 6 | double *x, *Bx, *Cx ; 7 | cs *C ; 8 | if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ; /* check inputs */ 9 | if (A->n != B->m) return (NULL) ; 10 | m = A->m ; anz = A->p [A->n] ; 11 | n = B->n ; Bp = B->p ; Bi = B->i ; Bx = B->x ; bnz = Bp [n] ; 12 | w = cs_calloc (m, sizeof (csi)) ; /* get workspace */ 13 | values = (A->x != NULL) && (Bx != NULL) ; 14 | x = values ? cs_malloc (m, sizeof (double)) : NULL ; /* get workspace */ 15 | C = cs_spalloc (m, n, anz + bnz, values, 0) ; /* allocate result */ 16 | if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ; 17 | Cp = C->p ; 18 | for (j = 0 ; j < n ; j++) 19 | { 20 | if (nz + m > C->nzmax && !cs_sprealloc (C, 2*(C->nzmax)+m)) 21 | { 22 | return (cs_done (C, w, x, 0)) ; /* out of memory */ 23 | } 24 | Ci = C->i ; Cx = C->x ; /* C->i and C->x may be reallocated */ 25 | Cp [j] = nz ; /* column j of C starts here */ 26 | for (p = Bp [j] ; p < Bp [j+1] ; p++) 27 | { 28 | nz = cs_scatter (A, Bi [p], Bx ? Bx [p] : 1, w, x, j+1, C, nz) ; 29 | } 30 | if (values) for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ; 31 | } 32 | Cp [n] = nz ; /* finalize the last column of C */ 33 | cs_sprealloc (C, 0) ; /* remove extra space from C */ 34 | return (cs_done (C, w, x, 1)) ; /* success; free workspace, return C */ 35 | } 36 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_norm.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* 1-norm of a sparse matrix = max (sum (abs (A))), largest column sum */ 3 | double cs_norm (const cs *A) 4 | { 5 | csi p, j, n, *Ap ; 6 | double *Ax, norm = 0, s ; 7 | if (!CS_CSC (A) || !A->x) return (-1) ; /* check inputs */ 8 | n = A->n ; Ap = A->p ; Ax = A->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | for (s = 0, p = Ap [j] ; p < Ap [j+1] ; p++) s += fabs (Ax [p]) ; 12 | norm = CS_MAX (norm, s) ; 13 | } 14 | return (norm) ; 15 | } 16 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_permute.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A(p,q) where p and q are permutations of 0..m-1 and 0..n-1. */ 3 | cs *cs_permute (const cs *A, const csi *pinv, const csi *q, csi values) 4 | { 5 | csi t, j, k, nz = 0, m, n, *Ap, *Ai, *Cp, *Ci ; 6 | double *Cx, *Ax ; 7 | cs *C ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 10 | C = cs_spalloc (m, n, Ap [n], values && Ax != NULL, 0) ; /* alloc result */ 11 | if (!C) return (cs_done (C, NULL, NULL, 0)) ; /* out of memory */ 12 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 13 | for (k = 0 ; k < n ; k++) 14 | { 15 | Cp [k] = nz ; /* column k of C is column q[k] of A */ 16 | j = q ? (q [k]) : k ; 17 | for (t = Ap [j] ; t < Ap [j+1] ; t++) 18 | { 19 | if (Cx) Cx [nz] = Ax [t] ; /* row i of A is row pinv[i] of C */ 20 | Ci [nz++] = pinv ? (pinv [Ai [t]]) : Ai [t] ; 21 | } 22 | } 23 | Cp [n] = nz ; /* finalize the last column of C */ 24 | return (cs_done (C, NULL, NULL, 1)) ; 25 | } 26 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_pinv.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* pinv = p', or p = pinv' */ 3 | csi *cs_pinv (csi const *p, csi n) 4 | { 5 | csi k, *pinv ; 6 | if (!p) return (NULL) ; /* p = NULL denotes identity */ 7 | pinv = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 8 | if (!pinv) return (NULL) ; /* out of memory */ 9 | for (k = 0 ; k < n ; k++) pinv [p [k]] = k ;/* invert the permutation */ 10 | return (pinv) ; /* return result */ 11 | } 12 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_post.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* post order a forest */ 3 | csi *cs_post (const csi *parent, csi n) 4 | { 5 | csi j, k = 0, *post, *w, *head, *next, *stack ; 6 | if (!parent) return (NULL) ; /* check inputs */ 7 | post = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 8 | w = cs_malloc (3*n, sizeof (csi)) ; /* get workspace */ 9 | if (!w || !post) return (cs_idone (post, NULL, w, 0)) ; 10 | head = w ; next = w + n ; stack = w + 2*n ; 11 | for (j = 0 ; j < n ; j++) head [j] = -1 ; /* empty linked lists */ 12 | for (j = n-1 ; j >= 0 ; j--) /* traverse nodes in reverse order*/ 13 | { 14 | if (parent [j] == -1) continue ; /* j is a root */ 15 | next [j] = head [parent [j]] ; /* add j to list of its parent */ 16 | head [parent [j]] = j ; 17 | } 18 | for (j = 0 ; j < n ; j++) 19 | { 20 | if (parent [j] != -1) continue ; /* skip j if it is not a root */ 21 | k = cs_tdfs (j, k, head, next, post, stack) ; 22 | } 23 | return (cs_idone (post, NULL, w, 1)) ; /* success; free w, return post */ 24 | } 25 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_print.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* print a sparse matrix; use %g for integers to avoid differences with csi */ 3 | csi cs_print (const cs *A, csi brief) 4 | { 5 | csi p, j, m, n, nzmax, nz, *Ap, *Ai ; 6 | double *Ax ; 7 | if (!A) { printf ("(null)\n") ; return (0) ; } 8 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 9 | nzmax = A->nzmax ; nz = A->nz ; 10 | printf ("CSparse Version %d.%d.%d, %s. %s\n", CS_VER, CS_SUBVER, 11 | CS_SUBSUB, CS_DATE, CS_COPYRIGHT) ; 12 | if (nz < 0) 13 | { 14 | printf ("%g-by-%g, nzmax: %g nnz: %g, 1-norm: %g\n", (double) m, 15 | (double) n, (double) nzmax, (double) (Ap [n]), cs_norm (A)) ; 16 | for (j = 0 ; j < n ; j++) 17 | { 18 | printf (" col %g : locations %g to %g\n", (double) j, 19 | (double) (Ap [j]), (double) (Ap [j+1]-1)) ; 20 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 21 | { 22 | printf (" %g : %g\n", (double) (Ai [p]), Ax ? Ax [p] : 1) ; 23 | if (brief && p > 20) { printf (" ...\n") ; return (1) ; } 24 | } 25 | } 26 | } 27 | else 28 | { 29 | printf ("triplet: %g-by-%g, nzmax: %g nnz: %g\n", (double) m, 30 | (double) n, (double) nzmax, (double) nz) ; 31 | for (p = 0 ; p < nz ; p++) 32 | { 33 | printf (" %g %g : %g\n", (double) (Ai [p]), (double) (Ap [p]), 34 | Ax ? Ax [p] : 1) ; 35 | if (brief && p > 20) { printf (" ...\n") ; return (1) ; } 36 | } 37 | } 38 | return (1) ; 39 | } 40 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_pvec.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x = b(p), for dense vectors x and b; p=NULL denotes identity */ 3 | csi cs_pvec (const csi *p, const double *b, double *x, csi n) 4 | { 5 | csi k ; 6 | if (!x || !b) return (0) ; /* check inputs */ 7 | for (k = 0 ; k < n ; k++) x [k] = b [p ? p [k] : k] ; 8 | return (1) ; 9 | } 10 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_qrsol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x=A\b where A can be rectangular; b overwritten with solution */ 3 | csi cs_qrsol (csi order, const cs *A, double *b) 4 | { 5 | double *x ; 6 | css *S ; 7 | csn *N ; 8 | cs *AT = NULL ; 9 | csi k, m, n, ok ; 10 | if (!CS_CSC (A) || !b) return (0) ; /* check inputs */ 11 | n = A->n ; 12 | m = A->m ; 13 | if (m >= n) 14 | { 15 | S = cs_sqr (order, A, 1) ; /* ordering and symbolic analysis */ 16 | N = cs_qr (A, S) ; /* numeric QR factorization */ 17 | x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ; /* get workspace */ 18 | ok = (S && N && x) ; 19 | if (ok) 20 | { 21 | cs_ipvec (S->pinv, b, x, m) ; /* x(0:m-1) = b(p(0:m-1) */ 22 | for (k = 0 ; k < n ; k++) /* apply Householder refl. to x */ 23 | { 24 | cs_happly (N->L, k, N->B [k], x) ; 25 | } 26 | cs_usolve (N->U, x) ; /* x = R\x */ 27 | cs_ipvec (S->q, x, b, n) ; /* b(q(0:n-1)) = x(0:n-1) */ 28 | } 29 | } 30 | else 31 | { 32 | AT = cs_transpose (A, 1) ; /* Ax=b is underdetermined */ 33 | S = cs_sqr (order, AT, 1) ; /* ordering and symbolic analysis */ 34 | N = cs_qr (AT, S) ; /* numeric QR factorization of A' */ 35 | x = cs_calloc (S ? S->m2 : 1, sizeof (double)) ; /* get workspace */ 36 | ok = (AT && S && N && x) ; 37 | if (ok) 38 | { 39 | cs_pvec (S->q, b, x, m) ; /* x(q(0:m-1)) = b(0:m-1) */ 40 | cs_utsolve (N->U, x) ; /* x = R'\x */ 41 | for (k = m-1 ; k >= 0 ; k--) /* apply Householder refl. to x */ 42 | { 43 | cs_happly (N->L, k, N->B [k], x) ; 44 | } 45 | cs_pvec (S->pinv, x, b, n) ; /* b(0:n-1) = x(p(0:n-1)) */ 46 | } 47 | } 48 | cs_free (x) ; 49 | cs_sfree (S) ; 50 | cs_nfree (N) ; 51 | cs_spfree (AT) ; 52 | return (ok) ; 53 | } 54 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_randperm.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* return a random permutation vector, the identity perm, or p = n-1:-1:0. 3 | * seed = -1 means p = n-1:-1:0. seed = 0 means p = identity. otherwise 4 | * p = random permutation. */ 5 | csi *cs_randperm (csi n, csi seed) 6 | { 7 | csi *p, k, j, t ; 8 | if (seed == 0) return (NULL) ; /* return p = NULL (identity) */ 9 | p = cs_malloc (n, sizeof (csi)) ; /* allocate result */ 10 | if (!p) return (NULL) ; /* out of memory */ 11 | for (k = 0 ; k < n ; k++) p [k] = n-k-1 ; 12 | if (seed == -1) return (p) ; /* return reverse permutation */ 13 | srand (seed) ; /* get new random number seed */ 14 | for (k = 0 ; k < n ; k++) 15 | { 16 | j = k + (rand ( ) % (n-k)) ; /* j = rand integer in range k to n-1 */ 17 | t = p [j] ; /* swap p[k] and p[j] */ 18 | p [j] = p [k] ; 19 | p [k] = t ; 20 | } 21 | return (p) ; 22 | } 23 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_reach.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* xi [top...n-1] = nodes reachable from graph of G*P' via nodes in B(:,k). 3 | * xi [n...2n-1] used as workspace */ 4 | csi cs_reach (cs *G, const cs *B, csi k, csi *xi, const csi *pinv) 5 | { 6 | csi p, n, top, *Bp, *Bi, *Gp ; 7 | if (!CS_CSC (G) || !CS_CSC (B) || !xi) return (-1) ; /* check inputs */ 8 | n = G->n ; Bp = B->p ; Bi = B->i ; Gp = G->p ; 9 | top = n ; 10 | for (p = Bp [k] ; p < Bp [k+1] ; p++) 11 | { 12 | if (!CS_MARKED (Gp, Bi [p])) /* start a dfs at unmarked node i */ 13 | { 14 | top = cs_dfs (Bi [p], G, top, xi, xi+n, pinv) ; 15 | } 16 | } 17 | for (p = top ; p < n ; p++) CS_MARK (Gp, xi [p]) ; /* restore G */ 18 | return (top) ; 19 | } 20 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_scatter.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* x = x + beta * A(:,j), where x is a dense vector and A(:,j) is sparse */ 3 | csi cs_scatter (const cs *A, csi j, double beta, csi *w, double *x, csi mark, 4 | cs *C, csi nz) 5 | { 6 | csi i, p, *Ap, *Ai, *Ci ; 7 | double *Ax ; 8 | if (!CS_CSC (A) || !w || !CS_CSC (C)) return (-1) ; /* check inputs */ 9 | Ap = A->p ; Ai = A->i ; Ax = A->x ; Ci = C->i ; 10 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 11 | { 12 | i = Ai [p] ; /* A(i,j) is nonzero */ 13 | if (w [i] < mark) 14 | { 15 | w [i] = mark ; /* i is new entry in column j */ 16 | Ci [nz++] = i ; /* add i to pattern of C(:,j) */ 17 | if (x) x [i] = beta * Ax [p] ; /* x(i) = beta*A(i,j) */ 18 | } 19 | else if (x) x [i] += beta * Ax [p] ; /* i exists in C(:,j) already */ 20 | } 21 | return (nz) ; 22 | } 23 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_scc.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* find the strongly connected components of a square matrix */ 3 | csd *cs_scc (cs *A) /* matrix A temporarily modified, then restored */ 4 | { 5 | csi n, i, k, b, nb = 0, top, *xi, *pstack, *p, *r, *Ap, *ATp, *rcopy, *Blk ; 6 | cs *AT ; 7 | csd *D ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | n = A->n ; Ap = A->p ; 10 | D = cs_dalloc (n, 0) ; /* allocate result */ 11 | AT = cs_transpose (A, 0) ; /* AT = A' */ 12 | xi = cs_malloc (2*n+1, sizeof (csi)) ; /* get workspace */ 13 | if (!D || !AT || !xi) return (cs_ddone (D, AT, xi, 0)) ; 14 | Blk = xi ; rcopy = pstack = xi + n ; 15 | p = D->p ; r = D->r ; ATp = AT->p ; 16 | top = n ; 17 | for (i = 0 ; i < n ; i++) /* first dfs(A) to find finish times (xi) */ 18 | { 19 | if (!CS_MARKED (Ap, i)) top = cs_dfs (i, A, top, xi, pstack, NULL) ; 20 | } 21 | for (i = 0 ; i < n ; i++) CS_MARK (Ap, i) ; /* restore A; unmark all nodes*/ 22 | top = n ; 23 | nb = n ; 24 | for (k = 0 ; k < n ; k++) /* dfs(A') to find strongly connnected comp */ 25 | { 26 | i = xi [k] ; /* get i in reverse order of finish times */ 27 | if (CS_MARKED (ATp, i)) continue ; /* skip node i if already ordered */ 28 | r [nb--] = top ; /* node i is the start of a component in p */ 29 | top = cs_dfs (i, AT, top, p, pstack, NULL) ; 30 | } 31 | r [nb] = 0 ; /* first block starts at zero; shift r up */ 32 | for (k = nb ; k <= n ; k++) r [k-nb] = r [k] ; 33 | D->nb = nb = n-nb ; /* nb = # of strongly connected components */ 34 | for (b = 0 ; b < nb ; b++) /* sort each block in natural order */ 35 | { 36 | for (k = r [b] ; k < r [b+1] ; k++) Blk [p [k]] = b ; 37 | } 38 | for (b = 0 ; b <= nb ; b++) rcopy [b] = r [b] ; 39 | for (i = 0 ; i < n ; i++) p [rcopy [Blk [i]]++] = i ; 40 | return (cs_ddone (D, AT, xi, 1)) ; 41 | } 42 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_schol.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* ordering and symbolic analysis for a Cholesky factorization */ 3 | css *cs_schol (csi order, const cs *A) 4 | { 5 | csi n, *c, *post, *P ; 6 | cs *C ; 7 | css *S ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | n = A->n ; 10 | S = cs_calloc (1, sizeof (css)) ; /* allocate result S */ 11 | if (!S) return (NULL) ; /* out of memory */ 12 | P = cs_amd (order, A) ; /* P = amd(A+A'), or natural */ 13 | S->pinv = cs_pinv (P, n) ; /* find inverse permutation */ 14 | cs_free (P) ; 15 | if (order && !S->pinv) return (cs_sfree (S)) ; 16 | C = cs_symperm (A, S->pinv, 0) ; /* C = spones(triu(A(P,P))) */ 17 | S->parent = cs_etree (C, 0) ; /* find etree of C */ 18 | post = cs_post (S->parent, n) ; /* postorder the etree */ 19 | c = cs_counts (C, S->parent, post, 0) ; /* find column counts of chol(C) */ 20 | cs_free (post) ; 21 | cs_spfree (C) ; 22 | S->cp = cs_malloc (n+1, sizeof (csi)) ; /* allocate result S->cp */ 23 | S->unz = S->lnz = cs_cumsum (S->cp, c, n) ; /* find column pointers for L */ 24 | cs_free (c) ; 25 | return ((S->lnz >= 0) ? S : cs_sfree (S)) ; 26 | } 27 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_spsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve Gx=b(:,k), where G is either upper (lo=0) or lower (lo=1) triangular */ 3 | csi cs_spsolve (cs *G, const cs *B, csi k, csi *xi, double *x, const csi *pinv, 4 | csi lo) 5 | { 6 | csi j, J, p, q, px, top, n, *Gp, *Gi, *Bp, *Bi ; 7 | double *Gx, *Bx ; 8 | if (!CS_CSC (G) || !CS_CSC (B) || !xi || !x) return (-1) ; 9 | Gp = G->p ; Gi = G->i ; Gx = G->x ; n = G->n ; 10 | Bp = B->p ; Bi = B->i ; Bx = B->x ; 11 | top = cs_reach (G, B, k, xi, pinv) ; /* xi[top..n-1]=Reach(B(:,k)) */ 12 | for (p = top ; p < n ; p++) x [xi [p]] = 0 ; /* clear x */ 13 | for (p = Bp [k] ; p < Bp [k+1] ; p++) x [Bi [p]] = Bx [p] ; /* scatter B */ 14 | for (px = top ; px < n ; px++) 15 | { 16 | j = xi [px] ; /* x(j) is nonzero */ 17 | J = pinv ? (pinv [j]) : j ; /* j maps to col J of G */ 18 | if (J < 0) continue ; /* column J is empty */ 19 | x [j] /= Gx [lo ? (Gp [J]) : (Gp [J+1]-1)] ;/* x(j) /= G(j,j) */ 20 | p = lo ? (Gp [J]+1) : (Gp [J]) ; /* lo: L(j,j) 1st entry */ 21 | q = lo ? (Gp [J+1]) : (Gp [J+1]-1) ; /* up: U(j,j) last entry */ 22 | for ( ; p < q ; p++) 23 | { 24 | x [Gi [p]] -= Gx [p] * x [j] ; /* x(i) -= G(i,j) * x(j) */ 25 | } 26 | } 27 | return (top) ; /* return top of stack */ 28 | } 29 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_symperm.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A(p,p) where A and C are symmetric the upper part stored; pinv not p */ 3 | cs *cs_symperm (const cs *A, const csi *pinv, csi values) 4 | { 5 | csi i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w ; 6 | double *Cx, *Ax ; 7 | cs *C ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 10 | C = cs_spalloc (n, n, Ap [n], values && (Ax != NULL), 0) ; /* alloc result*/ 11 | w = cs_calloc (n, sizeof (csi)) ; /* get workspace */ 12 | if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */ 13 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 14 | for (j = 0 ; j < n ; j++) /* count entries in each column of C */ 15 | { 16 | j2 = pinv ? pinv [j] : j ; /* column j of A is column j2 of C */ 17 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 18 | { 19 | i = Ai [p] ; 20 | if (i > j) continue ; /* skip lower triangular part of A */ 21 | i2 = pinv ? pinv [i] : i ; /* row i of A is row i2 of C */ 22 | w [CS_MAX (i2, j2)]++ ; /* column count of C */ 23 | } 24 | } 25 | cs_cumsum (Cp, w, n) ; /* compute column pointers of C */ 26 | for (j = 0 ; j < n ; j++) 27 | { 28 | j2 = pinv ? pinv [j] : j ; /* column j of A is column j2 of C */ 29 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 30 | { 31 | i = Ai [p] ; 32 | if (i > j) continue ; /* skip lower triangular part of A*/ 33 | i2 = pinv ? pinv [i] : i ; /* row i of A is row i2 of C */ 34 | Ci [q = w [CS_MAX (i2, j2)]++] = CS_MIN (i2, j2) ; 35 | if (Cx) Cx [q] = Ax [p] ; 36 | } 37 | } 38 | return (cs_done (C, w, NULL, 1)) ; /* success; free workspace, return C */ 39 | } 40 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_tdfs.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* depth-first search and postorder of a tree rooted at node j */ 3 | csi cs_tdfs (csi j, csi k, csi *head, const csi *next, csi *post, csi *stack) 4 | { 5 | csi i, p, top = 0 ; 6 | if (!head || !next || !post || !stack) return (-1) ; /* check inputs */ 7 | stack [0] = j ; /* place j on the stack */ 8 | while (top >= 0) /* while (stack is not empty) */ 9 | { 10 | p = stack [top] ; /* p = top of stack */ 11 | i = head [p] ; /* i = youngest child of p */ 12 | if (i == -1) 13 | { 14 | top-- ; /* p has no unordered children left */ 15 | post [k++] = p ; /* node p is the kth postordered node */ 16 | } 17 | else 18 | { 19 | head [p] = next [i] ; /* remove i from children of p */ 20 | stack [++top] = i ; /* start dfs on child node i */ 21 | } 22 | } 23 | return (k) ; 24 | } 25 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_transpose.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* C = A' */ 3 | cs *cs_transpose (const cs *A, csi values) 4 | { 5 | csi p, q, j, *Cp, *Ci, n, m, *Ap, *Ai, *w ; 6 | double *Cx, *Ax ; 7 | cs *C ; 8 | if (!CS_CSC (A)) return (NULL) ; /* check inputs */ 9 | m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ; 10 | C = cs_spalloc (n, m, Ap [n], values && Ax, 0) ; /* allocate result */ 11 | w = cs_calloc (m, sizeof (csi)) ; /* get workspace */ 12 | if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */ 13 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 14 | for (p = 0 ; p < Ap [n] ; p++) w [Ai [p]]++ ; /* row counts */ 15 | cs_cumsum (Cp, w, m) ; /* row pointers */ 16 | for (j = 0 ; j < n ; j++) 17 | { 18 | for (p = Ap [j] ; p < Ap [j+1] ; p++) 19 | { 20 | Ci [q = w [Ai [p]]++] = j ; /* place A(i,j) as entry C(j,i) */ 21 | if (Cx) Cx [q] = Ax [p] ; 22 | } 23 | } 24 | return (cs_done (C, w, NULL, 1)) ; /* success; free w and return C */ 25 | } 26 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_updown.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* sparse Cholesky update/downdate, L*L' + sigma*w*w' (sigma = +1 or -1) */ 3 | csi cs_updown (cs *L, csi sigma, const cs *C, const csi *parent) 4 | { 5 | csi n, p, f, j, *Lp, *Li, *Cp, *Ci ; 6 | double *Lx, *Cx, alpha, beta = 1, delta, gamma, w1, w2, *w, beta2 = 1 ; 7 | if (!CS_CSC (L) || !CS_CSC (C) || !parent) return (0) ; /* check inputs */ 8 | Lp = L->p ; Li = L->i ; Lx = L->x ; n = L->n ; 9 | Cp = C->p ; Ci = C->i ; Cx = C->x ; 10 | if ((p = Cp [0]) >= Cp [1]) return (1) ; /* return if C empty */ 11 | w = cs_malloc (n, sizeof (double)) ; /* get workspace */ 12 | if (!w) return (0) ; /* out of memory */ 13 | f = Ci [p] ; 14 | for ( ; p < Cp [1] ; p++) f = CS_MIN (f, Ci [p]) ; /* f = min (find (C)) */ 15 | for (j = f ; j != -1 ; j = parent [j]) w [j] = 0 ; /* clear workspace w */ 16 | for (p = Cp [0] ; p < Cp [1] ; p++) w [Ci [p]] = Cx [p] ; /* w = C */ 17 | for (j = f ; j != -1 ; j = parent [j]) /* walk path f up to root */ 18 | { 19 | p = Lp [j] ; 20 | alpha = w [j] / Lx [p] ; /* alpha = w(j) / L(j,j) */ 21 | beta2 = beta*beta + sigma*alpha*alpha ; 22 | if (beta2 <= 0) break ; /* not positive definite */ 23 | beta2 = sqrt (beta2) ; 24 | delta = (sigma > 0) ? (beta / beta2) : (beta2 / beta) ; 25 | gamma = sigma * alpha / (beta2 * beta) ; 26 | Lx [p] = delta * Lx [p] + ((sigma > 0) ? (gamma * w [j]) : 0) ; 27 | beta = beta2 ; 28 | for (p++ ; p < Lp [j+1] ; p++) 29 | { 30 | w1 = w [Li [p]] ; 31 | w [Li [p]] = w2 = w1 - alpha * Lx [p] ; 32 | Lx [p] = delta * Lx [p] + gamma * ((sigma > 0) ? w1 : w2) ; 33 | } 34 | } 35 | cs_free (w) ; 36 | return (beta2 > 0) ; 37 | } 38 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_usolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve Ux=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_usolve (const cs *U, double *x) 4 | { 5 | csi p, j, n, *Up, *Ui ; 6 | double *Ux ; 7 | if (!CS_CSC (U) || !x) return (0) ; /* check inputs */ 8 | n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ; 9 | for (j = n-1 ; j >= 0 ; j--) 10 | { 11 | x [j] /= Ux [Up [j+1]-1] ; 12 | for (p = Up [j] ; p < Up [j+1]-1 ; p++) 13 | { 14 | x [Ui [p]] -= Ux [p] * x [j] ; 15 | } 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /src/extern/csparse/cs_utsolve.c: -------------------------------------------------------------------------------- 1 | #include "cs.h" 2 | /* solve U'x=b where x and b are dense. x=b on input, solution on output. */ 3 | csi cs_utsolve (const cs *U, double *x) 4 | { 5 | csi p, j, n, *Up, *Ui ; 6 | double *Ux ; 7 | if (!CS_CSC (U) || !x) return (0) ; /* check inputs */ 8 | n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ; 9 | for (j = 0 ; j < n ; j++) 10 | { 11 | for (p = Up [j] ; p < Up [j+1]-1 ; p++) 12 | { 13 | x [j] -= Ux [p] * x [Ui [p]] ; 14 | } 15 | x [j] /= Ux [Up [j+1]-1] ; 16 | } 17 | return (1) ; 18 | } 19 | -------------------------------------------------------------------------------- /src/extern/lapack.h: -------------------------------------------------------------------------------- 1 | #ifndef LAPACK_H_ 2 | #define LAPACK_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | int dgelsy_(int *m, int *n, int *nrhs, double *a, int *lda, double *b, int *ldb, int * jpvt, double *rcond, int *rank, double *work, int * lwork, int *info); 9 | 10 | int dgetrf_(int *m, int *n, double *a, int * lda, int *ipiv, int *info); 11 | 12 | int dgetri_(int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info); 13 | 14 | void dpotrf_(char *uplo, int *n, double *a, int *lda, int *info); 15 | 16 | void dptsv_(int* n, int* nrhs, double* d, double* e, double* b, int* ldb, int* info); 17 | 18 | void dposv_(char* uplo, int* n, int* nrhs, double* a, int* lda, double* b, int* ldb, int* info); 19 | 20 | void dgeev_(char *jobvl, char *jobvr, int *n, double *A, int *lda, double *wr, double *wi, double *vl, int *ldvl, double *vr, int *ldvr, double *work, int *lwork, int *info); 21 | 22 | void dgeevx_(char balanc, char jobvl, char jobvr, char sense, int n, double *a, int lda, double *wr, double *wi, double *vl, int ldvl, double *vr, int ldvr, int *ilo, int *ihi, double *scale, double *abnrm, double *rcone, double *rconv, int *info); 23 | 24 | void dgesvd_(char *jobu, char *jobvt, int *m, int *n, double *A, int *lda, double *S, double *U, int *ldu, double *VT, int *ldvt, double *work, int *lwork, int *info); 25 | 26 | void dgemm_(char*, char*, int*, int *, int *, double *, double *, int*, double *, int *, double *, double *, int*); 27 | 28 | int dgeqrf_(int *m, int *n, double *a, int *lda, double *tau, double *work, int *lwork, int *info); 29 | 30 | int dorgqr_(int *m, int *n, int *k, double * a, int *lda, double *tau, double *work, int *lwork, int *info); 31 | 32 | int dgemv_(char *trans, int *m, int *n, double *alpha, double *a, int *lda, double *x, int *incx, double *beta, double *y, int *incy); 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif /* LAPACK_H_ */ 39 | 40 | -------------------------------------------------------------------------------- /src/extern/levmar/Axb.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Solution of linear systems involved in the Levenberg - Marquardt 4 | // minimization algorithm 5 | // Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr) 6 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 7 | // Heraklion, Crete, Greece. 8 | // 9 | // This program is free software; you can redistribute it and/or modify 10 | // it under the terms of the GNU General Public License as published by 11 | // the Free Software Foundation; either version 2 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // This program is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU General Public License for more details. 18 | // 19 | ///////////////////////////////////////////////////////////////////////////////// 20 | 21 | /******************************************************************************** 22 | * LAPACK-based implementations for various linear system solvers. The same core 23 | * code is used with appropriate #defines to derive single and double precision 24 | * solver versions, see also Axb_core.c 25 | ********************************************************************************/ 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "levmar.h" 32 | #include "misc.h" 33 | 34 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 35 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 36 | #endif 37 | 38 | 39 | #ifdef LM_DBL_PREC 40 | /* double precision definitions */ 41 | #define LM_REAL double 42 | #define LM_PREFIX d 43 | #define LM_CNST(x) (x) 44 | #ifndef HAVE_LAPACK 45 | #include 46 | #define LM_REAL_EPSILON DBL_EPSILON 47 | #endif 48 | 49 | #include "Axb_core.c" 50 | 51 | #undef LM_REAL 52 | #undef LM_PREFIX 53 | #undef LM_CNST 54 | #undef LM_REAL_EPSILON 55 | #endif /* LM_DBL_PREC */ 56 | 57 | #ifdef LM_SNGL_PREC 58 | /* single precision (float) definitions */ 59 | #define LM_REAL float 60 | #define LM_PREFIX s 61 | #define __SUBCNST(x) x##F 62 | #define LM_CNST(x) __SUBCNST(x) // force substitution 63 | #ifndef HAVE_LAPACK 64 | #define LM_REAL_EPSILON FLT_EPSILON 65 | #endif 66 | 67 | #include "Axb_core.c" 68 | 69 | #undef LM_REAL 70 | #undef LM_PREFIX 71 | #undef __SUBCNST 72 | #undef LM_CNST 73 | #undef LM_REAL_EPSILON 74 | #endif /* LM_SNGL_PREC */ 75 | -------------------------------------------------------------------------------- /src/extern/levmar/compiler.h: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | #ifndef _COMPILER_H_ 21 | #define _COMPILER_H_ 22 | 23 | /* note: intel's icc defines both __ICC & __INTEL_COMPILER. 24 | * Also, some compilers other than gcc define __GNUC__, 25 | * therefore gcc should be checked last 26 | */ 27 | #ifdef _MSC_VER 28 | #define inline __inline // MSVC 29 | #elif !defined(__ICC) && !defined(__INTEL_COMPILER) && !defined(__GNUC__) 30 | #define inline // other than MSVC, ICC, GCC: define empty 31 | #endif 32 | 33 | #ifdef _MSC_VER 34 | #define LM_FINITE _finite // MSVC 35 | #elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__) 36 | #define LM_FINITE finite // ICC, GCC 37 | #else 38 | #define LM_FINITE finite // other than MSVC, ICC, GCC, let's hope this will work 39 | #endif 40 | 41 | #ifdef _MSC_VER // avoid deprecation warnings in VS2005 42 | #define _CRT_SECURE_NO_WARNINGS 43 | #endif 44 | 45 | #endif /* _COMPILER_H_ */ 46 | -------------------------------------------------------------------------------- /src/extern/levmar/lm.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | /******************************************************************************** 21 | * Levenberg-Marquardt nonlinear minimization. The same core code is used with 22 | * appropriate #defines to derive single and double precision versions, see 23 | * also lm_core.c 24 | ********************************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "levmar.h" 32 | #include "compiler.h" 33 | #include "misc.h" 34 | 35 | #define EPSILON 1E-12 36 | #define ONE_THIRD 0.3333333334 /* 1.0/3.0 */ 37 | 38 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 39 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 40 | #endif 41 | 42 | 43 | #ifdef LM_SNGL_PREC 44 | /* single precision (float) definitions */ 45 | #define LM_REAL float 46 | #define LM_PREFIX s 47 | 48 | #define LM_REAL_MAX FLT_MAX 49 | #define LM_REAL_MIN -FLT_MAX 50 | #define LM_REAL_EPSILON FLT_EPSILON 51 | #define __SUBCNST(x) x##F 52 | #define LM_CNST(x) __SUBCNST(x) // force substitution 53 | 54 | #include "lm_core.c" // read in core code 55 | 56 | #undef LM_REAL 57 | #undef LM_PREFIX 58 | #undef LM_REAL_MAX 59 | #undef LM_REAL_EPSILON 60 | #undef LM_REAL_MIN 61 | #undef __SUBCNST 62 | #undef LM_CNST 63 | #endif /* LM_SNGL_PREC */ 64 | 65 | #ifdef LM_DBL_PREC 66 | /* double precision definitions */ 67 | #define LM_REAL double 68 | #define LM_PREFIX d 69 | 70 | #define LM_REAL_MAX DBL_MAX 71 | #define LM_REAL_MIN -DBL_MAX 72 | #define LM_REAL_EPSILON DBL_EPSILON 73 | #define LM_CNST(x) (x) 74 | 75 | #include "lm_core.c" // read in core code 76 | 77 | #undef LM_REAL 78 | #undef LM_PREFIX 79 | #undef LM_REAL_MAX 80 | #undef LM_REAL_EPSILON 81 | #undef LM_REAL_MIN 82 | #undef LM_CNST 83 | #endif /* LM_DBL_PREC */ 84 | -------------------------------------------------------------------------------- /src/extern/levmar/lmbc.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | /******************************************************************************** 21 | * Box-constrained Levenberg-Marquardt nonlinear minimization. The same core code 22 | * is used with appropriate #defines to derive single and double precision versions, 23 | * see also lmbc_core.c 24 | ********************************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "levmar.h" 32 | #include "compiler.h" 33 | #include "misc.h" 34 | 35 | #define EPSILON 1E-12 36 | #define ONE_THIRD 0.3333333334 /* 1.0/3.0 */ 37 | 38 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 39 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 40 | #endif 41 | 42 | 43 | #ifdef LM_SNGL_PREC 44 | /* single precision (float) definitions */ 45 | #define LM_REAL float 46 | #define LM_PREFIX s 47 | 48 | #define LM_REAL_MAX FLT_MAX 49 | #define LM_REAL_MIN -FLT_MAX 50 | 51 | #define LM_REAL_EPSILON FLT_EPSILON 52 | #define __SUBCNST(x) x##F 53 | #define LM_CNST(x) __SUBCNST(x) // force substitution 54 | 55 | #include "lmbc_core.c" // read in core code 56 | 57 | #undef LM_REAL 58 | #undef LM_PREFIX 59 | #undef LM_REAL_MAX 60 | #undef LM_REAL_MIN 61 | #undef LM_REAL_EPSILON 62 | #undef __SUBCNST 63 | #undef LM_CNST 64 | #endif /* LM_SNGL_PREC */ 65 | 66 | #ifdef LM_DBL_PREC 67 | /* double precision definitions */ 68 | #define LM_REAL double 69 | #define LM_PREFIX d 70 | 71 | #define LM_REAL_MAX DBL_MAX 72 | #define LM_REAL_MIN -DBL_MAX 73 | 74 | #define LM_REAL_EPSILON DBL_EPSILON 75 | #define LM_CNST(x) (x) 76 | 77 | #include "lmbc_core.c" // read in core code 78 | 79 | #undef LM_REAL 80 | #undef LM_PREFIX 81 | #undef LM_REAL_MAX 82 | #undef LM_REAL_MIN 83 | #undef LM_REAL_EPSILON 84 | #undef LM_CNST 85 | #endif /* LM_DBL_PREC */ 86 | -------------------------------------------------------------------------------- /src/extern/levmar/lmblec.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2004-06 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | /******************************************************************************** 21 | * combined box and linear equation constraints Levenberg-Marquardt nonlinear 22 | * minimization. The same core code is used with appropriate #defines to derive 23 | * single and double precision versions, see also lmblec_core.c 24 | ********************************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "levmar.h" 32 | #include "misc.h" 33 | 34 | #ifndef HAVE_LAPACK 35 | 36 | #ifdef _MSC_VER 37 | #pragma message("Combined box and linearly constrained optimization requires LAPACK and was not compiled!") 38 | #else 39 | #warning Combined box and linearly constrained optimization requires LAPACK and was not compiled! 40 | #endif // _MSC_VER 41 | 42 | #else // LAPACK present 43 | 44 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 45 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 46 | #endif 47 | 48 | 49 | #ifdef LM_SNGL_PREC 50 | /* single precision (float) definitions */ 51 | #define LM_REAL float 52 | #define LM_PREFIX s 53 | 54 | #define LM_REAL_MAX FLT_MAX 55 | #define LM_REAL_MIN -FLT_MAX 56 | #define __SUBCNST(x) x##F 57 | #define LM_CNST(x) __SUBCNST(x) // force substitution 58 | 59 | #include "lmblec_core.c" // read in core code 60 | 61 | #undef LM_REAL 62 | #undef LM_PREFIX 63 | #undef LM_REAL_MAX 64 | #undef LM_REAL_MIN 65 | #undef __SUBCNST 66 | #undef LM_CNST 67 | #endif /* LM_SNGL_PREC */ 68 | 69 | #ifdef LM_DBL_PREC 70 | /* double precision definitions */ 71 | #define LM_REAL double 72 | #define LM_PREFIX d 73 | 74 | #define LM_REAL_MAX DBL_MAX 75 | #define LM_REAL_MIN -DBL_MAX 76 | #define LM_CNST(x) (x) 77 | 78 | #include "lmblec_core.c" // read in core code 79 | 80 | #undef LM_REAL 81 | #undef LM_PREFIX 82 | #undef LM_REAL_MAX 83 | #undef LM_REAL_MIN 84 | #undef LM_CNST 85 | #endif /* LM_DBL_PREC */ 86 | 87 | #endif /* HAVE_LAPACK */ 88 | -------------------------------------------------------------------------------- /src/extern/levmar/lmbleic.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2009 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | /******************************************************************************* 21 | * Wrappers for linear inequality constrained Levenberg-Marquardt minimization. 22 | * The same core code is used with appropriate #defines to derive single and 23 | * double precision versions, see also lmbleic_core.c 24 | *******************************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "levmar.h" 32 | #include "misc.h" 33 | 34 | 35 | #ifndef HAVE_LAPACK 36 | 37 | #ifdef _MSC_VER 38 | #pragma message("Linear inequalities constrained optimization requires LAPACK and was not compiled!") 39 | #else 40 | #warning Linear inequalities constrained optimization requires LAPACK and was not compiled! 41 | #endif // _MSC_VER 42 | 43 | #else // LAPACK present 44 | 45 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 46 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 47 | #endif 48 | 49 | 50 | #ifdef LM_SNGL_PREC 51 | /* single precision (float) definitions */ 52 | #define LM_REAL float 53 | #define LM_PREFIX s 54 | 55 | #define LM_REAL_MAX FLT_MAX 56 | #define LM_REAL_MIN -FLT_MAX 57 | #define __SUBCNST(x) x##F 58 | #define LM_CNST(x) __SUBCNST(x) // force substitution 59 | 60 | #include "lmbleic_core.c" // read in core code 61 | 62 | #undef LM_REAL 63 | #undef LM_PREFIX 64 | #undef LM_REAL_MAX 65 | #undef LM_REAL_MIN 66 | #undef __SUBCNST 67 | #undef LM_CNST 68 | #endif /* LM_SNGL_PREC */ 69 | 70 | #ifdef LM_DBL_PREC 71 | /* double precision definitions */ 72 | #define LM_REAL double 73 | #define LM_PREFIX d 74 | 75 | #define LM_REAL_MAX DBL_MAX 76 | #define LM_REAL_MIN -DBL_MAX 77 | #define LM_CNST(x) (x) 78 | 79 | #include "lmbleic_core.c" // read in core code 80 | 81 | #undef LM_REAL 82 | #undef LM_PREFIX 83 | #undef LM_REAL_MAX 84 | #undef LM_REAL_MIN 85 | #undef LM_CNST 86 | #endif /* LM_DBL_PREC */ 87 | 88 | #endif /* HAVE_LAPACK */ 89 | 90 | -------------------------------------------------------------------------------- /src/extern/levmar/lmlec.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | /******************************************************************************* 21 | * Wrappers for linearly constrained Levenberg-Marquardt minimization. The same 22 | * core code is used with appropriate #defines to derive single and double 23 | * precision versions, see also lmlec_core.c 24 | *******************************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "levmar.h" 31 | #include "misc.h" 32 | 33 | 34 | #ifndef HAVE_LAPACK 35 | 36 | #ifdef _MSC_VER 37 | #pragma message("Linearly constrained optimization requires LAPACK and was not compiled!") 38 | #else 39 | #warning Linearly constrained optimization requires LAPACK and was not compiled! 40 | #endif // _MSC_VER 41 | 42 | #else // LAPACK present 43 | 44 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 45 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 46 | #endif 47 | 48 | 49 | #ifdef LM_SNGL_PREC 50 | /* single precision (float) definitions */ 51 | #define LM_REAL float 52 | #define LM_PREFIX s 53 | 54 | #define __SUBCNST(x) x##F 55 | #define LM_CNST(x) __SUBCNST(x) // force substitution 56 | 57 | #include "lmlec_core.c" // read in core code 58 | 59 | #undef LM_REAL 60 | #undef LM_PREFIX 61 | #undef __SUBCNST 62 | #undef LM_CNST 63 | #endif /* LM_SNGL_PREC */ 64 | 65 | #ifdef LM_DBL_PREC 66 | /* double precision definitions */ 67 | #define LM_REAL double 68 | #define LM_PREFIX d 69 | 70 | #define LM_CNST(x) (x) 71 | 72 | #include "lmlec_core.c" // read in core code 73 | 74 | #undef LM_REAL 75 | #undef LM_PREFIX 76 | #undef LM_CNST 77 | #endif /* LM_DBL_PREC */ 78 | 79 | #endif /* HAVE_LAPACK */ 80 | 81 | -------------------------------------------------------------------------------- /src/extern/levmar/misc.c: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // Levenberg - Marquardt non-linear minimization algorithm 4 | // Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr) 5 | // Institute of Computer Science, Foundation for Research & Technology - Hellas 6 | // Heraklion, Crete, Greece. 7 | // 8 | // This program is free software; you can redistribute it and/or modify 9 | // it under the terms of the GNU General Public License as published by 10 | // the Free Software Foundation; either version 2 of the License, or 11 | // (at your option) any later version. 12 | // 13 | // This program is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | // GNU General Public License for more details. 17 | // 18 | ///////////////////////////////////////////////////////////////////////////////// 19 | 20 | /******************************************************************************** 21 | * Miscelaneous functions for Levenberg-Marquardt nonlinear minimization. The 22 | * same core code is used with appropriate #defines to derive single and double 23 | * precision versions, see also misc_core.c 24 | ********************************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "levmar.h" 32 | #include "misc.h" 33 | 34 | #if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC) 35 | #error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined! 36 | #endif 37 | 38 | #ifdef LM_SNGL_PREC 39 | /* single precision (float) definitions */ 40 | #define LM_REAL float 41 | #define LM_PREFIX s 42 | 43 | #define LM_REAL_EPSILON FLT_EPSILON 44 | #define __SUBCNST(x) x##F 45 | #define LM_CNST(x) __SUBCNST(x) // force substitution 46 | 47 | #include "misc_core.c" // read in core code 48 | 49 | #undef LM_REAL 50 | #undef LM_PREFIX 51 | #undef LM_REAL_EPSILON 52 | #undef __SUBCNST 53 | #undef LM_CNST 54 | #endif /* LM_SNGL_PREC */ 55 | 56 | #ifdef LM_DBL_PREC 57 | /* double precision definitions */ 58 | #define LM_REAL double 59 | #define LM_PREFIX d 60 | 61 | #define LM_REAL_EPSILON DBL_EPSILON 62 | #define LM_CNST(x) (x) 63 | 64 | #include "misc_core.c" // read in core code 65 | 66 | #undef LM_REAL 67 | #undef LM_PREFIX 68 | #undef LM_REAL_EPSILON 69 | #undef LM_CNST 70 | #endif /* LM_DBL_PREC */ 71 | -------------------------------------------------------------------------------- /src/extern/sba-1.6/compiler.h: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | //// 3 | //// Compiler-specific definitions for sparse bundle adjustment based on the 4 | //// Levenberg - Marquardt minimization algorithm 5 | //// Copyright (C) 2004-2008 Manolis Lourakis (lourakis at ics forth gr) 6 | //// Institute of Computer Science, Foundation for Research & Technology - Hellas 7 | //// Heraklion, Crete, Greece. 8 | //// 9 | //// This program is free software; you can redistribute it and/or modify 10 | //// it under the terms of the GNU General Public License as published by 11 | //// the Free Software Foundation; either version 2 of the License, or 12 | //// (at your option) any later version. 13 | //// 14 | //// This program is distributed in the hope that it will be useful, 15 | //// but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | //// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | //// GNU General Public License for more details. 18 | //// 19 | /////////////////////////////////////////////////////////////////////////////////// 20 | 21 | #ifndef _COMPILER_H_ 22 | #define _COMPILER_H_ 23 | 24 | /* note: intel's icc defines both __ICC & __INTEL_COMPILER. 25 | * Also, some compilers other than gcc define __GNUC__, 26 | * therefore gcc should be checked last 27 | */ 28 | #ifdef _MSC_VER 29 | #define inline __inline // MSVC 30 | #elif !defined(__ICC) && !defined(__INTEL_COMPILER) && !defined(__GNUC__) 31 | #define inline // other than MSVC, ICC, GCC: define empty 32 | #endif 33 | 34 | #ifdef _MSC_VER 35 | #define SBA_FINITE _finite // MSVC 36 | #elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__) 37 | #define SBA_FINITE finite // ICC, GCC 38 | #else 39 | #define SBA_FINITE finite // other than MSVC, ICC, GCC, let's hope this will work 40 | #endif 41 | 42 | #endif /* _COMPILER_H_ */ 43 | -------------------------------------------------------------------------------- /src/extern/sba-1.6/sba_chkjac.h: -------------------------------------------------------------------------------- 1 | ///////////////////////////////////////////////////////////////////////////////// 2 | //// 3 | //// Prototypes and definitions for verification routines for the jacobians 4 | //// employed in the expert & simple drivers for sparse bundle adjustment 5 | //// Copyright (C) 2005-2008 Manolis Lourakis (lourakis at ics forth gr) 6 | //// Institute of Computer Science, Foundation for Research & Technology - Hellas 7 | //// Heraklion, Crete, Greece. 8 | //// 9 | //// This program is free software; you can redistribute it and/or modify 10 | //// it under the terms of the GNU General Public License as published by 11 | //// the Free Software Foundation; either version 2 of the License, or 12 | //// (at your option) any later version. 13 | //// 14 | //// This program is distributed in the hope that it will be useful, 15 | //// but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | //// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | //// GNU General Public License for more details. 18 | //// 19 | /////////////////////////////////////////////////////////////////////////////////// 20 | 21 | #ifndef _SBA_CHKJAC_H_ 22 | #define _SBA_CHKJAC_H_ 23 | 24 | //#ifdef __cplusplus 25 | // "C" { 26 | //#endif 27 | 28 | #if 0 29 | /* simple driver jacobians */ 30 | void sba_motstr_chkjac( 31 | void (*proj)(int jj, int ii, double *aj, double *bi, double *xij, void *adata), 32 | void (*projac)(int jj, int ii, double *aj, double *bi, double *Aij, double *Bij, void *adata), 33 | double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata); 34 | 35 | void sba_mot_chkjac( 36 | void (*proj)(int jj, int ii, double *aj, double *xij, void *adata), 37 | void (*projac)(int jj, int ii, double *aj, double *Aij, void *adata), 38 | double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata); 39 | 40 | void sba_str_chkjac( 41 | void (*proj)(int jj, int ii, double *bi, double *xij, void *adata), 42 | void (*projac)(int jj, int ii, double *bi, double *Bij, void *adata), 43 | double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata); 44 | #endif /* 0 */ 45 | 46 | /* expert driver jacobians */ 47 | void sba_motstr_chkjac_x( 48 | void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata), 49 | void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata), 50 | double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int mcon, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata); 51 | 52 | void sba_mot_chkjac_x( 53 | void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata), 54 | void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata), 55 | double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int mnp, void *func_adata, void *jac_adata); 56 | 57 | void sba_str_chkjac_x( 58 | void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata), 59 | void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata), 60 | double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int pnp, int mnp, void *func_adata, void *jac_adata); 61 | 62 | 63 | //#ifdef __cplusplus 64 | //} 65 | //#endif 66 | 67 | #endif /* _SBA_CHKJAC_H_ */ 68 | -------------------------------------------------------------------------------- /src/geometry/SL_8point.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_8point.cpp 3 | * 4 | * Created on: 2010-11-14 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_error.h" 9 | #include "math/SL_LinAlg.h" 10 | 11 | #include "SL_8point.h" 12 | #include "SL_5point.h" 13 | #include "SL_Triangulate.h" 14 | #include "SL_FundamentalMatrix.h" 15 | 16 | #include 17 | #include 18 | //have bugs 19 | void computeFMat8Pt(int n , const double* a , const double* b , double F[9]) { 20 | if (n < 7) 21 | repErr("more than 7 points are required."); 22 | double *A = new double[n * 9]; 23 | double* S = n > 9 ? new double[n * 9] : new double[81]; 24 | double* VT = new double[81]; 25 | 26 | int i; 27 | for (i = 0; i < n; ++i) { 28 | double ax = a[2 * i]; 29 | double ay = a[2 * i + 1]; 30 | double az = 1; 31 | 32 | double bx = b[2 * i]; 33 | double by = b[2 * i + 1]; 34 | double bz = 1; 35 | 36 | A[i * 9] = ax * bx; 37 | A[i * 9 + 1] = ay * bx; 38 | A[i * 9 + 2] = az * bx; 39 | A[i * 9 + 3] = ax * by; 40 | A[i * 9 + 4] = ay * by; 41 | A[i * 9 + 5] = az * by; 42 | A[i * 9 + 6] = ax * bz; 43 | A[i * 9 + 7] = ay * bz; 44 | A[i * 9 + 8] = az * bz; 45 | } 46 | dgesvdFor(n, 9, A, S, VT); 47 | memcpy(F, VT + 8 * 9, sizeof(double) * 9); 48 | delete[] A; 49 | delete[] S; 50 | delete[] VT; 51 | } 52 | int findFMatRansac( 53 | const int n , 54 | const double* a , 55 | const double* b , 56 | double* F , 57 | int iterMaxNum , 58 | double epiErrThes , 59 | double* epiErr) { 60 | int k, indices[8]; 61 | double pts1[16]; 62 | double pts2[16]; 63 | double tF[9]; 64 | 65 | double errMin = DBL_MAX; 66 | int inlierMax = 0; 67 | 68 | for (k = 0; k < iterMaxNum; ++k) { 69 | randChoose(n, indices, 8); 70 | int i; 71 | for (i = 0; i < 8; ++i) { 72 | pts1[i * 2] = a[indices[i] * 2]; 73 | pts1[i * 2 + 1] = a[indices[i] * 2 + 1]; 74 | 75 | pts2[i * 2] = b[indices[i] * 2]; 76 | pts2[i * 2 + 1] = b[indices[i] * 2 + 1]; 77 | } 78 | computeFMat8Pt(8, pts2, pts1, tF); 79 | double err; 80 | int inlier = evaluateEMat(n, a, b, tF, epiErrThes, err); 81 | if (inlier > inlierMax || (inlier == inlierMax && err < errMin)) { 82 | inlierMax = inlier; 83 | errMin = err; 84 | memcpy(F, tF, sizeof(double) * 9); 85 | } 86 | } 87 | 88 | if (epiErr) 89 | *epiErr = errMin; 90 | return inlierMax; 91 | } 92 | 93 | -------------------------------------------------------------------------------- /src/geometry/SL_8point.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_8point.h 3 | * 4 | * Created on: 2010-11-14 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_8POINT_H_ 9 | #define SL_8POINT_H_ 10 | void computeFMat8Pt(int n , const double* a , const double* b , double* E); 11 | int findFMatRansac( 12 | const int n , 13 | const double* a , 14 | const double* b , 15 | double* F , 16 | int iterMaxNum , 17 | double epiErrThes , 18 | double* epiErr); 19 | #endif /* SL_8POINT_H_ */ 20 | -------------------------------------------------------------------------------- /src/geometry/SL_AbsoluteOrientation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_AbsoluteOrientation.h 3 | * 4 | * Created on: 2010-12-3 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_AO_H_ 10 | #define SL_AO_H_ 11 | 12 | /* 13 | * absolute orientation algorithm using quaternion 14 | */ 15 | void aoGetMatrix(const int npts, const double r1[], const double r2[], double A[]); 16 | bool aoMaxEig(double N[], double v[]); 17 | bool absOrient(const int npts, const double pts1[], const double pts2[], double R[], double t[]); 18 | 19 | /* 20 | * absolute orientation using only three point pairs 21 | */ 22 | bool absOrient3(const double M1[], 23 | const double M2[], 24 | const double M3[], 25 | const double M1_[], 26 | const double M2_[], 27 | const double M3_[], 28 | double R[], 29 | double t[]); 30 | 31 | /** 32 | * weighted absolute orientation algorithm using svd 33 | */ 34 | void aoGetMatrixWeighted(const int npts, const double w[], const double r1[], const double r2[], double M[]); 35 | void aoGetOptRotation(const double M[], double R[]); 36 | void absOrientWeighted(const int npts, 37 | const double w[], 38 | const double pts1[], 39 | const double pts2[], 40 | double R[], 41 | double t[]); 42 | /** 43 | * apply reweighted least square to obtain the robust estimation using Tukey estimator 44 | */ 45 | void absOrientRobustTukey(const int npts, 46 | const double pts1[], 47 | const double pts2[], 48 | double R[], 49 | double t[], 50 | int maxIter = 10); 51 | /** 52 | * apply reweighted least square to obtain the robust estimation using Tukey estimator 53 | */ 54 | void absOrientRobustL1(const int npts, 55 | const double pts1[], 56 | const double pts2[], 57 | double R[], 58 | double t[], 59 | int maxIter = 10); 60 | #endif /* SL_AO_H_ */ 61 | -------------------------------------------------------------------------------- /src/geometry/SL_BundleAdjust.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_BundleAdjust.h 3 | * 4 | * Created on: 2011-7-12 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_BUNDLEADJUST_H_ 9 | #define SL_BUNDLEADJUST_H_ 10 | #include "math/SL_Matrix.h" 11 | #include "SL_Point.h" 12 | #include "SL_BundleHelper.h" 13 | 14 | #include 15 | #include 16 | 17 | class Meas2D { 18 | public: 19 | int viewId; 20 | union { 21 | struct { 22 | double x, y; 23 | }; 24 | double m[2]; 25 | }; 26 | double w; 27 | double cov[4]; 28 | int outlier; 29 | public: 30 | Meas2D() { 31 | cov[0] = cov[1] = cov[2] = cov[3] = 0; 32 | } 33 | Meas2D(int camId, double fx, double fy) : 34 | viewId(camId), x(fx), y(fy), w(1.0), outlier(0) { 35 | cov[0] = cov[1] = cov[2] = cov[3] = 0; 36 | } 37 | }; 38 | 39 | inline bool operator <(const Meas2D& item1, const Meas2D& item2) { 40 | return item1.viewId < item2.viewId; 41 | } 42 | 43 | void bundleAdjust(int nconcam, const std::vector& Ks, /*intrinsic matrices*/ 44 | std::vector& Rs, /*rotation matrices*/ 45 | std::vector& ts, /*translations*/ 46 | int nconpts, std::vector& pts3d, /*3D points*/ 47 | std::vector >& pts2d, /*2D projections*/ 48 | int nIter = 50, double* info = 0); 49 | /* 50 | * use M-estimator (Tukey) 51 | */ 52 | void bundleAdjustRobust(int nconcam, const std::vector& Ks, 53 | std::vector& Rs, std::vector& ts, int nconpts, 54 | std::vector& pts3d, std::vector >& pts2d, 55 | int maxErr = 3, int nIter = 10, int nInnerIter = 50); 56 | 57 | #endif /* SL_BUNDLEADJUST_H_ */ 58 | -------------------------------------------------------------------------------- /src/geometry/SL_ConvexHull2D.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_ConvexHull2D.cpp 3 | * 4 | * Created on: Feb 19, 2012 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_ConvexHull2D.h" 9 | #include 10 | 11 | static int ccw(coord **P, int i, int j, int k) { 12 | coord a = P[i][0] - P[j][0], b = P[i][1] - P[j][1], c = P[k][0] - P[j][0], d = P[k][1] - P[j][1]; 13 | return a * d - b * c <= 0; /* true if points i, j, k counterclockwise */ 14 | } 15 | 16 | #define CMPM(c,A,B) \ 17 | v = (*(coord**)A)[c] - (*(coord**)B)[c];\ 18 | if (v>0) return 1;\ 19 | if (v<0) return -1; 20 | 21 | static int cmpl(const void *a, const void *b) { 22 | double v; 23 | CMPM(0,a,b); 24 | CMPM(1,b,a); 25 | return 0; 26 | } 27 | 28 | static int cmph(const void *a, const void *b) { 29 | return cmpl(b, a); 30 | } 31 | 32 | static int makeChain(coord** V, int n, int(*cmp)(const void*, const void*)) { 33 | int i, j, s = 1; 34 | coord* t; 35 | 36 | qsort(V, n, sizeof(coord*), cmp); 37 | for (i = 2; i < n; i++) { 38 | for (j = s; j >= 1 && ccw(V, i, j, j - 1); j--) { 39 | } 40 | s = j + 1; 41 | t = V[s]; 42 | V[s] = V[i]; 43 | V[i] = t; 44 | } 45 | return s; 46 | } 47 | 48 | static int ch2d(coord **P, int n) { 49 | int u = makeChain(P, n, cmpl); /* make lower hull */ 50 | if (!n) 51 | return 0; 52 | P[n] = P[0]; 53 | return u + makeChain(P + u, n - u + 1, cmph); /* make upper hull */ 54 | } 55 | 56 | #define CONVEX_HULL_MAX_N 10000 57 | void get2DConvexHull(const std::vector& pts, std::vector& cxh) { 58 | double points[CONVEX_HULL_MAX_N][2], *P[CONVEX_HULL_MAX_N]; 59 | int n = 0; 60 | const size_t npts = pts.size() / 2; 61 | for (size_t i = 0; i < npts; i++) { 62 | points[i][0] = pts[2 * i]; 63 | points[i][1] = pts[2 * i + 1]; 64 | P[i] = points[i]; 65 | n++; 66 | } 67 | 68 | int m = ch2d(P, n); 69 | cxh.reserve(4 * m); 70 | 71 | for (int i = 0; i < m; i++) { 72 | cxh.push_back(P[i][0]); 73 | cxh.push_back(P[i][1]); 74 | } 75 | } 76 | double getPolyArea(const std::vector& poly) { 77 | size_t npts = poly.size() / 2; 78 | if (npts < 3) 79 | return 0; 80 | double s = 0; 81 | for (size_t i = 1; i < npts; i++) { 82 | double x0 = poly[2 * i - 2]; 83 | double y0 = poly[2 * i - 1]; 84 | double x1 = poly[2 * i]; 85 | double y1 = poly[2 * i + 1]; 86 | s += x0 * y1 - x1 * y0; 87 | } 88 | if (poly[0] != poly[2 * npts - 2] || poly[1] != poly[2 * npts - 1]) { 89 | s += poly[2 * npts - 2] * poly[1] - poly[2 * npts - 1] * poly[0]; 90 | } 91 | return 0.5 * fabs(s); 92 | } -------------------------------------------------------------------------------- /src/geometry/SL_ConvexHull2D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_ConvexHull2D.h 3 | * 4 | * Created on: 2011-5-10 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_CONVEXHULL2D_H_ 9 | #define SL_CONVEXHULL2D_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | typedef double coord; 15 | void get2DConvexHull(const std::vector& pts, std::vector& cxh); 16 | double getPolyArea(const std::vector& poly); 17 | #endif /* SL_CONVEXHULL2D_H_ */ 18 | -------------------------------------------------------------------------------- /src/geometry/SL_Depth.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Depth.cpp 3 | * 4 | * Created on: Feb 14, 2012 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_Depth.h" 9 | #include "math/SL_LinAlg.h" 10 | #include "geometry/SL_Distortion.h" 11 | #include "geometry/SL_RigidTransform.h" 12 | #include 13 | void depth2point3d(double nx, double ny, double d, double M0[3]) { 14 | double s = sqrt(nx * nx + ny * ny + 1); 15 | M0[0] = nx * d / s; 16 | M0[1] = ny * d / s; 17 | M0[2] = d / s; 18 | } 19 | void get3DCoord(const double* K, const double* R, const double* t, int u, int v, 20 | double d, double M[3]) { 21 | double invK[9]; 22 | double M0[3]; 23 | double nx, ny; 24 | mat33Inv(K, invK); 25 | normPoint(invK, (double) u, (double) v, nx, ny); 26 | depth2point3d(nx, ny, (K[0] * K[4]) < 0 ? -d : d, M0); 27 | invRigidTrans(R, t, M0, M); 28 | } 29 | 30 | -------------------------------------------------------------------------------- /src/geometry/SL_Depth.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Depth.h 3 | * 4 | * Created on: Feb 14, 2012 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_DEPTH_H_ 9 | #define SL_DEPTH_H_ 10 | 11 | void depth2point3d(double nx, double ny, double d, double M0[3]); 12 | void get3DCoord(const double* K, const double* R, const double* t, int u, int v, 13 | double d, double M[3]); 14 | 15 | #endif /* SL_DEPTH_H_ */ 16 | -------------------------------------------------------------------------------- /src/geometry/SL_Distortion.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Distortion.h 3 | * 4 | * Created on: 2010-11-6 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_DISTORTION_H_ 10 | #define SL_DISTORTION_H_ 11 | 12 | /*get the max range of the normalized image plane 13 | * input: 14 | * w, h : image size 15 | * iK : inverse of the intrinsic matrix 16 | * output: 17 | * return the maximum radius 18 | */ 19 | double maxRangeNormPlane(int w, int h, const double* iK); 20 | /* compute the polynomial for inverting the distortion 21 | * r : maximum range of image points 22 | * kc : distortion parameters (5x1) 23 | * k_ud : undistortion polynomial (stored as a 7x1 vector) 24 | */ 25 | void invDistorParam(double r, const double *kc, double *k_ud); 26 | /* compute the polynomial for inverting the distortion 27 | * w, h : image size 28 | * kc : distortion parameters (5x1) 29 | * k_ud : undistortion polynomial (stored as a 7x1 vector) 30 | */ 31 | void invDistorParam(int w, int h, const double* iK, const double* kc, double* k_ud); 32 | /* get the normalized image points with distortion removal 33 | * iK : inverse of intrinsic matrix 34 | * k_ud : undistortion parameters 35 | * n : number of points 36 | * a : nx2 array to store the image points 37 | * an : nx2 array to store the normalized image points 38 | */ 39 | void undistorNormPoints(const double* iK, const double* k_ud, int n, const double* a, double* an); 40 | void undistorPoint(const double* K, const double* k_ud, const double* pt, double * undistPt); 41 | 42 | /*get the normalize image points pn = K^-1 p*/ 43 | void normPoints(const double* iK, int n, const double* a, double *an); 44 | void normPoint(const double* iK, const double* a, double *an); 45 | void normPoint(const double* iK, const double x, const double y, double& xNorm, double& yNorm); 46 | 47 | void getInvK(const double* K, double* invK); 48 | /*get the image points from the normalized ones*/ 49 | void imagePoints(const double* K, int n, const double* an, double* a); 50 | void imagePoint(const double* K, const double* an, double* a); 51 | void imagePoints(const double* K, const double* kc, int n, const double* an, double* a); 52 | #endif /* SL_DISTORTION_H_ */ 53 | -------------------------------------------------------------------------------- /src/geometry/SL_EMatWrapper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_EssentialMatWrapper.cpp 3 | * 4 | * Created on: 2011-7-2 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_EMatWrapper.h" 9 | #include "SL_Distortion.h" 10 | 11 | bool estimateEMat(const double* K1, 12 | const double* K2, 13 | const Mat_d& pts1, 14 | const Mat_d& pts2, 15 | const Matching& matches, 16 | double E[9], 17 | int nRansac, 18 | double maxEpiErr, 19 | int minInlierNum) { 20 | int nm = matches.num; 21 | Mat_d pts1m(nm, 2), pts2m(nm, 2); 22 | Mat_d pts1mNorm(nm, 2), pts2mNorm(nm, 2); 23 | 24 | //get the matched points 25 | for (int i = 0; i < nm; i++) { 26 | int idx1 = matches[i].idx1; 27 | int idx2 = matches[i].idx2; 28 | 29 | pts1m.data[2 * i] = pts1.data[2 * idx1]; 30 | pts1m.data[2 * i + 1] = pts1.data[2 * idx1 + 1]; 31 | 32 | pts2m.data[2 * i] = pts2[2 * idx2]; 33 | pts2m.data[2 * i + 1] = pts2[2 * idx2 + 1]; 34 | } 35 | 36 | double iK1[9], iK2[9]; 37 | mat33Inv(K1, iK1); 38 | mat33Inv(K2, iK2); 39 | 40 | //get the normalized coordinates 41 | normPoints(iK1, nm, pts1m.data, pts1mNorm.data); 42 | normPoints(iK2, nm, pts2m.data, pts2mNorm.data); 43 | 44 | int inlierNum = findEMatRansac(iK2, iK1, nm, pts2m.data, pts1m.data, pts2mNorm.data, pts1mNorm.data, E, nRansac, 45 | maxEpiErr); 46 | 47 | if (inlierNum < minInlierNum) 48 | return false; 49 | return true; 50 | } 51 | 52 | bool estimateEMat(const double* K1, const double* K2, const Mat_d& pts1, const Mat_d& pts2, double E[9], int nRansac, double maxEpiErr, int minInlierNum) { 53 | assert( pts1.m > 0 && pts1.m == pts2.m); 54 | int nm = pts1.m; 55 | 56 | Mat_d pts1Norm(nm, 2), pts2Norm(nm, 2); 57 | 58 | double iK1[9], iK2[9]; 59 | mat33Inv(K1, iK1); 60 | mat33Inv(K2, iK2); 61 | 62 | //get the normalized coordinates 63 | normPoints(iK1, nm, pts1.data, pts1Norm.data); 64 | normPoints(iK2, nm, pts2.data, pts2Norm.data); 65 | 66 | int inlierNum = findEMatRansac(iK2, iK1, nm, pts1.data, pts2.data, pts1Norm.data, pts2Norm.data, E, nRansac, maxEpiErr); 67 | 68 | if (inlierNum < minInlierNum) 69 | return false; 70 | return true; 71 | } 72 | -------------------------------------------------------------------------------- /src/geometry/SL_EMatWrapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_EMatWrapper.h 3 | * 4 | * Created on: 2011-7-2 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_EMATWRAPPER_H_ 9 | #define SL_EMATWRAPPER_H_ 10 | #include "math/SL_LinAlgWarper.h" 11 | #include "geometry/SL_5point.h" 12 | #include "matching/SL_Matching.h" 13 | bool estimateEMat( 14 | const double* K1, 15 | const double* K2, 16 | const Mat_d& surfPts1, 17 | const Mat_d& surfPts2, 18 | const Matching& matches, 19 | double E[9], 20 | int nRansac, 21 | double maxEpiErr, 22 | int minInlierNum); 23 | bool estimateEMat( 24 | const double* K1, 25 | const double* K2, 26 | const Mat_d& surfPts1, 27 | const Mat_d& surfPts2, 28 | double E[9], 29 | int nRansac, 30 | double maxEpiErr, 31 | int minInlierNum); 32 | #endif /* SL_EMATWRAPPER_H_ */ 33 | -------------------------------------------------------------------------------- /src/geometry/SL_FundamentalMatrix.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_FundamentalMatrix.cpp 3 | * 4 | * Created on: 2010-11-12 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_FundamentalMatrix.h" 9 | #include "math/SL_LinAlg.h" 10 | #include "SL_error.h" 11 | #include "SL_Triangulate.h" 12 | #include 13 | #define Vx(v) (v)[0] 14 | #define Vy(v) (v)[1] 15 | #define Vz(v) (v)[2] 16 | 17 | /* m2^T*F*m1*/ 18 | double epipolarError(const double F[9], const double m2[2], const double m1[2]) { 19 | double Fl[3], Fr[3], pt; 20 | 21 | Fl[0] = F[0] * m1[0] + F[1] * m1[1] + F[2] * 1; 22 | Fl[1] = F[3] * m1[0] + F[4] * m1[1] + F[5] * 1; 23 | Fl[2] = F[6] * m1[0] + F[7] * m1[1] + F[8] * 1; 24 | 25 | Fr[0] = F[0] * m2[0] + F[3] * m2[1] + F[6] * 1; 26 | Fr[1] = F[1] * m2[0] + F[4] * m2[1] + F[7] * 1; 27 | Fr[2] = F[2] * m2[0] + F[5] * m2[1] + F[8] * 1; 28 | 29 | pt = m2[0] * Fl[0] + m2[1] * Fl[1] + 1 * Fl[2]; 30 | 31 | return 0.5 * (fabs(pt) / sqrt(Fl[0] * Fl[0] + Fl[1] * Fl[1]) + fabs(pt) / sqrt(Fr[0] * Fr[0] + Fr[1] * Fr[1])); 32 | } 33 | 34 | double epipolarError(const double* invKr, const double* invKl, const double* E, const double* m2, const double* m1) { 35 | double El[3], S; 36 | El[0] = E[0] * m1[0] + E[1] * m1[1] + E[2] * 1; 37 | El[1] = E[3] * m1[0] + E[4] * m1[1] + E[5] * 1; 38 | El[2] = E[6] * m1[0] + E[7] * m1[1] + E[8] * 1; 39 | S = m2[0] * El[0] + m2[1] * El[1] + 1 * El[2]; 40 | double A[9], B[9]; 41 | mat33ATB(invKr, E, A); 42 | mat33AB(E, invKl, B); 43 | double l1[3], l2[3]; 44 | mat33ProdVec(A, m1, l1); 45 | mat33TransProdVec(B, m2, l2); 46 | 47 | return 0.5 * (fabs(S) / sqrt(l1[0] * l1[0] + l1[1] * l1[1]) + fabs(S) / sqrt(l2[0] * l2[0] + l2[1] * l2[1])); 48 | } 49 | double epipolarErrorAvg(int n, const double* F, const double *m2, const double *m1) { 50 | int i; 51 | double s = 0; 52 | for (i = 0; i < n; ++i) { 53 | s += epipolarError(m2 + 2 * i, F, m1 + 2 * i); 54 | } 55 | return s / n; 56 | } 57 | -------------------------------------------------------------------------------- /src/geometry/SL_FundamentalMatrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_FundamentalMatrix.h 3 | * 4 | * Created on: 2010-11-12 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_FMATRIX_H_ 9 | #define SL_FMATRIX_H_ 10 | 11 | /*Given a fundamental matrix , compute the epipolar error of the corresponding points*/ 12 | double epipolarError(const double *F, const double* m2, const double* m1); 13 | /* Compute the epipolar error given inverse of intrinsic matrices and the essential matrix 14 | * input: 15 | * invKr, invKl : inverse of intrinsic matrices 16 | * E : essential matrix 17 | * rn , ln : normalized points on each camera 18 | */ 19 | double epipolarError(const double* invKr, const double* invKl, const double* E, const double* rn, const double* ln); 20 | double epipolarErrorAvg(int npts, const double* F, const double *m2, const double *m1 ); 21 | 22 | #endif /* SL_FMATRIX_H_ */ 23 | -------------------------------------------------------------------------------- /src/geometry/SL_Geometry.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Geometry.cpp 3 | * 4 | * Created on: Dec 15, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_Geometry.h" 9 | #include "SL_ConvexHull2D.h" 10 | #include "math/SL_LinAlg.h" 11 | 12 | #include 13 | #include 14 | using namespace std; 15 | double dist3(const double* v1, const double* v2) { 16 | double dx = v1[0] - v2[0]; 17 | double dy = v1[1] - v2[1]; 18 | double dz = v1[2] - v2[2]; 19 | return sqrt(dx * dx + dy * dy + dz * dz); 20 | } 21 | double dist2(const double* v1, const double* v2) { 22 | double dx = v1[0] - v2[0]; 23 | double dy = v1[1] - v2[1]; 24 | return sqrt(dx * dx + dy * dy); 25 | } 26 | double dist2(const double x1, const double y1, const double x2, 27 | const double y2) { 28 | double dx = x1 - x2; 29 | double dy = y1 - y2; 30 | return sqrt(dx * dx + dy * dy); 31 | } 32 | 33 | void crossProd(const double* v1, const double* v2, double* v3) { 34 | v3[0] = v1[1] * v2[2] - v1[2] * v2[1]; 35 | v3[1] = v1[2] * v2[0] - v1[0] * v2[2]; 36 | v3[2] = v1[0] * v2[1] - v1[1] * v2[0]; 37 | } 38 | 39 | void crossProd(double x1, double y1, double z1, double x2, double y2, double z2, 40 | double* v3) { 41 | v3[0] = y1 * z2 - z1 * y2; 42 | v3[1] = z1 * x2 - x1 * z2; 43 | v3[2] = x1 * y2 - y1 * x2; 44 | } 45 | 46 | void crossProd(double x1, double y1, double z1, double x2, double y2, double z2, 47 | double& x3, double& y3, double& z3) { 48 | x3 = y1 * z2 - z1 * y2; 49 | y3 = z1 * x2 - x1 * z2; 50 | z3 = x1 * y2 - y1 * x2; 51 | } 52 | 53 | void crossMat(const double* v, double* cmat) { 54 | matFill(3, 3, cmat, 0); 55 | cmat[1] = -v[2]; 56 | cmat[2] = v[1]; 57 | cmat[5] = -v[0]; 58 | cmat[3] = v[2]; 59 | cmat[6] = -v[1]; 60 | cmat[7] = v[0]; 61 | } 62 | double covInnerProd(int m, const double* C, const double* v1, 63 | const double* v2) { 64 | double * t = new double[m]; 65 | matAxpy(m, m, 1.0, C, v2, 0, 0, t); 66 | double d = inner_product(t, t + m, v1, 0.0); 67 | delete[] t; 68 | return d; 69 | } 70 | double covInnerProd3(const double* C, const double* v1, const double* v2) { 71 | double t[3]; 72 | mat33ProdVec(C, v2, t); 73 | return inner_product(t, t + 3, v1, 0.0); 74 | } 75 | 76 | double getAbsRadiansBetween(const double A[3], const double B[3], 77 | const double C[3]) { 78 | double AB[3] = { B[0] - A[0], B[1] - A[1], B[2] - A[2] }; 79 | double AC[3] = { C[0] - A[0], C[1] - A[1], C[2] - A[2] }; 80 | 81 | double s = innerProd3(AB, AC, 0); 82 | double n1 = vec3Len(AB); 83 | double n2 = vec3Len(AC); 84 | 85 | return fabs(acos(s / (n1 * n2))); 86 | } 87 | 88 | void poly2Mask(const std::vector& pts, ImgG& mask, uchar bk, uchar fg) { 89 | assert(!mask.empty()); 90 | mask.fill(bk); 91 | cv::Mat cvMask(mask.m, mask.n, CV_8UC1, mask.data); 92 | int npts = pts.size() / 2; 93 | cv::Point2i* cvPts = new cv::Point2i[npts]; 94 | for (int i = 0; i < npts; ++i) { 95 | cvPts[i].x = pts[2 * i]; 96 | cvPts[i].y = pts[2 * i + 1]; 97 | } 98 | cv::fillConvexPoly(cvMask, cvPts, npts, 99 | cv::Scalar(fg)); 100 | 101 | delete[] cvPts; 102 | } -------------------------------------------------------------------------------- /src/geometry/SL_Geometry.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Geometry.h 3 | * 4 | * Created on: Dec 15, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SLGEOMETRY_H_ 9 | #define SLGEOMETRY_H_ 10 | #include 11 | #include 12 | #include "imgproc/SL_Image.h" 13 | /* compute 3d distance*/ 14 | double dist3(const double* v1, const double* v2); 15 | /* compute 2d distance*/ 16 | double dist2(const double* v1, const double* v2); 17 | double dist2(const double x1, const double y1, const double x2, 18 | const double y2); 19 | 20 | /*cross production v3 = v1xv2 */ 21 | void crossProd(const double* v1, const double* v2, double* v3); 22 | void crossProd(double x1, double y1, double z1, double x2, double y2, double z2, 23 | double* v3); 24 | 25 | void crossProd(double x1, double y1, double z1, double x2, double y2, double z2, 26 | double& x3, double& y3, double& z3); 27 | 28 | void crossMat(const double* v, double* cmat); 29 | 30 | /* compute MahalanobisDist */ 31 | inline double mahaDist2(const double* v1, const double* v2, 32 | const double ivar[4]) { 33 | double dx = v1[0] - v2[0]; 34 | double dy = v1[1] - v2[1]; 35 | return sqrt( 36 | ivar[0] * dx * dx + ivar[1] * dx * dy + ivar[2] * dx * dy 37 | + ivar[3] * dy * dy); 38 | } 39 | inline double mahaDist2(double x1, double y1, double x2, double y2, 40 | const double ivar[4]) { 41 | double dx = x1 - x2; 42 | double dy = y1 - y2; 43 | return sqrt( 44 | ivar[0] * dx * dx + ivar[1] * dx * dy + ivar[2] * dx * dy 45 | + ivar[3] * dy * dy); 46 | } 47 | 48 | /*compute inner production with covariance matrix*/ 49 | double covInnerProd(int m, const double* C, const double* v1, const double* v2); 50 | /* C:3x3 matrix v1,v2 : 3x1 vectors */ 51 | double covInnerProd3(const double* C, const double* v1, const double* v2); 52 | 53 | /** 54 | * get the absolute angle between points B-A-C 55 | */ 56 | double getAbsRadiansBetween(const double A[3], const double B[3], 57 | const double C[3]); 58 | 59 | /** 60 | * convert polygon to mask image 61 | * 1 : in the region spanned by the polygon 62 | * 0 : out of the polygon 63 | */ 64 | void poly2Mask(const std::vector& pts, ImgG& mask, uchar bk=0, uchar fg=255); 65 | #endif /* SLGEOMETRY_H_ */ 66 | -------------------------------------------------------------------------------- /src/geometry/SL_Point.h: -------------------------------------------------------------------------------- 1 | /* 2 | * point.h 3 | * 4 | * Created on: 2010-11-12 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef POINT_H_ 9 | #define POINT_H_ 10 | 11 | #include 12 | typedef long long longInt; 13 | using namespace std; 14 | 15 | template 16 | class Point2 { 17 | public: 18 | union { 19 | struct { 20 | T x, y; 21 | }; 22 | T m[2]; 23 | }; 24 | public: 25 | Point2(const Point2& other) : 26 | x(other.x), y(other.y) { 27 | } 28 | Point2() { 29 | } 30 | Point2(T x0, T y0) { 31 | set(x0, y0); 32 | } 33 | Point2& operator =(const Point2& other) { 34 | if (&other != this) { 35 | x = other.x; 36 | y = other.y; 37 | } 38 | return *this; 39 | } 40 | void set(double x0, double y0) { 41 | m[0] = x0; 42 | m[1] = y0; 43 | } 44 | }; 45 | 46 | typedef Point2 Point2d; 47 | typedef Point2 Point2f; 48 | typedef Point2 Point2i; 49 | 50 | template 51 | inline bool Point2CompareLess(const Point2& pt1, const Point2& pt2) { 52 | if (pt1.y < pt2.y) 53 | return true; 54 | if (pt1.y == pt2.y) { 55 | if (pt1.x <= pt2.x) 56 | return true; 57 | } 58 | return false; 59 | } 60 | 61 | template 62 | class Point3 { 63 | public: 64 | union { 65 | struct { 66 | T x, y, z; 67 | }; 68 | T M[3]; 69 | }; 70 | public: 71 | Point3() { 72 | } 73 | Point3(const Point3& other) : 74 | x(other.x), y(other.y), z(other.z){ 75 | } 76 | Point3(T x0, T y0, T z0) { 77 | set(x0, y0, z0); 78 | } 79 | 80 | Point3& operator =(const Point3& other) { 81 | if (&other == this) { 82 | return *this; 83 | } 84 | x = other.x; 85 | y = other.y; 86 | z = other.z; 87 | return *this; 88 | } 89 | 90 | void set(T x0, T y0, T z0) { 91 | M[0] = x0; 92 | M[1] = y0; 93 | M[2] = z0; 94 | } 95 | }; 96 | 97 | typedef Point3 Point3d; 98 | typedef Point3 Point3f; 99 | typedef Point3 Point3i; 100 | 101 | class Point3dId: public Point3d { 102 | public: 103 | longInt id; 104 | public: 105 | Point3dId() : 106 | id(-1) { 107 | } 108 | Point3dId(double x0, double y0, double z0, int id0) : 109 | Point3(x0, y0, z0), id(id0) { 110 | } 111 | }; 112 | typedef vector VecPoint2d; 113 | typedef vector VecPoint3d; 114 | 115 | #endif /* POINT_H_ */ 116 | -------------------------------------------------------------------------------- /src/geometry/SL_Quaternion.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Quaternion.h 3 | * 4 | * Created on: 2010-11-18 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_QUATERNION_H_ 10 | #define SL_QUATERNION_H_ 11 | 12 | double quat_len(const double* q); 13 | void quat_norm(const double* q, double* qn); 14 | /*qj = q^-1*/ 15 | void quat_conj(const double* q, double* qj); 16 | /*q = q1*q2*/ 17 | void quat_mult(const double* q1, const double* q2, double* q); 18 | 19 | /*rotate a point v by a quaternion q to produce a new point v1*/ 20 | void quat_rot(const double* v, const double* q, double* v1); 21 | 22 | /*rotate a point v around the axis rot_v 23 | * theta : rotation angle (in degree) 24 | */ 25 | void quat_rot(const double* v, const double* rot_v, double theta, double* v1); 26 | /*convert a rotation matrix into a quaternion*/ 27 | void mat2quat(const double* R, double*q); 28 | void quat2mat(const double* q, double* R); 29 | #endif /* SL_QUATERNION_H_ */ 30 | -------------------------------------------------------------------------------- /src/geometry/SL_RigidTransform.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_RigidTransform.cpp 3 | * 4 | * Created on: 2011-6-9 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_RigidTransform.h" 9 | #include "math/SL_LinAlg.h" 10 | void getRigidTransFromTo( 11 | const double R1[9], 12 | const double t1[3], 13 | const double R2[9], 14 | const double t2[3], 15 | double dR[9], 16 | double dt[3]) { 17 | //dR = R2* R1' 18 | mat33ABT(R2, R1, dR); 19 | //dt = - dR*t1 + t2; 20 | mat33ProdVec(dR, t1, t2, dt, -1.0, 1.0); 21 | } 22 | void rigidTransFromTo( 23 | const double R1[9], 24 | const double t1[3], 25 | const double dR[9], 26 | const double dt[3], 27 | double R2[9], 28 | double t2[3]) { 29 | //R2 = dR*R1; 30 | mat33AB(dR, R1, R2); 31 | //t2 = dR*t1 + dt 32 | mat33ProdVec(dR, t1, dt, t2, 1.0, 1.0); 33 | } 34 | //inverse transformation of (R,t) -> (R^T, -R^T*t) 35 | void invRigidTransFromTo( 36 | const double R[9], 37 | const double t[3], 38 | double iR[9], 39 | double it[3]) { 40 | mat33Trans(R, iR); 41 | mat33TransProdVec(R, t, it); 42 | it[0] = -it[0]; 43 | it[1] = -it[1]; 44 | it[2] = -it[2]; 45 | } 46 | void rigidTrans( 47 | const double* R, 48 | const double* t, 49 | int nPts, 50 | const double* ptsOld, 51 | double* ptsNew) { 52 | for (int i = 0; i < nPts; i++) { 53 | mat33ProdVec(R, ptsOld + 3 * i, t, ptsNew + 3 * i, 1.0, 1.0); 54 | } 55 | } 56 | void rigidTrans( 57 | const double* R, 58 | const double* t, 59 | const double ptOld[3], 60 | double ptNew[3]) { 61 | mat33ProdVec(R, ptOld, t, ptNew, 1.0, 1.0); 62 | } 63 | void invRigidTrans( 64 | const double* R, 65 | const double* t, 66 | const double pt1[3], 67 | double pt2[3]) { 68 | double iR[9], it[3]; 69 | invRigidTransFromTo(R, t, iR, it); 70 | rigidTrans(iR, it, pt1, pt2); 71 | } 72 | void approxRotationMat(const double *A, double* R) { 73 | double U[9], S[3], VT[9]; 74 | dgesvdFor(3, 3, A, U, S, VT); 75 | mat33AB(U, VT, R); 76 | } 77 | 78 | void getRelativePose( 79 | const double R1[9], 80 | const double t1[3], 81 | const double R2[9], 82 | const double t2[3], 83 | double dR[9], 84 | double dt[3]) { 85 | //dR = R2*R1' 86 | mat33ABT(R2, R1, dR); 87 | //dt = - dR*t1 + t2; 88 | mat33ProdVec(dR, t1, t2, dt, -1.0, 1.0); 89 | } -------------------------------------------------------------------------------- /src/geometry/SL_RigidTransform.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_RigidTransform.h 3 | * 4 | * Created on: 2011-6-9 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_RIGIDTRANSFORM_H_ 9 | #define SL_RIGIDTRANSFORM_H_ 10 | 11 | /** 12 | * get incremental transform [dR dt; 0 1]*[R1 t1; 0 1]= [R t; 0 1]; 13 | */ 14 | void getRigidTransFromTo( 15 | const double R1[9], 16 | const double t1[3], 17 | const double R[9], 18 | const double t[3], 19 | double dR[9], 20 | double dt[3]); 21 | /** 22 | * incremental transform [ dR dt; 0 1]* [R1 t1; 0 1] -> [R t; 0 1]; 23 | */ 24 | void rigidTransFromTo( 25 | const double R1[9], 26 | const double t1[3], 27 | const double dR[9], 28 | const double dt[3], 29 | double R[9], 30 | double t[3]); 31 | /* 32 | * inverse transformation of (R,t) -> (R^T, -R^T*t) 33 | */ 34 | void invRigidTransFromTo( 35 | const double R[9], 36 | const double t[3], 37 | double iR[9], 38 | double it[3]); 39 | /** 40 | * ptsNew[i] = R* ptsOld[i] + t 41 | */ 42 | void rigidTrans( 43 | const double* R, 44 | const double* t, 45 | int nPts, 46 | const double* ptsOld, 47 | double* ptsNew); 48 | /** 49 | * ptNew = R * ptsOld + t 50 | */ 51 | void rigidTrans( 52 | const double* R, 53 | const double* t, 54 | const double ptOld[3], 55 | double ptNew[3]); 56 | 57 | /** 58 | * pt2 = R^T*pt1 - R^T*t) 59 | */ 60 | void invRigidTrans( 61 | const double* R, 62 | const double* t, 63 | const double pt1[3], 64 | double pt2[3]); 65 | /** 66 | * approximate the rotation matrix by SVD 67 | */ 68 | void approxRotationMat(const double *A, double* R); 69 | 70 | /** 71 | * get the classic relative camera pose between two camera poses [R1 t1] to [R2 t2]: 72 | * [I 0] and [dR dt] 73 | */ 74 | void getRelativePose( 75 | const double R1[9], 76 | const double t1[3], 77 | const double R2[9], 78 | const double t2[3], 79 | double dR[9], 80 | double dt[3]); 81 | #endif /* SL_RIGIDTRANSFORM_H_ */ 82 | -------------------------------------------------------------------------------- /src/geometry/SL_p3p.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_p3p.h 3 | * 4 | * Created on: 2010-12-2 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_P3P_H_ 10 | #define SL_P3P_H_ 11 | 12 | /*three point pose estimation*/ 13 | 14 | /* get coeffs for equation: dij^2 = x_i^2 + x_j^2 - 2 x_i*x_j*cos(theta_ij) 15 | * input: 16 | * K : intrinsic matrix of the camera 17 | * M1,M2,M3 : three scene points. Each one is 3x1 vector 18 | * m1,m2,m2 : projections of the three points (2x1 vectors) 19 | * output: 20 | * d[3] : store d12 d13 d23 21 | * cs[3] : store cos(theta_12),cos(theta_13),cos(theta_23) 22 | */ 23 | void p3p_get_coeffs(const double K[], const double M1[], const double M2[], const double M3[], const double m1[], 24 | const double m2[], const double m3[], double d[], double cs[]); 25 | /* compute the quartic equation for x_1^2: 26 | * a*x_1^8 + b *x_1^6 + c*x_1^4 + d*x_1^2 + e = 0 27 | * input: 28 | * d[3] : d12,d13,d23 29 | * cs[3] : cos(theta_12),cos(theta_13),cos(theta_23) 30 | * output: 31 | * cfs[5] : a,b,c,d,e 32 | */ 33 | void p3p_get_quartic(const double d[], const double cs[], double cfs[]); 34 | 35 | /** 36 | * solve the equations to get x1,x2,x3 37 | * d12^2 = x1^2 + x2^2 - 2*x1*x2*cos(theta_12) 38 | * d13^2 = x1^2 + x2^2 - 2*x1*x3*cos(theta_13) 39 | * d23^2 = x2^2 + x3^2 - 2*x2*x3*cos(theta_23) 40 | * input: 41 | * d[3] : d12,d13,d23 42 | * cs[3] : cos(theta_12),cos(theta_13),cos(theta_23) 43 | * output: 44 | * x[] : x1,x2,x3 45 | * err[] : error 46 | * @return : number of solutions 47 | */ 48 | int p3p_solve_xs(const double d[], const double cs[], const double cfs[], double x[], double err[]); 49 | 50 | /** 51 | * select two best solutions and refine it 52 | */ 53 | int p3p_select_best_two(const int nx, const double x[], const double err[], double bx[], double berr[]); 54 | 55 | /** 56 | * three point algorithm to estimate the camera pose based on the following papers 57 | * 58 | * @article{quan2002linear, 59 | title={{Linear n-point camera pose determination}}, 60 | author={Quan, L. and Lan, Z.}, 61 | journal={Pattern Analysis and Machine Intelligence, IEEE Transactions on}, 62 | volume={21}, 63 | number={8}, 64 | pages={774--780}, 65 | issn={0162-8828}, 66 | year={2002}, 67 | publisher={IEEE} 68 | } 69 | * @article{horn1988closed, 70 | title={{Closed-form solution of absolute orientation using orthonormal matrices}}, 71 | author={Horn, B.K.P. and Hilden, H.M. and Negahdaripour, S.}, 72 | journal={Journal of the Optical Society of America A}, 73 | volume={5}, 74 | number={7}, 75 | pages={1127--1135}, 76 | issn={1084-7529}, 77 | year={1988}, 78 | publisher={Citeseer} 79 | } 80 | * 81 | */ 82 | int p3p(const double K[], const double M1[], const double M2[], const double M3[], const double m1[], 83 | const double m2[], const double m3[], double Rs[], double ts[]); 84 | 85 | #endif /* SL_P3P_H_ */ 86 | -------------------------------------------------------------------------------- /src/geometry/epnp.h: -------------------------------------------------------------------------------- 1 | #ifndef epnp_h 2 | #define epnp_h 3 | 4 | #include 5 | class epnp { 6 | public: 7 | epnp(void); 8 | ~epnp(); 9 | 10 | void set_internal_parameters(const double uc, const double vc, const double fu, const double fv); 11 | 12 | void set_maximum_number_of_correspondences(const int n); 13 | void reset_correspondences(void); 14 | void add_correspondence(const double X, const double Y, const double Z, const double u, const double v); 15 | 16 | double compute_pose(double R[3][3], double T[3]); 17 | 18 | void relative_error(double & rot_err, 19 | double & transl_err, 20 | const double Rtrue[3][3], 21 | const double ttrue[3], 22 | const double Rest[3][3], 23 | const double test[3]); 24 | 25 | void print_pose(const double R[3][3], const double t[3]); 26 | double reprojection_error(const double R[3][3], const double t[3]); 27 | 28 | private: 29 | void choose_control_points(void); 30 | void compute_barycentric_coordinates(void); 31 | void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); 32 | void compute_ccs(const double * betas, const double * ut); 33 | void compute_pcs(void); 34 | 35 | void solve_for_sign(void); 36 | 37 | void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); 38 | void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); 39 | void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); 40 | void qr_solve(CvMat * A, CvMat * b, CvMat * X); 41 | 42 | double dot(const double * v1, const double * v2); 43 | double dist2(const double * p1, const double * p2); 44 | 45 | void compute_rho(double * rho); 46 | void compute_L_6x10(const double * ut, double * l_6x10); 47 | 48 | void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); 49 | void 50 | compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, double cb[4], CvMat * A, CvMat * b); 51 | 52 | double compute_R_and_t(const double * ut, const double * betas, double R[3][3], double t[3]); 53 | 54 | void estimate_R_and_t(double R[3][3], double t[3]); 55 | 56 | void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], double R_src[3][3], double t_src[3]); 57 | 58 | void mat_to_quat(const double R[3][3], double q[4]); 59 | 60 | double uc, vc, fu, fv; 61 | 62 | double * pws, *us, *alphas, *pcs; 63 | int maximum_number_of_correspondences; 64 | int number_of_correspondences; 65 | 66 | double cws[4][3], ccs[4][3]; 67 | double cws_determinant; 68 | }; 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/geometry/quartSolver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * quartic.h 3 | * 4 | * Created on: 2010-12-2 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef QUARTIC_H_ 10 | #define QUARTIC_H_ 11 | 12 | int quadratic(double b, double c, double rts[]); 13 | int cubic(double p, double q, double r, double v3[]); 14 | void cubnewton(double p, double q, double r, int n3, double v3[]); 15 | int quartic(double a, double b, double c, double d, double rts[]); 16 | int descartes(double a, double b, double c, double d, double rts[]); 17 | int ferrari(double a, double b, double c, double d, double rts[]); 18 | int neumark(double a, double b, double c, double d, double rts[]); 19 | int yacfraid(double a, double b, double c, double d, double rts[]); 20 | int chris(double a, double b, double c, double d, double rts[]); 21 | #endif /* QUARTIC_H_ */ 22 | -------------------------------------------------------------------------------- /src/imgproc/SL_BoxFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_BoxFilter.cpp 3 | * 4 | * Created on: Dec 18, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_BoxFilter.h" -------------------------------------------------------------------------------- /src/imgproc/SL_BoxFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_BoxFilter.h 3 | * 4 | * Created on: Dec 18, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_BOXFILTER_H_ 9 | #define SL_BOXFILTER_H_ 10 | #include "math/SL_Matrix.h" 11 | #include "util/SL_Utility.h" 12 | 13 | template 14 | void boxFilt2(const MyMat& in, MyMat& out, int r) { 15 | assert(r > 0); 16 | out.resize(in.m, in.n); 17 | for (int i = 0; i < in.m; i++) { 18 | for (int j = 0; j < in.n; j++) { 19 | U v = 0; 20 | for (int s = -r; s <= r; ++s) { 21 | for (int t = -r; t <= r; ++t) { 22 | int ii = i + s; 23 | int jj = j + t; 24 | if (ii < 0 || ii >= in.m || jj < 0 || jj >= in.n) 25 | continue; 26 | v += static_cast(in.data[ii * in.n + jj]); 27 | } 28 | } 29 | out.data[i * in.n + j] = v; 30 | } 31 | } 32 | } 33 | template 34 | void fastBoxFilt2(int m, int n, const T* in, U* out, int r) { 35 | MyMat tempSum(m, n); 36 | cumSum(m, n, in, tempSum.data, false); 37 | 38 | MyMat tmpRow(m, n); 39 | for (int j = 0; j < n; ++j) 40 | for (int i = 0; i <= r; ++i) 41 | tmpRow.data[i * n + j] = tempSum.data[(i + r) * n + j]; 42 | 43 | for (int j = 0; j < n; ++j) 44 | for (int i = r + 1; i <= m - r - 1; ++i) 45 | tmpRow.data[i * n + j] = tempSum.data[(i + r) * n + j] 46 | - tempSum.data[(i - r - 1) * n + j]; 47 | for (int j = 0; j < n; ++j) 48 | for (int i = m - r; i < m; ++i) 49 | tmpRow.data[i * n + j] = tempSum.data[(m - 1) * n + j] 50 | - tempSum.data[(i - r - 1) * n + j]; 51 | 52 | cumSum(m, n, tmpRow.data, tempSum.data, true); 53 | 54 | for (int i = 0; i < m; ++i) 55 | for (int j = 0; j <= r; ++j) 56 | out[i * n + j] = tempSum.data[i * n + j + r]; 57 | 58 | for (int i = 0; i < m; ++i) 59 | for (int j = r + 1; j <= n - r - 1; ++j) 60 | out[i * n + j] = tempSum.data[i * n + j + r] 61 | - tempSum.data[i * n + j - r - 1]; 62 | 63 | for (int i = 0; i < m; ++i) 64 | for (int j = n - r; j < n; ++j) 65 | out[i * n + j] = tempSum.data[i * n + n - 1] 66 | - tempSum.data[i * n + j - r - 1]; 67 | } 68 | /* fast box filter using cumsum*/ 69 | template 70 | void fastBoxFilt2(const MyMat& in, MyMat& out, int r) { 71 | assert(r > 0); 72 | out.resize(in.m, in.n); 73 | fastBoxFilt2(in.m, in.n, in.data, out.data, r); 74 | } 75 | #endif /* SL_BOXFILTER_H_ */ 76 | -------------------------------------------------------------------------------- /src/imgproc/SL_Gradient.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Gradient.h 3 | * 4 | * Created on: Feb 13, 2012 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_GRADIENT_H_ 9 | #define SL_GRADIENT_H_ 10 | #include 11 | template 12 | void compute_dpx(const U& I, V& Ix) { 13 | assert(Ix.m == I.m && Ix.n == I.n); 14 | int m = I.m; 15 | int n = I.n; 16 | for (int i = 0; i < m; ++i) { 17 | int j = 0; 18 | for (; j < n -1;++j) { 19 | Ix.data[i * n + j ] = I.data[i * n + j+1] - I.data[i * n + j]; 20 | } 21 | Ix.data[i * n + j ] = 0; 22 | } 23 | } 24 | template 25 | void compute_dmx(const U& I, V& Ix) { 26 | assert(Ix.m == I.m && Ix.n == I.n); 27 | int m = I.m; 28 | int n = I.n; 29 | for (int i = 0; i < m; ++i) { 30 | Ix.data[i * n] = I.data[i * n]; 31 | int j = 1; 32 | for (; j < n - 1; ++j) { 33 | Ix.data[i * n + j] = I.data[i * n + j] - I.data[i * n + j - 1]; 34 | } 35 | Ix.data[i * n + j ] = -I.data[i *n + j]; 36 | } 37 | } 38 | 39 | template 40 | void compute_dpy(const U& I, V& Iy) { 41 | assert(Iy.m == I.m && Iy.n == I.n); 42 | int m = I.m; 43 | int n = I.n; 44 | for (int j = 0; j < n; ++j) { 45 | int i = 0; 46 | for (; i < m - 1; ++i) { 47 | Iy.data[i * n + j] = I.data[(i+1) * n + j] - I.data[i * n + j]; 48 | } 49 | Iy.data[i * n + j] = 0; 50 | } 51 | } 52 | template 53 | void compute_dmy(const U& I, V& Iy) { 54 | assert(Iy.m == I.m && Iy.n == I.n); 55 | int m = I.m; 56 | int n = I.n; 57 | for (int j = 0; j < n; ++j) { 58 | Iy.data[j] = I.data[j]; 59 | int i = 1; 60 | for (; i < m - 1; ++i) { 61 | Iy.data[i * n + j] = I.data[i * n + j] - I.data[(i - 1) * n + j]; 62 | } 63 | 64 | Iy.data[i* n + j] = - I.data[(i-1)*n + j]; 65 | } 66 | } 67 | template 68 | void compute_div(const U& Ix, const U& Iy, V& divI) { 69 | assert(Ix.m == Iy.m && Ix.n == Iy.n); 70 | V Ixx(Ix.m, Ix.n); 71 | V Iyy(Iy.m, Iy.n); 72 | compute_dmx(Ix, Ixx); 73 | compute_dmy(Iy, Iyy); 74 | 75 | int len = Ix.m * Ix.n; 76 | for (int i = 0; i < len; ++i) 77 | divI.data[i] = Ixx.data[i] + Iyy.data[i]; 78 | } 79 | #endif /* SL_GRADIENT_H_ */ 80 | -------------------------------------------------------------------------------- /src/imgproc/SL_HarrisMatrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_HarrisMatrix.h 3 | * 4 | * Created on: Feb 13, 2012 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_HARRISMATRIX_H_ 9 | #define SL_HARRISMATRIX_H_ 10 | #include "imgproc/SL_Image.h" 11 | /** 12 | * img : w x h 13 | * harrisMat : (w x h) x 4 matrix 14 | */ 15 | void computeHarrisMatrices(const ImgG& img, Mat_d& IxIx, Mat_d& IxIy, 16 | Mat_d& IyIy, double sigma); 17 | 18 | void computeEigenValues(const Mat_d& IxIx, const Mat_d& IxIy, const Mat_d& IyIy, 19 | Mat_d& v1, Mat_d& v2, Mat_d& lambda1, Mat_d& lambda2); 20 | 21 | void computeHarrisResponse(const Mat_d& IxIx, const Mat_d& IxIy, 22 | const Mat_d& IyIy, Mat_d& res, double kata); 23 | 24 | void computeCornerEig(const ImgG& img, Mat_d& lambda1, Mat_d& lambda2, 25 | Mat_d& v1, Mat_d& v2, int block_size); 26 | 27 | #endif /* SL_HARRISMATRIX_H_ */ 28 | -------------------------------------------------------------------------------- /src/imgproc/SL_Image.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Image.cpp 3 | * 4 | * Created on: 2010-11-8 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #include "SL_Image.h" 10 | #include 11 | ImgRGB::ImgRGB() : 12 | w(0), h(0), data(0) { 13 | 14 | } 15 | ImgRGB::ImgRGB(int w_, int h_) : 16 | w(0), h(0), data(0) { 17 | if (h_ > 0 && w_ > 0) { 18 | w = w_; 19 | h = h_; 20 | data = new uchar[w * h * 3]; 21 | } 22 | } 23 | ImgRGB::~ImgRGB() { 24 | clear(); 25 | } 26 | 27 | void ImgRGB::clear() { 28 | if (data) { 29 | delete[] data; 30 | data = 0; 31 | } 32 | w = 0; 33 | h = 0; 34 | } 35 | void ImgRGB::fill(uchar r, uchar g, uchar b) { 36 | int i, len = w * h; 37 | uchar* pdata = data; 38 | for (i = 0; i < len; i++) { 39 | pdata[0] = r; 40 | pdata[1] = g; 41 | pdata[2] = b; 42 | pdata += 3; 43 | } 44 | } 45 | void ImgRGB::resize(int wd, int ht) { 46 | clear(); 47 | if (ht > 0 && wd > 0) { 48 | w = wd; 49 | h = ht; 50 | data = new uchar[h * w * 3]; 51 | } 52 | } 53 | void ImgRGB::split(ImgG& R, ImgG& G, ImgG& B) const { 54 | R.resize(w, h); 55 | G.resize(w, h); 56 | B.resize(w, h); 57 | 58 | for (int i = 0; i < h; i++) { 59 | for (int j = 0; j < w; j++) { 60 | R.data[i * w + j] = data[3 * i * w + 3 * j]; 61 | G.data[i * w + j] = data[3 * i * w + 3 * j + 1]; 62 | B.data[i * w + j] = data[3 * i * w + 3 * j + 2]; 63 | } 64 | } 65 | } 66 | #include "tools/SL_Print.h" 67 | void ImgRGB::getStdAvg(int startx, int endx, int start_row, int end_row, 68 | double avg[3], double std[3]) const { 69 | std::fill_n(avg, 3, 0.0); 70 | for (int r = start_row; r < end_row && r < h; ++r) { 71 | for (int c = startx; c < endx && c < w; ++c) { 72 | avg[0] += data[3 * r * w + 3 * c]; 73 | avg[1] += data[3 * r * w + 3 * c + 1]; 74 | avg[2] += data[3 * r * w + 3 * c + 2]; 75 | } 76 | } 77 | int N = (end_row - start_row) * w; 78 | avg[0] /= N; 79 | avg[1] /= N; 80 | avg[2] /= N; 81 | 82 | std::fill_n(std, 3, 0.0); 83 | for (int r = start_row; r < end_row && r < h; ++r) { 84 | for (int c = startx; c < endx && c < w; ++c) { 85 | double dR = data[3 * r * w + 3 * c] - avg[0]; 86 | double dG = data[3 * r * w + 3 * c + 1] - avg[1]; 87 | double dB = data[3 * r * w + 3 * c + 2] - avg[2]; 88 | std[0] += dR * dR; 89 | std[1] += dG * dG; 90 | std[2] += dB * dB; 91 | } 92 | } 93 | std[0] /= N; 94 | std[1] /= N; 95 | std[2] /= N; 96 | } 97 | ImgG::ImgG() : 98 | w(0), h(0), data(0) { 99 | 100 | } 101 | ImgG::ImgG(int w_, int h_) : 102 | w(w_), h(h_), data(0) { 103 | if (h_ > 0 && w_ > 0) { 104 | w = w_; 105 | h = h_; 106 | data = new uchar[h * w]; 107 | } else { 108 | w = 0; 109 | h = 0; 110 | data = 0; 111 | } 112 | } 113 | ImgG::~ImgG() { 114 | clear(); 115 | } 116 | void ImgG::clear() { 117 | if (data) { 118 | delete[] data; 119 | data = 0; 120 | } 121 | w = 0; 122 | h = 0; 123 | } 124 | void ImgG::resize(int wd, int ht) { 125 | if (wd == w && ht == h) 126 | return; 127 | clear(); 128 | if (ht > 0 && wd > 0) { 129 | w = wd; 130 | h = ht; 131 | data = 0; 132 | data = new uchar[h * w]; 133 | } 134 | } -------------------------------------------------------------------------------- /src/imgproc/SL_Image.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Image.h 3 | * 4 | * Created on: 2010-11-8 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_IMAGE_H_ 10 | #define SL_IMAGE_H_ 11 | 12 | typedef unsigned char uchar; 13 | #include 14 | class ImgG { 15 | public: 16 | union { 17 | int n; 18 | int w; 19 | int cols; 20 | }; 21 | union { 22 | int m; 23 | int h; 24 | int rows; 25 | }; 26 | uchar* data; 27 | public: 28 | ImgG(); 29 | ImgG(int w, int h); 30 | ~ImgG(); 31 | 32 | public: 33 | void clear(); 34 | void fill(uchar val) { 35 | memset(data, val, w * h * sizeof(uchar)); 36 | } 37 | void resize(int w, int h); 38 | bool empty() const { 39 | return w == 0 || h == 0; 40 | } 41 | 42 | public: 43 | typedef uchar* PUCHAR; 44 | typedef const uchar* CONST_PUCHAR; 45 | operator PUCHAR() { 46 | return data; 47 | } 48 | operator CONST_PUCHAR() const { 49 | return data; 50 | } 51 | uchar* getPtr() const { 52 | return data; 53 | } 54 | uchar& operator()(int y, int x) const { 55 | return data[y * w + x]; 56 | } 57 | int getMemLen() const { 58 | return w * h; 59 | } 60 | }; 61 | 62 | class ImgRGB { 63 | public: 64 | union { 65 | int w; 66 | int n; 67 | int cols; 68 | }; 69 | union { 70 | int h; 71 | int m; 72 | int rows; 73 | }; 74 | uchar* data; 75 | public: 76 | ImgRGB(); 77 | ImgRGB(int w, int h); 78 | ~ImgRGB(); 79 | 80 | public: 81 | void clear(); 82 | void fill(uchar r, uchar g, uchar b); 83 | void resize(int w, int h); 84 | void split(ImgG& R, ImgG& G, ImgG& B) const; 85 | bool empty() const { 86 | return w == 0 || h == 0; 87 | } 88 | public: 89 | typedef uchar* PUCHAR; 90 | typedef const uchar* CONST_PUCHAR; 91 | operator PUCHAR() { 92 | return data; 93 | } 94 | operator CONST_PUCHAR() const { 95 | return data; 96 | } 97 | uchar* get_ptr() const { 98 | return data; 99 | } 100 | uchar* operator ()(int y, int x) const { 101 | return data + y * w * 3 + x * 3; 102 | } 103 | int getMemLen() const { 104 | return h * w * 3; 105 | } 106 | void getStdAvg( 107 | int startx, 108 | int endx, 109 | int starty, 110 | int endy, 111 | double avg[3], 112 | double std[3]) const; 113 | 114 | uchar* getRowPtr(int y) const { 115 | return data + y * w * 3; 116 | } 117 | }; 118 | 119 | #include "SL_FloatImage.h" 120 | typedef FloatImgG ImgG_d; 121 | typedef FloatImgG ImgG_f; 122 | typedef FloatImgRGB ImgRGB_d; 123 | typedef FloatImgRGB ImgRGB_f; 124 | #endif /* SL_IMAGE_H_ */ 125 | -------------------------------------------------------------------------------- /src/imgproc/SL_ImageIO.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_ImageIO.h 3 | * 4 | * Created on: Dec 22, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_IMAGEIO_H_ 9 | #define SL_IMAGEIO_H_ 10 | #include "SL_Image.h" 11 | /* save as ppm (color image)*/ 12 | void savePPM(const ImgRGB& rgb_img, const char* fmtstr, ...); 13 | void savePPM(const uchar* data, int w, int h, const char* fmtstr, ...); 14 | 15 | /* save as pgm (gray image)*/ 16 | void savePGM(const ImgG& gray_img, const char* fmtstr, ...); 17 | void savePGM(const uchar* data, int w, int h, const char* fmtstr, ...); 18 | 19 | /* read pgm files*/ 20 | void loadPGM(ImgG& gray_img, const char* fmtstr, ...); 21 | void loadPPM(ImgRGB& rgb_img, const char* fmtstr, ...); 22 | 23 | /*read image*/ 24 | void imread(ImgG& gray_img, const char* fmtstr, ...); 25 | void imread(ImgRGB& rgb_img, const char* fmtstr, ...); 26 | void imwrite(const ImgG& gray_img, const char* fmtstr, ...); 27 | void imwrite(const ImgRGB& rgb_img, const char* fmtstr, ...); 28 | 29 | #endif /* SL_IMAGEIO_H_ */ 30 | -------------------------------------------------------------------------------- /src/lap/LAP.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LAP.h 3 | * 4 | * Created on: 2009-8-14 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef LAP_H_ 9 | #define LAP_H_ 10 | 11 | void printMatrix(double W[] , int m , int n , double* u = 0 , double* v = 0); 12 | void printMatrix(double W[] , int m , int n , bool* rf , bool* cf); 13 | void printResult( 14 | double W[] , 15 | int m , 16 | int n , 17 | int* ar , 18 | int* ac , 19 | double* u = 0 , 20 | double* v = 0 , 21 | bool* rf = 0 , 22 | bool *cf = 0); 23 | 24 | int getProblematicRow(); 25 | /** 26 | * solve the lap by using successive shortest path(SSP) method 27 | */ 28 | double lapSSP(double W[] , int m , //mxn matrix (m < n) 29 | int n , 30 | int x[] , //columns assigned to rows 31 | int y[] , //rows assigned to columns 32 | double u[] , //dual row variables 33 | double v[] , //dual column variables 34 | double d[] , //shortest path lengths 35 | int frow[] , //free rows 36 | int pred[] , //predecessor nodes in the shortest path 37 | int q[] , //queue for store columns 38 | // scaned (k = 1...low -1) 39 | // labeled and unscanned (k = low ... up-1) 40 | // unlabeled ( k = up ... n) 41 | char rf[] , // flags for valid rows (true - valid) 42 | char cf[] // flags for valid columns (true - valid) 43 | ); 44 | 45 | /** 46 | * one step of successive shortest path algorithm 47 | */ 48 | double lapSSPOneStep( 49 | double W[] , 50 | int m , 51 | int n , 52 | int x[] , 53 | int y[] , 54 | double u[] , 55 | double v[] , 56 | double d[] , 57 | int row , 58 | int pred[] , 59 | int q[] , 60 | char rf[] , 61 | char cf[]); 62 | 63 | double lap(double W[] , int m , //mxn matrix (m < n) 64 | int n , 65 | int x[] , //columns assigned to rows (-1 with no assignment) 66 | int y[] , //rows assigned to columns 67 | char rf[] , //flags for valid rows (0 - invalid, 1 - valid) 68 | char cf[] //flags for valid columns ( 0 - invalid, 1 - valid); 69 | ); 70 | 71 | #endif /* LAP_H_ */ 72 | -------------------------------------------------------------------------------- /src/matching/SL_BlockDescriptorExtractor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BlockDescriptorExtractor.cpp 3 | * 4 | * Created on: 2011-1-8 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_BlockDescriptorExtractor.h" 9 | #include "opencv2/imgproc/imgproc.hpp" 10 | BlockDescriptorExtractor::BlockDescriptorExtractor() { 11 | 12 | } 13 | 14 | BlockDescriptorExtractor::~BlockDescriptorExtractor() { 15 | 16 | } 17 | 18 | void BlockDescriptorExtractor::compute(const ImgG& img , const Mat_d& pts , Mat_d& desc) { 19 | if (pts.empty()) 20 | repErr("BlockDescriptorExtractor::compute - no point is provided"); 21 | 22 | cv::Size patchSize(2 * hW + 1, 2 * hW + 1); 23 | int nPts = pts.rows; 24 | int len = (2 * hW + 1) * (2 * hW + 1); 25 | desc.resize(nPts, len); 26 | 27 | cv::Mat imgRef(img.rows, img.cols, CV_8UC1, img.data); 28 | 29 | if (scale == 1.0) { 30 | for (int i = 0; i < nPts; i++) { 31 | double x = pts.data[2 * i]; 32 | double y = pts.data[2 * i + 1]; 33 | 34 | cv::Mat patch; 35 | cv::getRectSubPix(imgRef, patchSize, cv::Point2f(x, y), patch); 36 | //cv::Scalar avg = cv::mean(patch); 37 | CvMat patchRef = patch; 38 | 39 | for (int j = 0; j < len; j++) 40 | //desc.data[i * len + j] = (patchRef.data.ptr[j] - avg(0)); 41 | desc.data[i * len + j] = patchRef.data.ptr[j]; 42 | } 43 | } else { 44 | cv::Mat tmpImg; 45 | cv::resize(imgRef, tmpImg, cv::Size(), scale, scale); 46 | for (int i = 0; i < nPts; i++) { 47 | double x = pts.data[2 * i] * scale; 48 | double y = pts.data[2 * i + 1] * scale; 49 | 50 | cv::Mat patch; 51 | cv::getRectSubPix(tmpImg, patchSize, cv::Point2f(x, y), patch); 52 | //cv::Scalar avg = cv::mean(patch); 53 | CvMat patchRef = patch; 54 | 55 | for (int j = 0; j < len; j++) 56 | //desc.data[i * len + j] = (patchRef.data.ptr[j] - avg(0)); 57 | desc.data[i * len + j] = patchRef.data.ptr[j]; 58 | } 59 | } 60 | } 61 | 62 | -------------------------------------------------------------------------------- /src/matching/SL_BlockDescriptorExtractor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BlockDescriptorExtractor.h 3 | * 4 | * Created on: 2011-1-8 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef BLOCKDESCRIPTOREXTRACTOR_H_ 9 | #define BLOCKDESCRIPTOREXTRACTOR_H_ 10 | #include "math/SL_Matrix.h" 11 | #include "imgproc/SL_Image.h" 12 | 13 | class BlockDescriptorExtractorParam { 14 | public: 15 | int hW; 16 | double scale; 17 | public: 18 | BlockDescriptorExtractorParam() { 19 | hW = 4; 20 | scale = 0.3; 21 | } 22 | }; 23 | class BlockDescriptorExtractor : public BlockDescriptorExtractorParam { 24 | public: 25 | BlockDescriptorExtractor(); 26 | ~BlockDescriptorExtractor(); 27 | public: 28 | void compute(const ImgG& img , const Mat_d& pts , Mat_d& desc); 29 | }; 30 | 31 | //the above will be replaced by the `getImageBlocks' in `SL_StereoMatcherHelper.h' 32 | #endif /* BLOCKDESCRIPTOREXTRACTOR_H_ */ 33 | -------------------------------------------------------------------------------- /src/matching/SL_GuidedNCCMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_GuidedNCCMatcher.h 3 | * 4 | * Created on: 2011-3-30 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_GUIDEDNCCMATCHER_H_ 9 | #define SL_GUIDEDNCCMATCHER_H_ 10 | #include "SL_Matching.h" 11 | #include "math/SL_Matrix.h" 12 | 13 | void getSeedDisparities(const Mat_d& seed1, const Mat_d& seed2, Mat_d& seed1Disp, Mat_d& seed2Disp); 14 | void getNearestSeeds(const Mat_d& corners, const Mat_d& seed, Mat_i& seedId); 15 | 16 | void getDisparityMat(const Mat_d& corners1, 17 | const Mat_d& corners2, 18 | const Mat_d& seed1, 19 | const Mat_d& seed2, 20 | double dispMax, 21 | Mat_d& dispMat); 22 | 23 | void greedyNCCMatch(const Mat_d& nccMat, Matching& matches); 24 | int greedyNCCMatch(const Mat_d& nccMat, Mat_i& jInd); 25 | void greedyGuidedNCCMatch(const Mat_d& nccMat, const Mat_d& dispMat, Matching& matches); 26 | void lapGuidedNCCMatch(const Mat_d& nccMat, const Mat_d& dispMat, Matching& matches, double largeWeight = 1e+5); 27 | #endif /* SL_GUIDEDNCCMATCHER_H_ */ 28 | -------------------------------------------------------------------------------- /src/matching/SL_Matching.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_MatchKeyPoint.h 3 | * 4 | * Created on: 2010-11-9 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_MATCHING 10 | #define SL_MATCHING 11 | 12 | #include 13 | #include 14 | #include "math/SL_Matrix.h" 15 | 16 | class MatchItem { 17 | public: 18 | int idx1, idx2; 19 | double dist; 20 | }; 21 | 22 | class Matching { 23 | protected: 24 | int _max_num; 25 | public: 26 | MatchItem* data; 27 | int num; 28 | public: 29 | Matching() : 30 | _max_num(0), data(0), num(0) { 31 | } 32 | ~Matching() { 33 | clear(); 34 | } 35 | bool empty() { 36 | return num == 0; 37 | } 38 | void clear() { 39 | if (data) 40 | delete[] data; 41 | _max_num = 0; 42 | num = 0; 43 | data = 0; 44 | } 45 | void reset() { 46 | num = 0; 47 | data = 0; 48 | } 49 | void clone(const Matching& other) { 50 | if (&other != this) { 51 | reserve(other.num); 52 | num = other.num; 53 | memcpy(data, other.data, sizeof(MatchItem) * num); 54 | } 55 | } 56 | void reserve(int n) { 57 | clear(); 58 | if (n > 0) { 59 | data = new MatchItem[n]; 60 | _max_num = n; 61 | } 62 | } 63 | void add(int i, int j, double dist) { 64 | assert(num < _max_num); 65 | data[num].idx1 = i; 66 | data[num].idx2 = j; 67 | data[num].dist = dist; 68 | num++; 69 | } 70 | MatchItem& operator[](int i) { 71 | return data[i]; 72 | } 73 | MatchItem operator[](int i) const { 74 | return data[i]; 75 | } 76 | }; 77 | #endif /* SL_MATCHING */ 78 | -------------------------------------------------------------------------------- /src/matching/SL_StereoMatcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * StereoMatcher.h 3 | * 4 | * Created on: 2011-1-7 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_STEREOMATCHER_H_ 9 | #define SL_STEREOMATCHER_H_ 10 | #include "math/SL_Matrix.h" 11 | #include "SL_Matching.h" 12 | 13 | class StereoMatcherParam { 14 | public: 15 | double epiDistThres; 16 | double descDistThres; 17 | double dispDistThres; 18 | 19 | StereoMatcherParam() { 20 | epiDistThres = 2; 21 | descDistThres = 0.4; 22 | dispDistThres = 50; 23 | } 24 | }; 25 | 26 | class StereoMatcher : public StereoMatcherParam { 27 | protected: 28 | //fundamental matrix 29 | const double* F; 30 | Mat_d epiDistMat; 31 | Mat_d descDistMat; 32 | protected: 33 | double _getDisplacementError(const double* pts1 , const double* pts2 , const double avgDx , const double avgDy); 34 | void _computeEpipolarDistMat(const Mat_d& pts1 , const Mat_d& pts2 , Mat_d& distMat); 35 | void _computeEpipolarDisplacementMat( 36 | const Mat_d& pts1 , 37 | const Mat_d& pts2 , 38 | double avgDx , 39 | double avgDy , 40 | double distThes , 41 | Mat_d& distMat); 42 | void _computeDescDistMat(const Mat_d& epiMat , const Mat_d& ds1 , const Mat_d& ds2 , Mat_d& distMat); 43 | int _findBestMatchRow(Mat_d& distMat , int iRow); 44 | int _findBestMatchCol(Mat_d& distMat , int iCol); 45 | public: 46 | StereoMatcher() : 47 | F(0) { 48 | } 49 | StereoMatcher(const double* FMat); 50 | virtual ~StereoMatcher(); 51 | 52 | public: 53 | //use only epipolar constraint 54 | void match(const Mat_d& pts1 , const Mat_d& pts2 , Matching& matches); 55 | //use epipolar constraint + descriptors 56 | void match(const Mat_d& pts1 , const Mat_d& pts2 , const Mat_d& desc1 , const Mat_d& desc2 , Matching& matches); 57 | 58 | //use epipolar constraint + descriptors + average displacement 59 | void match( 60 | const Mat_d& pts1 , 61 | const Mat_d& pts2 , 62 | const Mat_d& desc1 , 63 | const Mat_d& desc2 , 64 | Matching& matches , 65 | double avgDx , 66 | double avgDy , 67 | double dispThres); 68 | 69 | //remove the ourliers that have far 70 | void removeOutlier(const Mat_d& pts1 , const Mat_d& pts2 , Matching& matches); 71 | void removeOutlier( 72 | const Mat_d& pts1 , 73 | const Mat_d& pts2 , 74 | Matching& matches , 75 | double avgDx , 76 | double avgDy , 77 | double distThres); 78 | }; 79 | 80 | #endif /* SL_STEREOMATCHER_H_ */ 81 | -------------------------------------------------------------------------------- /src/matching/SL_StereoMatcherHelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_StereoMatcher.h 3 | * 4 | * Created on: 2011-1-13 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_STEREOMACHERHELPER_H_ 9 | #define SL_STEREOMACHERHELPER_H_ 10 | #include "math/SL_Matrix.h" 11 | #include "math/SL_LinAlg.h" 12 | #include "geometry/SL_FundamentalMatrix.h" 13 | 14 | #include "matching/SL_Matching.h" 15 | #include "imgproc/SL_Image.h" 16 | 17 | int getMatchedPts(const Matching& matches, 18 | const Mat_d& pts1, 19 | const Mat_d& pts2, 20 | Mat_d& pts1Matched, 21 | Mat_d& pts2Matched); 22 | 23 | /*compute distance matrix for a set of 2D points*/ 24 | void getDistMat(const Mat_d& pts, Mat_d& distMat); 25 | 26 | /*compute epipolar error matrix for image points in two cameras*/ 27 | void getEpiMat(const double* F, const Mat_d& pts1, const Mat_d& pts2, double epiDistThres, Mat_d& epiMat); 28 | 29 | void getEpiASDMat(const double* F, 30 | const Mat_d& pts1, 31 | const Mat_d& pts2, 32 | const Mat_d& blks1, 33 | const Mat_d& blks2, 34 | double epiMax, 35 | double asdMax, 36 | Mat_d& epiMat, 37 | Mat_d& asdMat, 38 | double wNone = -1); 39 | 40 | void getImageBlock(const ImgG& img, double x, double y, Mat_d& blk, int hw = 4); 41 | void getImageBlock(const ImgG& img, double x, double y, double* blkData, int hw); 42 | void getImageBlocks(const ImgG& img, const Mat_d& pts, Mat_d& blks, int hW = 4, double scale = 0.3); 43 | 44 | double computeASD(int nDim, const double* blk1, const double* blk2); 45 | double computeDescDist(int nDim, const double* ds1, const double* ds2); 46 | 47 | /*search the nearest point of (x0,y0)*/ 48 | int searchNearestPoint(const Mat_d& pts, double x0, double y0); 49 | int searchNearestPoint(const Mat_d& pts, double x0, double y0, double maxDist, double* dist); 50 | //using Mahalanobis distance 51 | int searchNearestPoint(const Mat_d& pts, double x0, double y0, double var[4], double maxDist, double* dist); 52 | 53 | int findBestMatch(const Mat_d& desc1, 54 | const Mat_d& desc2, 55 | int idx1, 56 | int idx2, 57 | const Mat_d& desc, 58 | const Mat_d& pts, 59 | double x0, 60 | double y0, 61 | double maxDistThes, 62 | double maxDescThres); 63 | 64 | void getFlaged2DPoints(const Mat_d& pts, const Mat_uc& flag, Mat_d& ptsFlaged); 65 | #endif /* SL_STEREOMACHERHELPER_H_ */ 66 | -------------------------------------------------------------------------------- /src/matching/SL_SurfMatching.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_SurfMatching.h 3 | * 4 | * Created on: 2011-2-3 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_SURFMATCHING_H_ 9 | #define SL_SURFMATCHING_H_ 10 | #include "SL_Matching.h" 11 | 12 | #include "math/SL_Matrix.h" 13 | #include "imgproc/SL_Image.h" 14 | #include "tools/cvHelper.h" 15 | 16 | float computeSurfDescDist(const float* desc0, const float* desc1, int dimDesc); 17 | void matchSurf(const Mat_f& kdesc0, const Mat_f& kdesc1, Matching& matches, float ratio); 18 | 19 | void matchSurf(int dimDesc, std::vector& desc1, std::vector& desc2, Matching& matches, float ratio, float maxDist); 20 | /** 21 | * refine the matching results by remove the correspondences with different directions from the majority 22 | */ 23 | int refineMatchedPoints(const Mat_d& pts1, const Mat_d& pts2, Matching& matches, Matching& newMatches, double ratio); 24 | /** 25 | * detect SURF feature points on the image 26 | * @return the dimension of the descriptor 27 | */ 28 | int detectSURFPoints(const ImgG& img, Mat_d& surfPts, std::vector& surfDesc, double hessianThreshold = 2000); 29 | #endif /* SL_SURFMATCHING_H_ */ 30 | -------------------------------------------------------------------------------- /src/math/SL_Array3D.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Array3D.cpp 3 | * 4 | * Created on: Jul 14, 2012 5 | * Author: Danping Zou 6 | */ 7 | #include "math/SL_Array3D.h" 8 | #include 9 | #include 10 | 11 | void arr3Rand(int c, int m, int n, Arr3d& arr) { 12 | arr.resize(3, m, n); 13 | int i, len = 3 * m * n; 14 | for (i = 0; i < len; ++i) { 15 | double rnd_val = double(rand()) / RAND_MAX - 0.5; 16 | arr.data[i] = rnd_val; 17 | } 18 | } 19 | 20 | -------------------------------------------------------------------------------- /src/math/SL_Array3D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Arrary3D.h 3 | * 4 | * Created on: Jul 14, 2012 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_ARRARY3D_H_ 9 | #define SL_ARRARY3D_H_ 10 | 11 | #include 12 | 13 | template 14 | class MyArr3 { 15 | public: 16 | T* data; 17 | union { 18 | int nch; 19 | int c; 20 | }; 21 | union { 22 | int rows; 23 | int m; 24 | }; 25 | union { 26 | int cols; 27 | int n; 28 | }; 29 | public: 30 | typedef T value_type; 31 | MyArr3(void) : 32 | data(0), c(0), m(0), n(0) { 33 | 34 | } 35 | MyArr3(int _c, int _m, int _n) { 36 | if (_c <= 0 || _m <= 0 || _n <= 0) { 37 | data = 0; 38 | c = 0; 39 | m = 0; 40 | n = 0; 41 | } else { 42 | c = _c; 43 | m = _m; 44 | n = _n; 45 | data = new T[c * m * n]; 46 | } 47 | } 48 | 49 | MyArr3(int _c, int _m, int _n, const T* externData) { 50 | if (_c <= 0 || _m <= 0 || _n <= 0) { 51 | c = 0; 52 | data = 0; 53 | m = 0; 54 | n = 0; 55 | } else { 56 | c = _c; 57 | m = _m; 58 | n = _n; 59 | data = new T[c * m * n]; 60 | memcpy(data, externData, sizeof(T) * c * m * n); 61 | } 62 | } 63 | 64 | ~MyArr3() { 65 | clear(); 66 | } 67 | 68 | void clear() { 69 | if (data) 70 | delete[] data; 71 | data = 0; 72 | c = m = n = 0; 73 | } 74 | 75 | void resize(int _c, int _m, int _n) { 76 | if (_c == c && _m == m && _n == n) 77 | return; 78 | clear(); 79 | if (_c > 0 && _m > 0 && _n > 0) { 80 | c = _c; 81 | m = _m; 82 | n = _n; 83 | data = new T[c * m * n]; 84 | } 85 | } 86 | 87 | void fill(T val) { 88 | std::fill_n(data, c * m * n, val); 89 | } 90 | 91 | void cloneFrom(const MyArr3& other) { 92 | if (&other == this) 93 | return; 94 | 95 | resize(other.c, other.m, other.n); 96 | if (c > 0 && m > 0 && n > 0) { 97 | int N = c * m * n; 98 | std::copy(other.data, other.data + N, data); 99 | } 100 | } 101 | public: 102 | bool empty() const { 103 | return c == 0 || m == 0 || n == 0; 104 | } 105 | 106 | public: 107 | typedef T* PTYPE; 108 | typedef const T* CONST_PTYPE; 109 | operator PTYPE() { 110 | return data; 111 | } 112 | operator CONST_PTYPE() const { 113 | return data; 114 | } 115 | T* getPtr() const { 116 | return data; 117 | } 118 | inline T& operator()(int c, int i, int j) { 119 | return data[c * m * n + i * n + j]; 120 | } 121 | inline T& operator()(int c, int i, int j) const { 122 | return data[c * m * n + i * n + j]; 123 | } 124 | inline T* getPtrChannel(int c) const { 125 | return data + c * m * n; 126 | } 127 | inline T* getPtrRow(int c, int i) const { 128 | return data + c * m * n + i * n; 129 | } 130 | }; 131 | 132 | /*useful types*/ 133 | typedef MyArr3 Arr3d; 134 | typedef MyArr3 Arr3f; 135 | typedef MyArr3 Arr3i; 136 | typedef MyArr3 Arr3ui; 137 | typedef MyArr3 Arr3c; 138 | typedef MyArr3 Arr3uc; 139 | 140 | void arr3Rand(int c, int m, int n, Arr3d& arr); 141 | #endif /* SL_ARRARY3D_H_ */ 142 | -------------------------------------------------------------------------------- /src/math/SL_LinAlgWarper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_LinAlgWarper.h 3 | * 4 | * Created on: 2010-11-15 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_LINALGWARPER_H_ 10 | #define SL_LINALGWARPER_H_ 11 | #include "math/SL_LinAlg.h" 12 | #include "math/SL_Matrix.h" 13 | inline void matScale(Mat_d& A, double scale) { 14 | matScale(A.rows, A.cols, A.data, scale, A.data); 15 | } 16 | inline void matAddScale(Mat_d& A, double val) { 17 | matAddScale(A.rows, A.cols, A.data, val); 18 | } 19 | inline void matTrans(const Mat_d& A, Mat_d& AT) { 20 | AT.resize(A.n, A.m); 21 | matTrans(A.m, A.n, A.data, AT.data); 22 | } 23 | inline void matAB(const Mat_d& A, const Mat_d& B, Mat_d& C) { 24 | C.resize(A.rows, B.cols); 25 | matAB(A.rows, A.cols, B.rows, B.cols, A.data, B.data, C.data); 26 | } 27 | inline void matATB(const Mat_d& A, const Mat_d& B, Mat_d& C) { 28 | C.resize(A.cols, B.cols); 29 | matATB(A.rows, A.cols, B.rows, B.cols, A.data, B.data, C.data); 30 | } 31 | inline void matAx(const Mat_d& A, const Mat_d& x, Mat_d& y) { 32 | assert(x.m == A.n && x.n == 1); 33 | y.resize(A.m, 1); 34 | matAxpy(A.m, A.n, 1.0, A.data, x.data, 0, 0, y.data); 35 | } 36 | inline void matAx(int m, int n, const double* A, const double* x, double* y) { 37 | matAxpy(m, n, 1.0, A, x, 0, 0, y); 38 | } 39 | inline void matQR(const Mat_d& A, Mat_d& Q, Mat_d& R) { 40 | assert(A.m > 0 && A.n > 0); 41 | Q.resize(A.m, A.m); 42 | Q.fill(0); 43 | R.resize(A.m, A.n); 44 | R.fill(0); 45 | dgeqrFor(A.m, A.n, A.data, Q.data, R.data); 46 | } 47 | #endif /* SL_LINALGWARPER_H_ */ 48 | -------------------------------------------------------------------------------- /src/math/SL_Math.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Math.cpp 3 | * 4 | * Created on: Dec 22, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_Math.h" 9 | -------------------------------------------------------------------------------- /src/math/SL_Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Math.h 3 | * 4 | * Created on: Dec 22, 2011 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_MATH_H_ 9 | #define SL_MATH_H_ 10 | #include "SL_Matrix.h" 11 | #include "SL_LinAlgWarper.h" 12 | #include "SL_SparseMat.h" 13 | #include "SL_SparseLinearSystem.h" 14 | #include "SL_ProbFuncs.h" 15 | //vectorization operations 16 | template 17 | void div(T iter1, T iter2, U val) { 18 | for (T iter = iter1; iter != iter2; ++iter) 19 | *iter /= val; 20 | } 21 | template 22 | void mult(T iter1, T iter2, U val) { 23 | for (T iter = iter1; iter != iter2; ++iter) 24 | *iter *= val; 25 | } 26 | template 27 | void add(T iter1, T iter2, U val) { 28 | for (T iter = iter1; iter != iter2; ++iter) 29 | *iter += val; 30 | } 31 | template 32 | void sub(T iter1, T iter2, U val) { 33 | for (T iter = iter1; iter != iter2; ++iter) 34 | *iter -= val; 35 | } 36 | 37 | #endif /* SL_MATH_H_ */ 38 | -------------------------------------------------------------------------------- /src/math/SL_Matrix.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Matrix.cpp 3 | * 4 | * Created on: 2010-12-26 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "math/SL_Matrix.h" 9 | #include 10 | #include 11 | 12 | void matRand(int m, int n, Mat_d& mat) { 13 | mat.resize(m, n); 14 | int i, len = m * n; 15 | for (i = 0; i < len; ++i) { 16 | double rnd_val = double(rand()) / RAND_MAX - 0.5; 17 | mat.data[i] = rnd_val; 18 | } 19 | } 20 | void matRand(int m, int n, Mat_f& mat) { 21 | mat.resize(m, n); 22 | int i, len = m * n; 23 | for (i = 0; i < len; ++i) { 24 | float rnd_val = float(rand()) / RAND_MAX - 0.5; 25 | mat.data[i] = rnd_val; 26 | } 27 | } 28 | void matOnes(int m, int n, Mat_d& mat) { 29 | mat.resize(m, n); 30 | mat.fill(1); 31 | } 32 | void matZeros(int m, int n, Mat_d& mat) { 33 | mat.resize(m, n); 34 | mat.fill(0); 35 | } 36 | void matZeros(int m, int n, Mat_f& mat) { 37 | mat.resize(m, n); 38 | mat.fill(0); 39 | } 40 | 41 | void matZeros(int m, int n, double* mat) { 42 | memset(mat, 0, sizeof(double) * m * n); 43 | } 44 | 45 | void matEyes(int n, Mat_d& mat) { 46 | mat.resize(n, n); 47 | mat.fill(0); 48 | int i; 49 | for (i = 0; i < n; ++i) { 50 | mat.data[i * n + i] = 1; 51 | } 52 | } 53 | void matEyes(int n, double* mat) { 54 | memset(mat, 0, sizeof(double) * n * n); 55 | int i; 56 | for (i = 0; i < n; ++i) { 57 | mat[i * n + i] = 1; 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /src/math/SL_ProbFuncs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_ProbFuncs.cpp 3 | * 4 | * Created on: 2011-10-20 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_ProbFuncs.h" 9 | #include "SL_LinAlg.h" 10 | #include "geometry/SL_Geometry.h" 11 | 12 | static const double PI = 3.1415926535897932384626; 13 | double normpdf2(const double x[], const double miu[], const double sigma[]) { 14 | double dx[2] = { x[0] - miu[0], x[1] - miu[1] }; 15 | return normpdf2(dx, sigma); 16 | } 17 | 18 | double normpdf2(const double x[], const double sigma[]) { 19 | double inv2PI = 1.0 / (2 * PI); 20 | double det = mat22Det(sigma); 21 | det = 1.0 / sqrt(det); 22 | double invsigma[4]; 23 | mat22Inv(sigma, invsigma); 24 | double dist = covInnerProd(2, invsigma, x, x); 25 | return inv2PI * det * exp(-0.5 * dist); 26 | } 27 | 28 | double normpdf3(const double x[], const double sigma[]) { 29 | double inv2PI = 1.0 / (2 * PI); 30 | double inv2PIsqrt = sqrt(inv2PI); 31 | double a = inv2PIsqrt * inv2PIsqrt * inv2PIsqrt; 32 | double det = mat33Det(sigma); 33 | det = 1.0 / sqrt(det); 34 | double invsigma[9]; 35 | mat33Inv(sigma, invsigma); 36 | double dist = covInnerProd(3, invsigma, x, x); 37 | return a * det * exp(-0.5 * dist); 38 | } 39 | double normpdf_const(int dim, const double invsigma[]) { 40 | double inv2Pi = 1.0 / sqrt(2 * PI); 41 | double s = sqrt(matDet(dim, invsigma)); 42 | for (int i = 0; i < dim; i++) 43 | s *= inv2Pi; 44 | return s; 45 | } 46 | double normpdf_expval(int dim, const double x[], const double invsigma[]) { 47 | double dist = covInnerProd(dim, invsigma, x, x); 48 | return exp(-0.5 * dist); 49 | } 50 | double normpdf(int dim, const double x[], const double sigma[]) { 51 | double* invSigma = new double[dim * dim]; 52 | matInv(dim,sigma,invSigma); 53 | 54 | double con_val = normpdf_const(dim,invSigma); 55 | double exp_val = normpdf_expval(dim,x,invSigma); 56 | return con_val*exp_val; 57 | } -------------------------------------------------------------------------------- /src/math/SL_ProbFuncs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_ProbFuncs.h 3 | * 4 | * Created on: 2011-10-20 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef PROBFUNCS_H_ 9 | #define PROBFUNCS_H_ 10 | /* 11 | * compute normal distribution 12 | */ 13 | double normpdf2(const double x[], const double miu[], const double sigma[]); 14 | double normpdf2(const double x[], const double sigma[]); 15 | double normpdf3(const double x[], const double miu[], const double sigma[]); 16 | double normpdf3(const double x[], const double sigma[]); 17 | 18 | /* compute (2*pi)^{-dim/2} det(\sigma)^{1/2}*/ 19 | double normpdf_const(int dim, const double invsigma[]); 20 | /* compute exp(-0.5 x'*invsigma*x)*/ 21 | double normpdf_expval(int dim, const double x[], const double invsigma[]); 22 | 23 | double normpdf(int dim, const double x[], const double sigma[]); 24 | #endif /* PROBFUNCS_H_ */ 25 | -------------------------------------------------------------------------------- /src/math/SL_SparseLinearSystem.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_SparseLinearSystem.h 3 | * 4 | * Created on: 2011-6-15 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_SPARSELINEARSYSTEM_H_ 9 | #define SL_SPARSELINEARSYSTEM_H_ 10 | #include "SL_SparseMat.h" 11 | 12 | /** 13 | * solve Ax = b; A: mxn matrix 14 | */ 15 | bool sparseSolveLin(SparseMat& A, double b[], double x[]); 16 | 17 | /** 18 | * solve Ax = b using direct method 19 | */ 20 | bool sparseSolveLin(Triplets& A, double b[], double x[]); 21 | 22 | /** 23 | * solve [A B][x y]' = b 24 | */ 25 | bool sparseSolveLin(Triplets& A, Triplets& B, double b[], double x[], double y[]); 26 | bool sparseSolveLin(SparseMat& A, SparseMat& B, double b[], double x[], double y[]); 27 | 28 | /** 29 | * B = A*B 30 | */ 31 | void sparseMatMul(const SparseMat& A, const SparseMat& B, SparseMat& C); 32 | 33 | 34 | /** 35 | *solve Ax = b using conjugate gradient 36 | */ 37 | //bool sparseSolveLinCG(Triplets& A, double b[], double x[]); 38 | 39 | #endif /* SL_SPARSELINEARSYSTEM_H_ */ 40 | -------------------------------------------------------------------------------- /src/math/lsqr.h: -------------------------------------------------------------------------------- 1 | /* lsqr.h 2 | $Revision: 229 $ $Date: 2006-04-15 18:40:08 -0700 (Sat, 15 Apr 2006) $ 3 | */ 4 | /*! 5 | \file 6 | Header file for ISO C version of LSQR. 7 | */ 8 | 9 | void 10 | lsqr( int m, 11 | int n, 12 | void (*aprod)(int mode, int m, int n, double x[], double y[], 13 | void *UsrWrk ), 14 | double damp, 15 | void *UsrWrk, 16 | double u[], // len = m 17 | double v[], // len = n 18 | double w[], // len = n 19 | double x[], // len = n 20 | double se[], // len = * 21 | double atol, 22 | double btol, 23 | double conlim, 24 | int itnlim, 25 | FILE *nout, 26 | // The remaining variables are output only. 27 | int *istop_out, 28 | int *itn_out, 29 | double *anorm_out, 30 | double *acond_out, 31 | double *rnorm_out, 32 | double *arnorm_out, 33 | double *xnorm_out 34 | ); 35 | -------------------------------------------------------------------------------- /src/tools/GUI_ImageViewer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GUI_ImageViewer.cpp 3 | * 4 | * Created on: 2010-11-8 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #include "GUI_ImageViewer.h" 10 | #include "imgproc/SL_ImageOp.h" 11 | #include "imgproc/SL_ImageIO.h" 12 | #include "tools/SL_TypeConversion.h" 13 | 14 | #include 15 | #include 16 | using namespace std; 17 | //#ifdef USE_OPENCV 18 | void imshow(const char* name, const ImgRGB_d& img, double scale) { 19 | assert(!img.empty()); 20 | ImgRGB rgb; 21 | convert2ColorImage(img, rgb, 0.0, 255.0); 22 | imshow(name, rgb, scale); 23 | } 24 | void imshow(const char* name, const ImgRGB& img, double scale) { 25 | assert(!img.empty()); 26 | cv::Mat mat(img.rows, img.cols, CV_8UC3, img.data); 27 | cv::Mat mat_rgb(img.rows, img.cols, CV_8UC3); 28 | cv::cvtColor(mat, mat_rgb, CV_BGR2RGB); 29 | if (scale == 1.0) { 30 | cv::imshow(name, mat_rgb); 31 | } else { 32 | cv::Mat tmp_rgb(img.rows, img.cols, CV_8UC3); 33 | cv::resize(mat_rgb, tmp_rgb, 34 | cv::Size(img.rows * scale, img.cols * scale)); 35 | cv::imshow(name, tmp_rgb); 36 | } 37 | } 38 | void imshow(const char* name, const ImgG& img, double scale) { 39 | assert(!img.empty()); 40 | cv::Mat mat(img.rows, img.cols, CV_8UC1, img.data); 41 | if (scale == 1.0) 42 | cv::imshow(name, mat); 43 | else { 44 | cv::Mat tmp(img.rows, img.cols, CV_8UC1); 45 | cv::resize(mat, tmp, cv::Size(img.rows * scale, img.cols * scale)); 46 | cv::imshow(name, tmp); 47 | } 48 | //cv::waitKey(-1); 49 | } 50 | void imshow(const char* name, int w, int h, const uchar* data, bool gray) { 51 | if (gray) { 52 | cv::Mat mat(h, w, CV_8UC1, const_cast(data)); 53 | cv::imshow(name, mat); 54 | //cv::waitKey(-1); 55 | } else { 56 | cv::Mat mat(h, w, CV_8UC3, const_cast(data)); 57 | cv::Mat mat_rgb(h, w, CV_8UC3); 58 | cv::cvtColor(mat, mat_rgb, CV_BGR2RGB); 59 | 60 | cv::imshow(name, mat_rgb); 61 | //cv::waitKey(-1); 62 | } 63 | } 64 | 65 | void imshow(const char* name, const Arr3d& arr3) { 66 | Mat_d r, g, b; 67 | splitChannel(arr3, r, g, b); 68 | char wndName[256]; 69 | sprintf(wndName, "%s_ch_01", name); 70 | imshow(wndName, r); 71 | sprintf(wndName, "%s_ch_02", name); 72 | imshow(wndName, g); 73 | sprintf(wndName, "%s_ch_03", name); 74 | imshow(wndName, b); 75 | } 76 | 77 | void imshowext(const char* name, const ImgRGB& img) { 78 | char* file_path = tmpnam(0); 79 | savePPM(img, file_path); 80 | char str[1024]; 81 | sprintf(str, "eog -n %s\n", file_path); 82 | system(str); 83 | } 84 | void imshowext(const char* name, const ImgG& img) { 85 | char* file_path = tmpnam(0); 86 | savePGM(img, file_path); 87 | char str[1024]; 88 | sprintf(str, "eog -n %s\n", file_path); 89 | system(str); 90 | } 91 | void imshowext(const char* name, const uchar* data, int w, int h, bool gray) { 92 | char* file_path = tmpnam(0); 93 | if (gray) 94 | savePGM(data, w, h, file_path); 95 | else 96 | savePPM(data, w, h, file_path); 97 | char str[1024]; 98 | sprintf(str, "eog -n %s\n", file_path); 99 | system(str); 100 | } 101 | -------------------------------------------------------------------------------- /src/tools/GUI_ImageViewer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * GUI_ImageViewer.h 3 | * 4 | * Created on: 2010-11-8 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef GUI_IMAGEVIEWER_H_ 10 | #define GUI_IMAGEVIEWER_H_ 11 | 12 | #include "imgproc/SL_Image.h" 13 | #include "imgproc/SL_ImageOp.h" 14 | #include "math/SL_Matrix.h" 15 | #include "tools/SL_TypeConversion.h" 16 | #include "opencv2/opencv.hpp" 17 | 18 | #include 19 | 20 | void imshow(const char* name, const ImgRGB_d& img, double scale = 1.0); 21 | void imshow(const char* name, const ImgRGB& img, double scale = 1.0); 22 | void imshow(const char* name, const ImgG& img, double scale = 1.0); 23 | void imshow(const char* name, int w, int h, const uchar* data, 24 | bool gray = true); 25 | template 26 | void imshow(const char* name, int m, int n, const T* data, T minval, 27 | double maxval) { 28 | ImgG gray(n, m); 29 | mat2Img(n, m, data, gray, minval, maxval); 30 | imshow(name, gray); 31 | } 32 | template 33 | void imshow(const char* name, int m, int n, const T* data) { 34 | int len = m * n; 35 | T minval = *std::min_element(data, data + len); 36 | T maxval = *std::max_element(data, data + len); 37 | cout << name << " [" << minval <<" - " << maxval << "]" << endl; 38 | imshow(name, m, n, data, minval, maxval); 39 | } 40 | template 41 | void imshow(const char* name, const MyMat& mat, T minval, T maxval) { 42 | ImgG gray(mat.n, mat.m); 43 | mat2Img(mat.n, mat.m, mat.data, gray, minval, maxval); 44 | imshow(name, gray); 45 | } 46 | template 47 | void imshow(const char* name, const MyMat& mat) { 48 | //test 49 | cout << name << endl; 50 | ImgG gray(mat.n, mat.m); 51 | mat2ImgAuto(mat.n, mat.m, mat.data, gray); 52 | imshow(name, gray); 53 | } 54 | void imshow(const char* name, const Arr3d& arr3); 55 | void imshowext(const char* name, const ImgRGB& img); 56 | void imshowext(const char* name, const ImgG& img); 57 | void imshowext(const char* name, const uchar* data, int w, int h, bool gray); 58 | #endif /* GUI_IMAGEVIEWER_H_ */ 59 | -------------------------------------------------------------------------------- /src/tools/SL_AVIReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_AviReader.h 3 | * 4 | * Created on: 2010-11-7 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_AVIREADER_H_ 10 | #define SL_AVIREADER_H_ 11 | #include 12 | #include "SL_VideoReader.h" 13 | 14 | class AVIReader:public VideoReader { 15 | protected: 16 | uint32_t _timestamp; 17 | #if(CV_MAJOR_VERSION <= 2) 18 | CvCapture* videoCap; 19 | #else 20 | cv::VideoCapture videoCap; 21 | #endif 22 | public: 23 | std::string filePath; 24 | public: 25 | AVIReader() : 26 | videoCap(0){ 27 | avi = true; 28 | } 29 | virtual void open(); 30 | virtual void grabFrame(); 31 | virtual void skip(int nSkippedFrame); 32 | virtual int getTotalFrame(); 33 | virtual void readCurFrameRGB(unsigned char* imgdata); 34 | virtual void readCurFrameGray(unsigned char* grayImgData); 35 | virtual void readCurFrame(unsigned char* rgbdata, unsigned char* graydata); 36 | virtual uint32_t getTimeStamp(); 37 | 38 | void grabReadFrame(int Frame, unsigned char* imgdata); 39 | void releaseCamera(); 40 | }; 41 | #endif /* SL_AVIREADER_H_ */ 42 | -------------------------------------------------------------------------------- /src/tools/SL_Debug.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Debug.cpp 3 | * 4 | * Created on: 2010-11-6 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #include "SL_Debug.h" 10 | #include "SL_error.h" 11 | 12 | void draw_reprojection_points( 13 | const double* K , 14 | const double* kc , 15 | int v_id , 16 | int nv , 17 | const double* Rs , 18 | const double* ts , 19 | int npts , 20 | const double* pts , 21 | const double* meas , 22 | const char* vmask) { 23 | const double* R = Rs + v_id * 9; 24 | const double* t = ts + v_id * 3; 25 | 26 | ImgRGB img(800, 450); 27 | img.fill(0, 0, 0); 28 | Mat_d rp_n(npts, 2); 29 | Mat_d mms_n(npts, 2); 30 | 31 | Mat_d rp(npts, 2); 32 | Mat_d mms(npts, 2); 33 | 34 | Mat_i indices(npts, nv); 35 | indices.fill(-1); 36 | 37 | int k = 0; 38 | for (int i = 0; i < npts * nv; i++) { 39 | if (vmask[i] == 1) 40 | indices.data[i] = k++; 41 | } 42 | 43 | k = 0; 44 | double m[2]; 45 | 46 | Mat_i A(npts, 1); 47 | for (int i = 0; i < npts; i++) { 48 | if (vmask[i * nv + v_id] == 0) 49 | continue; 50 | 51 | project(K, kc, R, t, pts + 3 * i, m); 52 | rp_n.data[2 * k] = m[0]; 53 | rp_n.data[2 * k + 1] = m[1]; 54 | 55 | int ind = indices[i * nv + v_id]; 56 | const double* pmeas = meas + 2 * ind; 57 | 58 | mms_n.data[2 * k] = pmeas[0]; 59 | mms_n.data[2 * k + 1] = pmeas[1]; 60 | 61 | A.data[k] = i; 62 | k++; 63 | } 64 | 65 | rp_n.rows = k; 66 | mms_n.rows = k; 67 | // ms.rows = k; 68 | mms.rows = k; 69 | 70 | // imagePoints(K, kc, npts, ms_n, ms); 71 | imagePoints(K, kc, npts, mms_n, mms); 72 | drawCorners(img, rp_n, 0, 0, 255, 's'); 73 | drawCorners(img, mms, 255, 0, 0, 'c'); 74 | 75 | for (int i = 0; i < k; i++) { 76 | //draw_cross(img, rp_n.data[idx1 * 2], rp_n.data[idx1 * 2 + 1], 255, 255, 0); 77 | drawLine(img, rp_n.data[i * 2], rp_n.data[i * 2 + 1], mms.data[i * 2], mms.data[i * 2 + 1], 255, 255, 0); 78 | double dist = dist2(rp_n.data + 2 * i, mms.data + 2 * i); 79 | //test 80 | int id = A.data[i]; 81 | logInfo("[id:%d](%lf,%lf,%lf) -> (%lf,%lf) - %lf - measure:(%lf,%lf)\n", id, pts[3 * id], pts[3 * id + 1], pts[3 82 | * id + 2], rp_n[2 * i], rp_n[2 * i + 1], dist, mms[2 * i], mms[2 * i + 1]); 83 | } 84 | 85 | char buf[256]; 86 | sprintf(buf, "%d", v_id); 87 | imshow(buf, img); 88 | } 89 | -------------------------------------------------------------------------------- /src/tools/SL_Debug.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Debug.h 3 | * 4 | * Created on: 2010-11-6 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_DEBUG_H_ 10 | #define SL_DEBUG_H_ 11 | #include "math/SL_Matrix.h" 12 | #include "math/SL_LinAlg.h" 13 | 14 | #include "geometry/SL_Geometry.h" 15 | #include "geometry/SL_Point.h" 16 | #include "geometry/SL_Distortion.h" 17 | #include "geometry/SL_Triangulate.h" 18 | 19 | #include "imgproc/SL_Image.h" 20 | 21 | #include "SL_Print.h" 22 | #include "SL_WriteRead.h" 23 | #include "SL_DrawCorners.h" 24 | #include "SL_Tictoc.h" 25 | 26 | #include "GUI_ImageViewer.h" 27 | template 28 | int check_same_mat(T& mat1, T& mat2) { 29 | if (mat1.rows != mat2.rows || mat1.cols != mat2.cols) 30 | return -1; 31 | int len = mat1.rows * mat1.cols; 32 | for (int i = 0; i < len; ++i) { 33 | if (mat1.data[i] != mat2.data[i]) { 34 | int y = i / mat1.cols; 35 | int x = i - y * mat1.cols; 36 | logInfo("(%d,%d)%d != %d\n", x, y, mat1.data[i], mat2.data[i]); 37 | return -i; 38 | } 39 | } 40 | return 1; 41 | } 42 | void draw_reprojection_points(const double* K, 43 | const double* kc, 44 | int v_id, 45 | int nv, 46 | const double* Rs, 47 | const double* ts, 48 | int npts, 49 | const double* pts, 50 | const double* meas, 51 | const char* vmask); 52 | 53 | #endif /* SL_DEBUG_H_ */ 54 | -------------------------------------------------------------------------------- /src/tools/SL_Print.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Helper.h 3 | * 4 | * Created on: 2011-1-6 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_HELPER_H_ 9 | #define SL_HELPER_H_ 10 | #include "math/SL_Matrix.h" 11 | #include "matching/SL_Matching.h" 12 | #include "geometry/SL_Point.h" 13 | #include "SL_TypeConversion.h" 14 | 15 | void printMat(int m, int n, const double* mat); 16 | void printMat(FILE* fp, int m, int n, const double* mat); 17 | void print(const Mat_d& mat); 18 | void print(FILE* fp, const Mat_d& mat); 19 | void print(FILE* fp, const Mat_i& mat); 20 | void print(const Mat_f& mat); 21 | void print(const Mat_i& mat); 22 | void printMat(FILE* fp, int m, int n, const int* mat); 23 | void print(const Mat_ui& mat); 24 | void print(const Mat_c& mat); 25 | void print(const VecPoint2d& pts); 26 | void print(const Matching& matches); 27 | 28 | void printMat(int m, int n, const float* mat); 29 | void printMat(int m, int n, const int * mat); 30 | void printMat(int m, int n, const unsigned int * mat); 31 | void printMat(int m, int n, const char* mat); 32 | void printTransMat(int m, int n, const float* mat); 33 | void printPts(int n, const Point2d* pts); 34 | 35 | template 36 | void releasePtrList(T& lst) { 37 | typename T::iterator iter = lst.begin(); 38 | while (iter != lst.end()) { 39 | delete *iter; 40 | ++iter; 41 | } 42 | lst.clear(); 43 | } 44 | void VecPoint2D2Matrix(const VecPoint2d& pts, Mat_d& mat); 45 | #endif /* SL_HELPER_H_ */ 46 | -------------------------------------------------------------------------------- /src/tools/SL_Tictoc.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Tictoc.cpp 3 | * 4 | * Created on: Feb 28, 2011 5 | * Author: Danping Zou 6 | */ 7 | #include "SL_Tictoc.h" 8 | #ifdef WIN32 9 | int 10 | gettimeofday(struct timeval *tp, void *tzp) 11 | { 12 | time_t clock; 13 | struct tm tm; 14 | SYSTEMTIME wtm; 15 | GetLocalTime(&wtm); 16 | tm.tm_year = wtm.wYear - 1900; 17 | tm.tm_mon = wtm.wMonth - 1; 18 | tm.tm_mday = wtm.wDay; 19 | tm.tm_hour = wtm.wHour; 20 | tm.tm_min = wtm.wMinute; 21 | tm.tm_sec = wtm.wSecond; 22 | tm. tm_isdst = -1; 23 | clock = mktime(&tm); 24 | tp->tv_sec = clock; 25 | tp->tv_usec = wtm.wMilliseconds * 1000; 26 | return (0); 27 | } 28 | #endif 29 | static TimeMeasurer g_tmMeasurer; 30 | void tic() { 31 | g_tmMeasurer.tic(); 32 | } 33 | double toc() { 34 | double tm = g_tmMeasurer.toc(); 35 | printf("passing time :%lf (ms) \n", tm); 36 | return tm; 37 | } 38 | -------------------------------------------------------------------------------- /src/tools/SL_Tictoc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Tictoc.h 3 | * 4 | * Created on: 2010-11-9 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_TICTOC_H_ 10 | #define SL_TICTOC_H_ 11 | 12 | #include 13 | #ifndef WIN32 14 | #include 15 | #else 16 | #include "winsock2.h" 17 | #include 18 | int gettimeofday(struct timeval *tp, void *tzp); 19 | #endif 20 | 21 | #include 22 | //for measurement of timing 23 | class TimeMeasurer { 24 | protected: 25 | struct timeval tm_s, tm_e; 26 | public: 27 | TimeMeasurer(void) { 28 | } 29 | ~TimeMeasurer(void) { 30 | } 31 | public: 32 | void tic() { 33 | gettimeofday(&tm_s, 0); 34 | } 35 | double toc() { 36 | gettimeofday(&tm_e, 0); 37 | double tm = get_pass_time(); 38 | return tm; 39 | } 40 | double get_pass_time() { 41 | double tm = (tm_e.tv_sec - tm_s.tv_sec) * 1000.0 + (tm_e.tv_usec - tm_s.tv_usec) / 1000.0; 42 | return tm; 43 | } 44 | }; 45 | void tic(); 46 | double toc(); 47 | #endif /* SL_TICTOC_H_ */ 48 | -------------------------------------------------------------------------------- /src/tools/SL_Timing.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Timing.cpp 3 | * 4 | * Created on: 2011-5-18 5 | * Author: Danping Zou 6 | */ 7 | 8 | #include "SL_Timing.h" 9 | 10 | Timing::Timing() { 11 | // TODO Auto-generated constructor stub 12 | 13 | } 14 | 15 | Timing::~Timing() { 16 | // TODO Auto-generated destructor stub 17 | } 18 | -------------------------------------------------------------------------------- /src/tools/SL_Timing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Timing.h 3 | * 4 | * Created on: 2011-5-18 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_TIMING_H_ 9 | #define SL_TIMING_H_ 10 | #include "SL_error.h" 11 | #include 12 | #include 13 | 14 | #define TIMING_DATA_LEN 4 15 | 16 | class Timing { 17 | public: 18 | int f; 19 | double t; 20 | double data[TIMING_DATA_LEN]; 21 | public: 22 | Timing(); 23 | Timing(int frame, double tm) : 24 | f(frame), t(tm) { 25 | memset(data, 0, sizeof(double) * TIMING_DATA_LEN); 26 | } 27 | Timing(const Timing& other) : 28 | f(other.f), t(other.t) { 29 | memcpy(data, other.data, sizeof(double) * TIMING_DATA_LEN); 30 | } 31 | Timing& operator =(const Timing& other) { 32 | if (&other != this) { 33 | f = other.f; 34 | t = other.t; 35 | memcpy(data, other.data, sizeof(double) * TIMING_DATA_LEN); 36 | } 37 | return *this; 38 | } 39 | ~Timing(); 40 | }; 41 | 42 | class Timings { 43 | public: 44 | std::vector tms; 45 | public: 46 | void clear() { 47 | tms.clear(); 48 | } 49 | Timing& add(int f, double t) { 50 | tms.push_back(Timing(f, t)); 51 | return tms.back(); 52 | } 53 | void save(const char* fmtstr, ...) { 54 | char buf[1024]; 55 | GET_FMT_STR(fmtstr,buf) 56 | 57 | FILE * fp = fopen(buf, "w"); 58 | if (!fp) 59 | repErr("Timings::save - cannot open the file!\n"); 60 | 61 | for (size_t i = 0; i < tms.size(); i++) { 62 | fprintf(fp, "%d\t%lf\t%lf\t%lf\t%lf\t%lf\n", tms[i].f, tms[i].t, tms[i].data[0], tms[i].data[1], 63 | tms[i].data[2], tms[i].data[3]); 64 | } 65 | fclose(fp); 66 | } 67 | }; 68 | #endif /* SL_TIMING_H_ */ 69 | -------------------------------------------------------------------------------- /src/tools/SL_TypeConversion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_TypeConversion.cpp 3 | * 4 | * Created on: 2010-11-9 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #include "SL_TypeConversion.h" 10 | 11 | void matDouble2Int(const Mat_d& md, Mat_i& mi) { 12 | int rows = md.rows; 13 | int cols = md.cols; 14 | if (mi.rows != rows || mi.cols != cols) 15 | mi.resize(rows, cols); 16 | 17 | int i, len = rows * cols; 18 | for (i = 0; i < len; ++i) { 19 | mi.data[i] = static_cast(md.data[i] + 0.5); 20 | } 21 | } 22 | void matDouble2Float(const Mat_d& md, Mat_f& mf) { 23 | int rows = md.rows; 24 | int cols = md.cols; 25 | if (mf.rows != rows || mf.cols != cols) 26 | mf.resize(rows, cols); 27 | 28 | int i, len = rows * cols; 29 | for (i = 0; i < len; ++i) { 30 | mf.data[i] = static_cast(md.data[i]); 31 | } 32 | } 33 | void matFloat2Double(const Mat_f& m0, Mat_d& m1) { 34 | int rows = m0.rows; 35 | int cols = m0.cols; 36 | if (m1.rows != rows || m1.cols != cols) 37 | m1.resize(rows, cols); 38 | 39 | int i, len = rows * cols; 40 | for (i = 0; i < len; ++i) { 41 | m1.data[i] = static_cast(m0.data[i]); 42 | } 43 | } 44 | void matFloat2Int(const Mat_f& mf, Mat_i& mi) { 45 | int rows = mf.rows; 46 | int cols = mf.cols; 47 | if (mi.rows != rows || mi.cols != cols) 48 | mi.resize(rows, cols); 49 | 50 | int i, len = rows * cols; 51 | for (i = 0; i < len; ++i) { 52 | mi.data[i] = static_cast(mf.data[i] + 0.5); 53 | } 54 | } 55 | void VecPoint2D2Matrix(const VecPoint2d& pts, Mat_d& mat) { 56 | mat.resize((int) pts.size(), 2); 57 | for (size_t i = 0; i < pts.size(); ++i) { 58 | mat.data[2 * i] = pts[i].x; 59 | mat.data[2 * i + 1] = pts[i].y; 60 | } 61 | } -------------------------------------------------------------------------------- /src/tools/SL_TypeConversion.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_TypeConversion.h 3 | * 4 | * Created on: 2010-11-9 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_TYPECONVERSION_H_ 10 | #define SL_TYPECONVERSION_H_ 11 | #include "math/SL_Matrix.h" 12 | #include "imgproc/SL_Image.h" 13 | #include "imgproc/SL_ImageOp.h" 14 | #include "geometry/SL_Point.h" 15 | #include 16 | #include 17 | #include 18 | void matDouble2Int(const Mat_d& md, Mat_i& mi); 19 | void matDouble2Float(const Mat_d& md, Mat_f& mf); 20 | void matFloat2Double(const Mat_f& mf, Mat_d& md); 21 | void matFloat2Int(const Mat_f& mf, Mat_i& mi); 22 | void VecPoint2D2Matrix(const VecPoint2d& pts, Mat_d& mat); 23 | 24 | template 25 | void mat2ImgAuto(int W, int H, T* data, ImgG& gray) { 26 | assert(W > 0 && H > 0); 27 | gray.resize(W, H); 28 | 29 | int i, len = W * H; 30 | double max_v = static_cast(*std::max_element(data, data + len)); 31 | double min_v = static_cast(*std::min_element(data, data + len)); 32 | 33 | double r = max_v - min_v; 34 | if (r > 0) { 35 | for (i = 0; i < len; ++i) 36 | gray.data[i] = static_cast((data[i] - min_v) / r * 255); 37 | } else { 38 | for (i = 0; i < len; ++i) 39 | gray.data[i] = 100; 40 | } 41 | } 42 | template 43 | void mat2Img(int W, int H, const T* data, ImgG& gray, T minval, T maxval) { 44 | assert(maxval > minval); 45 | assert(W > 0 && H > 0); 46 | gray.resize(W, H); 47 | 48 | double r = maxval - minval; 49 | int i, len = W * H; 50 | if (r > 0) { 51 | for (i = 0; i < len; ++i) { 52 | T val = data[i]; 53 | if (val > maxval) 54 | val = maxval; 55 | if (val < minval) 56 | val = minval; 57 | 58 | double new_val = (val - minval) / r * 255.0; 59 | 60 | if (new_val > 255.0) 61 | new_val = 255.0; 62 | if (new_val < 0.0) 63 | new_val = 0.0; 64 | gray.data[i] = static_cast(new_val); 65 | } 66 | } else { 67 | for (int i = 0; i < len; ++i) { 68 | gray.data[i] = 100; 69 | } 70 | } 71 | } 72 | template 73 | void mat2Img(const T& mat, ImgG& gray) { 74 | int W = mat.cols; 75 | int H = mat.rows; 76 | mat2ImgAuto(W, H, mat.data, gray); 77 | } 78 | 79 | template 80 | void mat2Img(const MyMat& mat, ImgG& gray, T minval, T maxval) { 81 | int W = mat.cols; 82 | int H = mat.rows; 83 | mat2Img(W, H, mat.data, gray, minval, maxval); 84 | } 85 | template 86 | void mat2RGBImg(const MyMat& mat, ImgRGB& rgb, T minval, T maxval) { 87 | ImgG G; 88 | mat2Img(mat, G, minval, maxval); 89 | gray2rgb(G, rgb); 90 | } 91 | template 92 | void img2Mat(const ImgG& gray, MyMat& mat, T maxval /*= 255.0*/) { 93 | int len = gray.w * gray.h; 94 | mat.resize(gray.h, gray.w); 95 | for (int i = 0; i < len; ++i) { 96 | mat.data[i] = (T) (gray.data[i]) / maxval; 97 | } 98 | } 99 | #endif /* SL_TYPECONVERSION_H_ */ 100 | -------------------------------------------------------------------------------- /src/tools/SL_USBCamReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_USBCamReader.cpp 3 | * 4 | * Created on: 2012-4-10 5 | * Author: elezoud 6 | */ 7 | 8 | #include "SL_USBCamReader.h" 9 | #include "SL_error.h" 10 | 11 | void USBCamReader::open() { 12 | #if(CV_MAJOR_VERSION <= 2) 13 | if (videoCap) 14 | cvReleaseCapture(&videoCap); 15 | 16 | videoCap = cvCaptureFromCAM(camid); 17 | if (!videoCap) { 18 | repErr("ERROR: Fail to detect the USB camera %d\n", camid); 19 | } 20 | 21 | cvSetCaptureProperty(videoCap, CV_CAP_PROP_FRAME_WIDTH, 640); 22 | cvSetCaptureProperty(videoCap, CV_CAP_PROP_FRAME_HEIGHT, 480); 23 | cvSetCaptureProperty(videoCap, CV_CAP_PROP_FPS, 30); 24 | _w = (int)cvGetCaptureProperty(videoCap, CV_CAP_PROP_FRAME_WIDTH); 25 | _h = (int)cvGetCaptureProperty(videoCap, CV_CAP_PROP_FRAME_HEIGHT); 26 | #else 27 | if( !videoCap.open(camid)){ 28 | repErr("ERROR: Fail to detect the USB camera %d \n", camid); 29 | } 30 | videoCap.set(CV_CAP_PROP_FRAME_WIDTH,640); 31 | videoCap.set(CV_CAP_PROP_FRAME_WIDTH,480); 32 | videoCap.set(CV_CAP_PROP_FPS,30); 33 | 34 | _w = (int) videoCap.get(CV_CAP_PROP_FRAME_WIDTH); 35 | _w = (int) videoCap.get(CV_CAP_PROP_FRAME_HEIGHT); 36 | #endif 37 | 38 | if (_w <=0 || _h <= 0) 39 | repErr("ERROR: Fail to open the USB camera \n"); 40 | 41 | _tm.tic(); 42 | } 43 | USBCamReader::~USBCamReader(){ 44 | #if(CV_MAJOR_VERSION <= 2) 45 | if(videoCap) 46 | cvReleaseCapture(&videoCap); 47 | #endif 48 | } 49 | 50 | void USBCamReader::grabFrame() { 51 | #if(CV_MAJOR_VERSION <= 2) 52 | assert(videoCap); 53 | cvGrabFrame(videoCap); 54 | #else 55 | videoCap.grab(); 56 | #endif 57 | } 58 | void USBCamReader::readCurFrameRGB(unsigned char* imgdata) { 59 | #if(CV_MAJOR_VERSION <= 2) 60 | assert(videoCap); 61 | IplImage* img = cvRetrieveFrame(videoCap); 62 | memcpy(imgdata, img->imageData, _w * _h * 3); 63 | #else 64 | cv::Mat cvImg(_w,_h,CV_8UC3, imgdata); 65 | videoCap.retrieve(cvImg); 66 | #endif 67 | } 68 | void USBCamReader::readCurFrameGray(unsigned char* grayImgData) { 69 | #if(CV_MAJOR_VERSION <= 2) 70 | assert(videoCap); 71 | IplImage* img = cvRetrieveFrame(videoCap); 72 | cv::Mat rawFrame(img); 73 | #else 74 | cv::Mat rawFrame; 75 | videoCap.retrieve(rawFrame); 76 | #endif 77 | cv::Mat videoFrame(_h, _w, CV_8UC1, grayImgData); 78 | cv::cvtColor(rawFrame, videoFrame, CV_RGB2GRAY); 79 | } 80 | void USBCamReader::readCurFrame(unsigned char* rgbdata, 81 | unsigned char* graydata) { 82 | #if(CV_MAJOR_VERSION <= 2) 83 | assert(videoCap); 84 | IplImage* img = cvRetrieveFrame(videoCap); 85 | cv::Mat rawFrame(img); 86 | #else 87 | cv::Mat rawFrame; 88 | videoCap.retrieve(rawFrame); 89 | #endif 90 | 91 | cv::Mat rgbImg(_h, _w, CV_8UC3, rgbdata); 92 | cv::cvtColor(rawFrame, rgbImg, CV_BGR2RGB); 93 | 94 | cv::Mat videoFrame(_h, _w, CV_8UC1, graydata); 95 | cv::cvtColor(rawFrame, videoFrame, CV_RGB2GRAY); 96 | } 97 | uint32_t USBCamReader::getTimeStamp(){ 98 | double ts = _tm.get_pass_time(); 99 | return (uint32_t)(ts+0.5); 100 | } 101 | -------------------------------------------------------------------------------- /src/tools/SL_USBCamReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_USBCamReader.h 3 | * 4 | * Created on: 2012-4-10 5 | * Author: elezoud 6 | */ 7 | 8 | #ifndef SL_USBCAMREADER_H_ 9 | #define SL_USBCAMREADER_H_ 10 | #include "SL_VideoReader.h" 11 | #include "SL_Tictoc.h" 12 | 13 | class USBCamReader:public VideoReader { 14 | protected: 15 | #if(CV_MAJOR_VERSION <= 2) 16 | CvCapture* videoCap; 17 | #else 18 | cv::VideoCapture videoCap; 19 | #endif 20 | TimeMeasurer _tm; 21 | uint32_t _timstamp; 22 | public: 23 | int camid; 24 | public: 25 | USBCamReader() : 26 | videoCap(0), camid(-1){ 27 | avi = false; 28 | } 29 | virtual ~USBCamReader(); 30 | virtual void open(); 31 | virtual void grabFrame(); 32 | virtual void readCurFrameRGB(unsigned char* imgdata); 33 | virtual void readCurFrameGray(unsigned char* grayImgData); 34 | virtual void readCurFrame(unsigned char* rgbdata, unsigned char* graydata); 35 | virtual uint32_t getTimeStamp(); 36 | }; 37 | #endif /* SL_USBCAMREADER_H_ */ 38 | -------------------------------------------------------------------------------- /src/tools/SL_VideoReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_AviReader.cpp 3 | * 4 | * Created on: 2010-11-7 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #include "SL_VideoReader.h" 10 | #include "SL_error.h" 11 | -------------------------------------------------------------------------------- /src/tools/SL_VideoReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_AviReader.h 3 | * 4 | * Created on: 2010-11-7 5 | * Author: Danping Zou 6 | * E-mail: dannis.zou@gmail.com 7 | */ 8 | 9 | #ifndef SL_VideoREADER_H_ 10 | #define SL_VideoREADER_H_ 11 | #include 12 | #include 13 | class VideoReader { 14 | public: 15 | bool avi; 16 | int _w,_h; 17 | public: 18 | VideoReader() :avi(false),_w(0),_h(0) { 19 | } 20 | virtual ~VideoReader(){}; 21 | virtual void open() = 0; 22 | virtual void grabFrame() = 0; 23 | virtual void readCurFrameRGB(unsigned char* imgdata) = 0; 24 | virtual void readCurFrameGray(unsigned char* grayImgData) = 0; 25 | virtual void readCurFrame(unsigned char* rgbdata, unsigned char* graydata) = 0; 26 | virtual void skip(int nSkippedFrame){}; 27 | virtual uint32_t getTimeStamp() = 0; 28 | virtual int getTotalFrame(){return 0;} 29 | }; 30 | #endif /* SL_AVIREADER_H_ */ 31 | -------------------------------------------------------------------------------- /src/tools/cvHelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * cvHelper.h 3 | * 4 | * Created on: 2011-1-6 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef CVHELPER_H_ 9 | #define CVHELPER_H_ 10 | 11 | #include "math/SL_Matrix.h" 12 | #include "imgproc/SL_Image.h" 13 | #include "matching/SL_Matching.h" 14 | 15 | #include "opencv2/features2d/features2d.hpp" 16 | #include "opencv2/imgproc/imgproc.hpp" 17 | 18 | typedef std::vector KpVec; 19 | typedef std::vector DMatchVec; 20 | 21 | void printCVMat(const cv::Mat& cvMat); 22 | 23 | void mat2KpVec(const Mat_d& matPts, KpVec& vecPts); 24 | void KpVec2Mat(const KpVec& vecPts, Mat_d& matPts); 25 | void cvMat2MyMat(const cv::Mat& cvMat, Mat_d& mat); 26 | void myMat2CvMat(const Mat_d& mat, cv::Mat& cvMat); 27 | void cvMatch2MyMatch(const DMatchVec& cvMatch, Matching& myMatch); 28 | void myMatch2CvMatch(const Matching& myMatch, DMatchVec& cvMatch); 29 | 30 | void cvImg2ImgRGB(const cv::Mat& cvImg, ImgRGB& img); 31 | #endif /* CVHELPER_H_ */ 32 | -------------------------------------------------------------------------------- /src/util/SL_Graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Graph.h 3 | * 4 | * Created on: 2011-7-6 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_GRAPH_H_ 9 | #define SL_GRAPH_H_ 10 | #include 11 | #include "math/SL_Matrix.h" 12 | /** 13 | * find connected components defined by a symmetric matrix M = M^T 14 | */ 15 | template 16 | void findConnectedComponents(const T& M, std::vector& conns) { 17 | assert(M.m == M.n); 18 | int num = M.m; 19 | conns.clear(); 20 | int* VQ = new int[num]; 21 | int* CON = new int[num]; 22 | bool* flag = new bool[num]; 23 | for (int i = 0; i < num; i++) { 24 | if (0 == flag[i]) { 25 | //start to grow 26 | int nVQ = 0, nCon = 0; 27 | nVQ = nCon = 0; 28 | CON[nCon++] = i; 29 | VQ[nVQ++] = i; 30 | flag[i] = true; 31 | while (nVQ > 0) { 32 | int j0 = VQ[nVQ - 1]; 33 | nVQ--; 34 | for (int j = 0; j < num; j++) { 35 | if (j != j0 && !flag[j] && M.data[j0 * num + j] > 0) { 36 | CON[nCon++] = j; 37 | VQ[nVQ++] = j; 38 | flag[j] = true; 39 | } 40 | } 41 | } 42 | //output the connected components 43 | Mat_i conn(1, nCon); 44 | for (int k = 0; k < nCon; k++) 45 | conn.data[k] = CON[k]; 46 | conns.push_back(conn); 47 | } 48 | 49 | } 50 | delete[] VQ; 51 | delete[] CON; 52 | delete[] flag; 53 | } 54 | #endif /* SL_GRAPH_H_ */ 55 | -------------------------------------------------------------------------------- /src/util/SL_Utility.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SL_Utility.h 3 | * 4 | * Created on: 2011-6-14 5 | * Author: Danping Zou 6 | */ 7 | 8 | #ifndef SL_UTILITY_H_ 9 | #define SL_UTILITY_H_ 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "math/SL_Matrix.h" 15 | 16 | template 17 | class PairLess { 18 | public: 19 | bool operator ()(const std::pair& pair1 20 | , const std::pair& pair2) const { 21 | return pair1.first < pair2.first; 22 | } 23 | }; 24 | 25 | template 26 | class PairGreater { 27 | public: 28 | bool operator ()(const std::pair& pair1 29 | , const std::pair& pair2) const { 30 | return pair1.first > pair2.first; 31 | } 32 | }; 33 | 34 | template 35 | void sortWithInd(U n, T* x, U ind[], bool less = true) { 36 | assert(n > 0); 37 | //construct a pair array 38 | std::pair* xi = new std::pair[n]; 39 | for (U i = 0; i < n; i++) { 40 | xi[i].first = x[i]; 41 | xi[i].second = i; 42 | } 43 | 44 | if (less) 45 | std::sort(xi, xi + n, PairLess()); 46 | else 47 | std::sort(xi, xi + n, PairGreater()); 48 | 49 | for (U i = 0; i < n; i++) { 50 | ind[i] = xi[i].second; 51 | } 52 | 53 | delete[] xi; 54 | } 55 | template 56 | void partialSortWithInd(U n, T*x, U ind[], U top_num, bool less = true) { 57 | assert(n > 0 && top_num > 0 && n >= top_num); 58 | //construct a pair array 59 | std::pair* xi = new std::pair[n]; 60 | for (U i = 0; i < n; i++) { 61 | xi[i].first = x[i]; 62 | xi[i].second = i; 63 | } 64 | 65 | if (less) 66 | std::partial_sort(xi, xi + top_num, xi + n, PairLess()); 67 | else 68 | std::partial_sort(xi, xi + top_num, xi + n, PairGreater()); 69 | 70 | for (U i = 0; i < n; i++) { 71 | ind[i] = xi[i].second; 72 | } 73 | 74 | delete[] xi; 75 | } 76 | template 77 | void cumSum(const U n, const T* x, T* cx) { 78 | T s = 0; 79 | for (U i = 0; i < n; i++) { 80 | s += x[i]; 81 | cx[i] = s; 82 | } 83 | } 84 | 85 | template 86 | void cumSum(int m, int n, const T* mat, U* sum, bool row) { 87 | if (row) { 88 | for (int i = 0; i < m; ++i) { 89 | sum[i * n] = static_cast(mat[i * n]); 90 | for (int j = 1; j < n; ++j) { 91 | sum[i * n + j] = sum[i * n + j - 1] + mat[i * n + j]; 92 | } 93 | } 94 | } else { 95 | for (int j = 0; j < n; ++j) { 96 | sum[j] = static_cast(mat[j]); 97 | for (int i = 1; i < m; ++i) { 98 | sum[i * n + j] = sum[(i - 1) * n + j] + mat[i * n + j]; 99 | } 100 | } 101 | } 102 | } 103 | 104 | template 105 | void cumSum(const MyMat& mat, MyMat& sum, bool row) { 106 | sum.resize(mat.m, mat.n); 107 | cumSum(mat.m, mat.n, mat.data, sum.data, row); 108 | } 109 | #endif /* SL_UTILITY_H_ */ 110 | -------------------------------------------------------------------------------- /src/util/testSparse.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "math/SL_SparseLinearSystem.h" 3 | #include "tools/SL_WriteRead.h" 4 | #include "tools/SL_Print.h" 5 | //void testSolveNormEqn() { 6 | // int m = 10, n = 10; 7 | // Mat_d mat; 8 | // //matRand(m, n, mat); 9 | // //write(mat,"mat.txt"); 10 | // readMat(mat, "mat.txt"); 11 | // printMat(m, n, mat); 12 | // 13 | // SparseMat cssMat; 14 | // dense2Sparse(mat, cssMat); 15 | // print(cssMat); 16 | // 17 | // //test 18 | // printf("m,n:%d,%d\n", m, n); 19 | // 20 | // double r, x[10]; 21 | // sparseSolveNorm(cssMat, x, &r); 22 | // 23 | // //test 24 | // logInfo("residual:%g\n", r); 25 | // printMat(1, 10, x); 26 | // 27 | //} 28 | void testSolveLinEqn() { 29 | int m = 10, n = 10; 30 | Mat_d mat; 31 | //matRand(m, n, mat); 32 | //write(mat,"mat.txt"); 33 | readMat(mat, "mat.txt"); 34 | printMat(m, n, mat); 35 | 36 | SparseMat cssMat; 37 | dense2Sparse(mat, cssMat); 38 | print(cssMat); 39 | 40 | //test 41 | printf("m,n:%d,%d\n", m, n); 42 | 43 | double b[10]; 44 | for (int i = 0; i < 10; i++) 45 | b[i] = 1.0; 46 | 47 | double x[10]; 48 | sparseSolveLin(cssMat, b, x); 49 | printMat(1, 10, x); 50 | } 51 | void testSolveLinEqn2() { 52 | Mat_d Ad, b; 53 | //matRand(m, n, mat); 54 | //write(mat,"mat.txt"); 55 | readMat(Ad, "/home/tsou/Ad.txt"); 56 | readMat(b, "/home/tsou/b.txt"); 57 | 58 | 59 | print(Ad); 60 | print(b); 61 | SparseMat A; 62 | dense2Sparse(Ad, A); 63 | 64 | print(A); 65 | const int m = A.m; 66 | const int n = A.n; 67 | //test 68 | printf("m,n:%d,%d\n", m, n); 69 | 70 | double* x = new double[n]; 71 | memset(x, 0, sizeof(double) * n); 72 | sparseSolveLin(A, b, x); 73 | printMat(1, n, x); 74 | delete[] x; 75 | } 76 | void testTri2CSS() { 77 | Mat_d mat; 78 | matRand(10, 10, mat); 79 | print(mat); 80 | 81 | longInt len = mat.rows * mat.cols; 82 | longInt nnz = 0; 83 | for (longInt i = 0; i < len; i++) { 84 | mat.data[i] = mat.data[i] < 0.1 ? 0 : mat.data[i]; 85 | if (mat.data[i] > 0) 86 | nnz++; 87 | } 88 | print(mat); 89 | 90 | longInt* ri = new longInt[nnz]; 91 | longInt* ci = new longInt[nnz]; 92 | double* nzval = new double[nnz]; 93 | 94 | int k = 0; 95 | for (int i = 0; i < mat.rows; i++) { 96 | for (int j = 0; j < mat.cols; j++) { 97 | if (mat.data[i * mat.rows + j] > 0) { 98 | ci[k] = j; 99 | ri[k] = i; 100 | nzval[k] = mat.data[i * mat.rows + j]; 101 | k++; 102 | } 103 | } 104 | } 105 | 106 | checkTriplets(10, 10, nnz, ri, ci, nzval); 107 | SparseMat cssMat; 108 | triplets2Sparse(10, 10, nnz, ri, ci, nzval, cssMat); 109 | 110 | //test 111 | print(cssMat); 112 | sparse2Dense(cssMat, mat); 113 | print(mat); 114 | //logInfo("sorted:'%s'\n", isTripletsSorted(10, 10, nnz, ri, ci, nzval) ? "true" : "false"); 115 | 116 | delete[] ri; 117 | delete[] ci; 118 | delete[] nzval; 119 | } 120 | //int main(int argc, char** argv) { 121 | // 122 | // //testTri2CSS(); 123 | // //printMat(10,1,eigvecs); 124 | // 125 | // //testSolveLinEqn(); 126 | // //testSolveLinEqn2(); 127 | // return 0; 128 | //} 129 | --------------------------------------------------------------------------------