├── aslam_cv_common ├── proto │ └── aslam │ │ └── common │ │ └── id.proto ├── src │ ├── hash-id.cc │ ├── internal │ │ └── unique-id.cc │ ├── covariance-helpers.cc │ ├── reader-first-reader-writer-lock.cc │ ├── channel.cc │ ├── reader-writer-lock.cc │ └── sensor.cc ├── include │ └── aslam │ │ └── common │ │ ├── covariance-helpers.h │ │ ├── predicates.h │ │ ├── reader-first-reader-writer-lock.h │ │ ├── eigen-hash.h │ │ ├── entrypoint.h │ │ ├── internal │ │ └── unique-id.h │ │ ├── meta.h │ │ ├── types.h │ │ ├── crtp-clone.h │ │ ├── pose-types.h │ │ ├── yaml-file-serialization.h │ │ ├── macros.h │ │ ├── reader-writer-lock.h │ │ ├── channel-definitions.h │ │ ├── neon-helpers.h │ │ ├── yaml-serialization.h │ │ ├── sensor.h │ │ ├── channel-external-declaration.h │ │ ├── memory.h │ │ ├── statistics │ │ └── accumulator.h │ │ └── time.h ├── package.xml ├── cmake │ ├── detect_simd.cmake │ ├── export_flags.cmake │ └── setup_openmp.cmake ├── test │ ├── test-covariance-helpers.cc │ ├── reader_writer_mutex_fixture.h │ ├── test-hash-id.cc │ ├── reader_writer_lock_test.cc │ ├── test-channels.cc │ ├── reader_writer_mutex_fixture_inl.h │ ├── test-time.cc │ ├── test-thread-pool.cc │ └── test-stl-helpers.cc └── CMakeLists.txt ├── mainpage.dox ├── aslam_cv_pipeline ├── src │ ├── undistorter.cc │ ├── visual-pipeline-null.cc │ ├── visual-pipeline.cc │ └── undistorter-mapped.cc ├── include │ └── aslam │ │ └── pipeline │ │ ├── test │ │ └── convert-maps-legacy.h │ │ ├── visual-pipeline-null.h │ │ ├── undistorter.h │ │ └── undistorter-mapped-inl.h ├── package.xml └── CMakeLists.txt ├── CONTRIBUTORS.md ├── .gitignore ├── README.md ├── aslam_cv_cameras ├── include │ └── aslam │ │ └── cameras │ │ ├── distortion-inl.h │ │ ├── random-camera-generator.h │ │ ├── distortion-fisheye-inl.h │ │ ├── camera-inl.h │ │ ├── camera-3d-lidar-inl.h │ │ ├── camera-pinhole-inl.h │ │ └── camera-factory.h ├── package.xml ├── CMakeLists.txt └── src │ ├── distortion.cc │ ├── random-camera-generator.cc │ └── camera-factory.cc ├── aslam_cv_visualization ├── CMakeLists.txt ├── package.xml ├── include │ └── aslam │ │ └── visualization │ │ └── basic-visualization.h └── src │ └── basic-visualization.cc ├── aslam_cv_frames ├── package.xml ├── CMakeLists.txt ├── include │ └── aslam │ │ └── frames │ │ ├── feature-track-inl.h │ │ ├── keypoint-identifier.h │ │ └── feature-track.h └── test │ └── test-visual-nframe.cc ├── aslam_cv_matcher ├── CMakeLists.txt ├── package.xml ├── include │ └── aslam │ │ └── matcher │ │ ├── match.h │ │ └── match-helpers.h └── src │ └── match-helpers.cc ├── aslam_cv_triangulation ├── package.xml └── CMakeLists.txt ├── aslam_cv_detector ├── CMakeLists.txt ├── include │ ├── aslam │ │ └── detectors │ │ │ ├── line.h │ │ │ └── line-segment-detector.h │ └── kaze │ │ ├── fed.h │ │ └── KAZEConfig.h ├── package.xml └── src │ ├── kaze │ └── LICENSE │ └── line-segment-detector.cc ├── aslam_cv_calibration ├── package.xml ├── CMakeLists.txt ├── include │ └── aslam │ │ └── calibration │ │ ├── target-algorithms.h │ │ ├── focallength-initializers.h │ │ ├── internal │ │ └── focallength-initializers-traits-inl.h │ │ ├── target-base.h │ │ ├── helpers.h │ │ ├── target-aprilgrid.h │ │ └── target-observation.h └── src │ ├── target-algorithms.cc │ └── target-base.cc ├── aslam_cv_geometric_vision ├── package.xml ├── CMakeLists.txt └── include │ └── aslam │ └── geometric-vision │ └── match-outlier-rejection-twopt.h └── aslam_cv_tracker ├── package.xml ├── CMakeLists.txt ├── include └── aslam │ └── tracker │ ├── tracking-helpers.h │ ├── feature-tracker.h │ └── track-manager.h ├── src └── track-manager.cc └── test └── test-track-manager.cc /aslam_cv_common/proto/aslam/common/id.proto: -------------------------------------------------------------------------------- 1 | package aslam.proto; 2 | 3 | message Id { 4 | repeated uint64 uint = 1; 5 | } 6 | -------------------------------------------------------------------------------- /aslam_cv_common/src/hash-id.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | 5 | const char HashId::kHexConversion[] = "0123456789abcdef"; 6 | 7 | } // namespace aslam 8 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \mainpage aslam_cv -- Computer Vision from ASL 4 | 5 | This is the mainpage description for the "aslam_cv -- Computer Vision from ASL" project as found in 6 | mainpage.dox. 7 | 8 | */ 9 | 10 | 11 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/src/undistorter.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | 5 | Undistorter::Undistorter(Camera::Ptr input_camera, Camera::Ptr output_camera) 6 | : input_camera_(input_camera), 7 | output_camera_(output_camera) {} 8 | 9 | 10 | } // namespace aslam 11 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/src/visual-pipeline-null.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace aslam { 7 | 8 | NullVisualPipeline::NullVisualPipeline(const Camera::ConstPtr& camera, bool copy_image) : 9 | VisualPipeline(camera, camera, copy_image) {} 10 | 11 | } // namespace aslam 12 | -------------------------------------------------------------------------------- /CONTRIBUTORS.md: -------------------------------------------------------------------------------- 1 | # Contributors 2 | 3 | * Thomas Schneider 4 | * Marcin Dymczyk 5 | * Paul Furgale 6 | * Mathias Bürki 7 | * Simon Lynen 8 | * Mathias Gehrig 9 | * Mike Bosse 10 | * Titus Cieslewski 11 | * Marius Fehr 12 | * Christian Forster 13 | * Kevin Egger 14 | * Timo Hinzmann 15 | * Fabian Dubios 16 | * Helen Oleynikova 17 | * Michael Burri 18 | * Ricard Boada 19 | * Mohit Agarwal 20 | * Andreas Forster 21 | * Fadri Furrer 22 | * Hannes Sommer 23 | * Renaud Dube 24 | * Fabian Blöchliger 25 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | *~ 23 | 24 | # Eclipse project files 25 | .project 26 | .cproject 27 | .settings 28 | *~ 29 | 30 | # IntelliJ IDEA directory 31 | .idea 32 | 33 | # MacOS Desktop Services Store 34 | .DS_Store 35 | 36 | .vi_command_history 37 | mapapi-discovery.txt 38 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/covariance-helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COVARIANCE_HELPERS_H_ 2 | #define ASLAM_COVARIANCE_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace aslam { 8 | namespace common { 9 | 10 | void rotateCovariance( 11 | const aslam::Transformation& T_B_A, 12 | const aslam::TransformationCovariance& A_covariance, 13 | aslam::TransformationCovariance* B_covariance); 14 | 15 | } // namespace common 16 | } // namespace aslam 17 | #endif // ASLAM_COVARIANCE_HELPERS_H_ 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # aslam_cv2 2 | 3 | Refactored computer vision algorithms for the ASL. 4 | 5 | Before adding to this library, please read the following pages: 6 | 7 | * [Aslam cv code & devel style](https://github.com/ethz-asl/aslam_cv2/wiki/Aslam-cv-specific-code-style) 8 | * [Aslam cv mathematical notation](https://github.com/ethz-asl/aslam_cv2/wiki/Expressing-frame-transformations-in-code) 9 | 10 | ### Please strictly keep to the pull-request & merge workflow. 11 | 12 | ## Authors 13 | 14 | * Thomas Schneider 15 | * Marcin Dymczyk 16 | * Paul Furgale 17 | * Mathias Bürki 18 | * Simon Lynen 19 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/predicates.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_PREDICATES_H 2 | #define ASLAM_PREDICATES_H 3 | 4 | namespace aslam { 5 | 6 | template 7 | bool checkSharedEqual(const std::shared_ptr & lhs, 8 | const std::shared_ptr & rhs) { 9 | if(lhs && rhs) { 10 | // if they are both nonnull, check for equality 11 | return (*lhs) == (*rhs); 12 | } else { 13 | // otherwise, check if they are both null 14 | return (!lhs) && (!rhs); 15 | } 16 | } 17 | 18 | } // namespace aslam 19 | 20 | 21 | #endif /* ASLAM_PREDICATES_H */ 22 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/reader-first-reader-writer-lock.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_READER_FIRST_READER_WRITER_LOCK_H_ 2 | #define ASLAM_COMMON_READER_FIRST_READER_WRITER_LOCK_H_ 3 | 4 | #include "aslam/common/reader-writer-lock.h" 5 | 6 | namespace aslam { 7 | 8 | class ReaderFirstReaderWriterMutex : public ReaderWriterMutex { 9 | public: 10 | ReaderFirstReaderWriterMutex(); 11 | ~ReaderFirstReaderWriterMutex(); 12 | 13 | virtual void acquireReadLock() override; 14 | 15 | virtual void acquireWriteLock() override; 16 | }; 17 | 18 | } // namespace aslam 19 | 20 | #endif // ASLAM_COMMON_READER_FIRST_READER_WRITER_LOCK_H_ 21 | -------------------------------------------------------------------------------- /aslam_cv_cameras/include/aslam/cameras/distortion-inl.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | 5 | inline std::ostream& operator<<(std::ostream& out, const Distortion::Type& value) { 6 | static std::map names; 7 | if (names.size() == 0) { 8 | #define INSERT_ELEMENT(type, val) names[type::val] = #val 9 | INSERT_ELEMENT(Distortion::Type, kNoDistortion); 10 | INSERT_ELEMENT(Distortion::Type, kEquidistant); 11 | INSERT_ELEMENT(Distortion::Type, kFisheye); 12 | INSERT_ELEMENT(Distortion::Type, kRadTan); 13 | #undef INSERT_ELEMENT 14 | } 15 | return out << names[value]; 16 | } 17 | 18 | } // namespace aslam 19 | -------------------------------------------------------------------------------- /aslam_cv_common/src/internal/unique-id.cc: -------------------------------------------------------------------------------- 1 | #include "aslam/common/internal/unique-id.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace aslam { 7 | namespace internal { 8 | void generateUnique128BitHash(uint64_t hash[2]) { 9 | static std::atomic counter; 10 | hash[0] = 11 | aslam::internal::UniqueIdHashSeed::instance().seed() ^ 12 | std::hash()( 13 | std::chrono::high_resolution_clock::now().time_since_epoch().count()); 14 | // Increment must happen here, otherwise the sampling is not atomic. 15 | hash[1] = std::hash()(++counter); 16 | } 17 | } // namespace internal 18 | } // namespace common 19 | -------------------------------------------------------------------------------- /aslam_cv_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_visualization) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(HEADERS 11 | include/aslam/visualization/basic-visualization.h 12 | ) 13 | 14 | set(SOURCES 15 | src/basic-visualization.cc 16 | ) 17 | 18 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 19 | 20 | add_doxygen(NOT_AUTOMATIC) 21 | 22 | SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") 23 | 24 | ########## 25 | # EXPORT # 26 | ########## 27 | cs_install() 28 | cs_export() 29 | -------------------------------------------------------------------------------- /aslam_cv_cameras/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_cameras 4 | 0.0.0 5 | aslam_cv_cameras 6 | schneith 7 | Apache 2.0 8 | 9 | catkin 10 | catkin_simple 11 | 12 | aslam_cv_common 13 | doxygen_catkin 14 | eigen_catkin 15 | eigen_checks 16 | gflags_catkin 17 | glog_catkin 18 | opencv3_catkin 19 | yaml_cpp_catkin 20 | 21 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/include/aslam/pipeline/test/convert-maps-legacy.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_TEST_CONVERT_MAPS_LEGACY_H 2 | #define ASLAM_TEST_CONVERT_MAPS_LEGACY_H 3 | 4 | #include 5 | 6 | namespace aslam { 7 | 8 | // This function is a copy of the convertMaps function of OpenCV's 2.4.13.1 9 | // branch to circumvent a SSE2 bug that was introduced with OpenCV 3.* that 10 | // causes a loss of accuracy in the map conversion from CV16SC2 to CV32FC1. 11 | void convertMapsLegacy(cv::InputArray _map1, cv::InputArray _map2, 12 | cv::OutputArray _dstmap1, cv::OutputArray _dstmap2, 13 | int dstm1type, bool nninterpolate = false); 14 | } // namespace aslam 15 | 16 | #endif // ASLAM_TEST_CONVERT_MAPS_LEGACY_H 17 | -------------------------------------------------------------------------------- /aslam_cv_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_common 4 | 0.0.0 5 | aslam_cv_common 6 | schneith 7 | Apache 2.0 8 | 9 | catkin 10 | catkin_simple 11 | 12 | doxygen_catkin 13 | eigen_catkin 14 | eigen_checks 15 | gflags_catkin 16 | glog_catkin 17 | minkindr 18 | opencv3_catkin 19 | protobuf_catkin 20 | yaml_cpp_catkin 21 | 22 | -------------------------------------------------------------------------------- /aslam_cv_frames/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_frames 4 | 0.0.0 5 | aslam_cv_frames 6 | schneith 7 | Apache 2.0 8 | 9 | catkin 10 | catkin_simple 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | doxygen_catkin 15 | eigen_catkin 16 | eigen_checks 17 | gflags_catkin 18 | glog_catkin 19 | minkindr 20 | yaml_cpp_catkin 21 | 22 | -------------------------------------------------------------------------------- /aslam_cv_matcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_matcher) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(HEADERS 11 | include/aslam/matcher/gyro-two-frame-matcher.h 12 | include/aslam/matcher/match.h 13 | include/aslam/matcher/match-helpers.h 14 | ) 15 | 16 | set(SOURCES 17 | src/gyro-two-frame-matcher.cc 18 | src/match-helpers.cc 19 | ) 20 | 21 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 22 | 23 | add_doxygen(NOT_AUTOMATIC) 24 | 25 | SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") 26 | 27 | ########## 28 | # EXPORT # 29 | ########## 30 | cs_install() 31 | cs_export() 32 | -------------------------------------------------------------------------------- /aslam_cv_triangulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_triangulation 4 | 0.0.0 5 | aslam_cv_triangulation 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_common 13 | aslam_cv_frames 14 | doxygen_catkin 15 | eigen_catkin 16 | eigen_checks 17 | gflags_catkin 18 | glog_catkin 19 | minkindr 20 | yaml_cpp_catkin 21 | 22 | -------------------------------------------------------------------------------- /aslam_cv_frames/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_frames) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(SOURCES 11 | src/visual-frame.cc 12 | src/visual-nframe.cc 13 | ) 14 | cs_add_library(${PROJECT_NAME} ${SOURCES}) 15 | 16 | add_doxygen(NOT_AUTOMATIC) 17 | 18 | ########## 19 | # GTESTS # 20 | ########## 21 | catkin_add_gtest(test_visual-frame test/test-visual-frame.cc) 22 | target_link_libraries(test_visual-frame ${PROJECT_NAME}) 23 | 24 | catkin_add_gtest(test_visual-nframe test/test-visual-nframe.cc) 25 | target_link_libraries(test_visual-nframe ${PROJECT_NAME}) 26 | 27 | ########## 28 | # EXPORT # 29 | ########## 30 | cs_install() 31 | cs_export() 32 | -------------------------------------------------------------------------------- /aslam_cv_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_visualization 4 | 0.0.0 5 | visualization for aslam_cv2 6 | mbuerki 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_common 13 | aslam_cv_frames 14 | aslam_cv_matcher 15 | doxygen_catkin 16 | eigen_catkin 17 | eigen_checks 18 | gflags_catkin 19 | glog_catkin 20 | minkindr 21 | 22 | -------------------------------------------------------------------------------- /aslam_cv_common/src/covariance-helpers.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | namespace common { 5 | 6 | void rotateCovariance( 7 | const aslam::Transformation& T_B_A, 8 | const aslam::TransformationCovariance& A_covariance, 9 | aslam::TransformationCovariance* B_covariance) { 10 | Eigen::Matrix covariance_transformation_B_A = 11 | Eigen::Matrix::Identity(); 12 | covariance_transformation_B_A.block<3, 3>(0, 0) = T_B_A.getRotationMatrix(); 13 | covariance_transformation_B_A.block<3, 3>(3, 3) = T_B_A.getRotationMatrix(); 14 | *B_covariance = covariance_transformation_B_A * A_covariance * 15 | covariance_transformation_B_A.transpose(); 16 | } 17 | 18 | } // namespace common 19 | } // namespace aslam 20 | -------------------------------------------------------------------------------- /aslam_cv_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_detector) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | add_definitions(-std=c++11) 11 | 12 | cs_add_library(${PROJECT_NAME}_kaze 13 | src/kaze/fed.cpp 14 | src/kaze/KAZE.cpp 15 | src/kaze/nldiffusion_functions.cpp 16 | src/kaze/utils.cpp 17 | ) 18 | 19 | cs_add_library(${PROJECT_NAME}_lsd 20 | src/lsd/lsd-opencv.cc 21 | ) 22 | 23 | cs_add_library(${PROJECT_NAME} 24 | src/line-segment-detector.cc 25 | ) 26 | 27 | target_link_libraries(${PROJECT_NAME} 28 | ${PROJECT_NAME}_kaze 29 | ${PROJECT_NAME}_lsd 30 | ) 31 | 32 | ########## 33 | # EXPORT # 34 | ########## 35 | cs_install() 36 | cs_export() 37 | -------------------------------------------------------------------------------- /aslam_cv_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_calibration 4 | 0.0.0 5 | aslam_cv_calibration 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | aslam_cv_geometric_vision 15 | doxygen_catkin 16 | eigen_catkin 17 | eigen_checks 18 | ethzasl_apriltag2 19 | gflags_catkin 20 | glog_catkin 21 | minkindr 22 | 23 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/eigen-hash.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_EIGEN_HASH_H_ 2 | #define ASLAM_COMMON_EIGEN_HASH_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace std { 9 | 10 | template 11 | struct hash> { 12 | // https://wjngkoh.wordpress.com/2015/03/04/c-hash-function-for-eigen-matrix-and-vector/ 13 | size_t operator()(const Eigen::Matrix& matrix) const { 14 | size_t seed = 0; 15 | for (size_t i = 0; i < static_cast(matrix.size()); ++i) { 16 | Scalar elem = *(matrix.data() + i); 17 | seed ^= 18 | std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); 19 | } 20 | return seed; 21 | } 22 | }; 23 | 24 | } // namespace std 25 | 26 | #endif // ASLAM_COMMON_EIGEN_HASH_H_ 27 | -------------------------------------------------------------------------------- /aslam_cv_common/cmake/detect_simd.cmake: -------------------------------------------------------------------------------- 1 | # Detect the preprocessor directives which are set by the compiler. 2 | execute_process(COMMAND ${CMAKE_CXX_COMPILER} -march=native -dM -E -x c /dev/null 3 | OUTPUT_VARIABLE PREPROCESSOR_DIRECTIVES) 4 | 5 | set(IS_SSE_ENABLED FALSE) 6 | set(IS_NEON_ENABLED FALSE) 7 | if (PREPROCESSOR_DIRECTIVES MATCHES "__SSSE3__") 8 | add_definitions(-mssse3) 9 | set(IS_SSE_ENABLED TRUE) 10 | # For both armv7 and armv8, __ARM_NEON is used as preprocessor directive. 11 | elseif (PREPROCESSOR_DIRECTIVES MATCHES "__ARM_ARCH 7") 12 | add_definitions(-mfpu=neon) # Needs to be set for armv7. 13 | set(IS_NEON_ENABLED TRUE) 14 | elseif (PREPROCESSOR_DIRECTIVES MATCHES "__ARM_ARCH 8") 15 | set(IS_NEON_ENABLED TRUE) 16 | else() 17 | message(WARNING "No SIMD instruction set detected.") 18 | endif() 19 | 20 | unset(PREPROCESSOR_DIRECTIVES CACHE) 21 | -------------------------------------------------------------------------------- /aslam_cv_matcher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_matcher 4 | 0.0.0 5 | aslam_cv_matcher 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | aslam_cv_frames 15 | doxygen_catkin 16 | eigen_catkin 17 | eigen_checks 18 | gflags_catkin 19 | glog_catkin 20 | libnabo 21 | minkindr 22 | opencv3_catkin 23 | yaml_cpp_catkin 24 | 25 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_pipeline 4 | 0.0.0 5 | aslam_cv_pipeline 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | aslam_cv_frames 15 | brisk 16 | doxygen_catkin 17 | eigen_catkin 18 | eigen_checks 19 | gflags_catkin 20 | glog_catkin 21 | minkindr 22 | opencv3_catkin 23 | yaml_cpp_catkin 24 | 25 | -------------------------------------------------------------------------------- /aslam_cv_detector/include/aslam/detectors/line.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CV_DETECTORS_LINE 2 | #define ASLAM_CV_DETECTORS_LINE 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace aslam { 11 | 12 | template 13 | struct LineImpl { 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | typedef Eigen::Matrix PointType; 16 | 17 | LineImpl(Type x0, Type y0, Type x1, Type y1) : 18 | start_point(x0, y0), end_point(x1, y1) {}; 19 | 20 | LineImpl(const PointType& start, const PointType& end) : 21 | start_point(start), end_point(end) {}; 22 | 23 | PointType start_point; 24 | PointType end_point; 25 | }; 26 | typedef LineImpl Line; 27 | typedef std::vector Lines; 28 | 29 | } // namespace aslam 30 | 31 | #endif // ASLAM_CV_DETECTORS_LINE 32 | -------------------------------------------------------------------------------- /aslam_cv_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_detector 4 | 0.0.0 5 | aslam_cv_detector 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | aslam_cv_frames 15 | aslam_cv_matcher 16 | brisk 17 | doxygen_catkin 18 | eigen_catkin 19 | eigen_checks 20 | gflags_catkin 21 | glog_catkin 22 | minkindr 23 | opencv3_catkin 24 | yaml_cpp_catkin 25 | 26 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/entrypoint.h: -------------------------------------------------------------------------------- 1 | #ifndef GTEST_CATKIN_ENTRYPOINT_H_ 2 | #define GTEST_CATKIN_ENTRYPOINT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define ASLAM_UNITTEST_ENTRYPOINT\ 9 | int main(int argc, char** argv) {\ 10 | ::testing::InitGoogleTest(&argc, argv);\ 11 | google::InitGoogleLogging(argv[0]);\ 12 | google::ParseCommandLineFlags(&argc, &argv, false);\ 13 | google::InstallFailureSignalHandler();\ 14 | ::testing::FLAGS_gtest_death_test_style = "threadsafe";\ 15 | FLAGS_alsologtostderr = true; \ 16 | FLAGS_colorlogtostderr = true; \ 17 | return RUN_ALL_TESTS();\ 18 | } 19 | 20 | // Make the eclipse parser silent. 21 | #ifndef TYPED_TEST 22 | #define TYPED_TEST(x,y) int x##y() 23 | #endif 24 | #ifndef TYPED_TEST_CASE 25 | #define TYPED_TEST_CASE(x,y) int x##y() 26 | #endif 27 | 28 | #endif // GTEST_CATKIN_ENTRYPOINT_H_ 29 | -------------------------------------------------------------------------------- /aslam_cv_geometric_vision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_geometric_vision 4 | 0.0.0 5 | aslam_cv_geometric_vision 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | aslam_cv_frames 15 | aslam_cv_matcher 16 | doxygen_catkin 17 | eigen_catkin 18 | eigen_checks 19 | gflags_catkin 20 | glog_catkin 21 | minkindr 22 | opencv3_catkin 23 | opengv 24 | yaml_cpp_catkin 25 | 26 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/internal/unique-id.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_INTERNAL_UNIQUE_ID_H_ // NOLINT 2 | #define ASLAM_COMMON_INTERNAL_UNIQUE_ID_H_ // NOLINT 3 | 4 | #include 5 | #include 6 | 7 | #include "aslam/common/id.pb.h" 8 | 9 | namespace aslam { 10 | namespace internal { 11 | class UniqueIdHashSeed { 12 | public: 13 | class Key { 14 | Key() {} 15 | }; 16 | 17 | UniqueIdHashSeed() : seed_(31u) {} 18 | static UniqueIdHashSeed& instance() { 19 | static UniqueIdHashSeed instance; 20 | return instance; 21 | } 22 | 23 | void saltSeed(const Key&, uint64_t salt) { 24 | seed_ ^= salt; 25 | } 26 | 27 | uint64_t seed() const { 28 | return seed_; 29 | } 30 | 31 | private: 32 | std::atomic seed_; 33 | }; 34 | 35 | void generateUnique128BitHash(uint64_t hash[2]); 36 | } // namespace internal 37 | } // namespace aslam 38 | 39 | #endif // ASLAM_COMMON_INTERNAL_UNIQUE_ID_H_ NOLINT 40 | -------------------------------------------------------------------------------- /aslam_cv_common/test/test-covariance-helpers.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace aslam { 7 | namespace common { 8 | 9 | TEST(TestCovarianceHelpers, ForwardAndBackwardRotation) { 10 | for (int i = 0; i < 1000; i++) { 11 | aslam::Transformation T_A_B, T_B_A; 12 | T_A_B.setRandom(); 13 | T_B_A = T_A_B.inverse(); 14 | aslam::TransformationCovariance A_covariance, B_covariance, 15 | A_covariance_expected; 16 | 17 | A_covariance_expected = Eigen::Matrix::Random(); 18 | rotateCovariance(T_B_A, A_covariance_expected, &B_covariance); 19 | rotateCovariance(T_A_B, B_covariance, &A_covariance); 20 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(A_covariance, A_covariance_expected, 1E-4)); 21 | } 22 | } 23 | 24 | } // namespace common 25 | } // namespace aslam 26 | 27 | ASLAM_UNITTEST_ENTRYPOINT 28 | -------------------------------------------------------------------------------- /aslam_cv_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_cv_tracker 4 | 0.0.0 5 | aslam_cv_tracker 6 | schneith 7 | Apache 2.0 8 | 9 | catkin_simple 10 | catkin 11 | 12 | aslam_cv_cameras 13 | aslam_cv_common 14 | aslam_cv_detector 15 | aslam_cv_frames 16 | aslam_cv_matcher 17 | brisk 18 | doxygen_catkin 19 | eigen_catkin 20 | eigen_checks 21 | gflags_catkin 22 | glog_catkin 23 | minkindr 24 | opencv3_catkin 25 | yaml_cpp_catkin 26 | 27 | -------------------------------------------------------------------------------- /aslam_cv_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_tracker) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(HEADERS 11 | include/aslam/tracker/feature-tracker.h 12 | include/aslam/tracker/feature-tracker-gyro.h 13 | include/aslam/tracker/track-manager.h 14 | ) 15 | 16 | set(SOURCES 17 | src/feature-tracker-gyro.cc 18 | src/track-manager.cc 19 | src/tracking-helpers.cc 20 | ) 21 | 22 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 23 | 24 | add_doxygen(NOT_AUTOMATIC) 25 | 26 | SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") 27 | 28 | ########## 29 | # GTESTS # 30 | ########## 31 | catkin_add_gtest(test_track_manager test/test-track-manager.cc) 32 | target_link_libraries(test_track_manager ${PROJECT_NAME}) 33 | 34 | ########## 35 | # EXPORT # 36 | ########## 37 | cs_install() 38 | cs_export() 39 | -------------------------------------------------------------------------------- /aslam_cv_detector/include/aslam/detectors/line-segment-detector.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CV_DETECTORS_LSD 2 | #define ASLAM_CV_DETECTORS_LSD 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "aslam/detectors/line.h" 11 | 12 | namespace aslam { 13 | 14 | class LineSegmentDetector { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | struct Options { 19 | size_t min_segment_length_px; 20 | Options() : 21 | min_segment_length_px(20u) {}; 22 | }; 23 | 24 | LineSegmentDetector(const Options& options); 25 | ~LineSegmentDetector(); 26 | 27 | void detect(const cv::Mat& image, Lines* lines); 28 | 29 | /// Draw a list of lines onto a color(!) image. 30 | void drawLines(const Lines& lines, cv::Mat* image); 31 | 32 | private: 33 | cv::Ptr line_detector_; 34 | const Options options_; 35 | }; 36 | 37 | } // namespace aslam 38 | 39 | #endif // ASLAM_CV_DETECTORS_LSD 40 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/meta.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_META_H_ 2 | #define ASLAM_META_H_ 3 | 4 | template 5 | class this_type { 6 | public: 7 | using is_valid = void; 8 | }; 9 | 10 | template 11 | class is_cloneable : public std::false_type {}; 12 | 13 | template 14 | class is_cloneable().clone())>::is_valid> : 15 | public std::true_type {}; 16 | 17 | template struct is_not_pointer_helper : std::true_type {}; 18 | template struct is_not_pointer_helper : std::false_type {}; 19 | template struct is_not_pointer_helper> : std::false_type {}; 20 | template struct is_not_pointer_helper> : std::false_type {}; 21 | template struct is_not_pointer_helper>> : std::false_type {}; 22 | template struct is_not_pointer : is_not_pointer_helper::type> {}; 23 | 24 | #endif // ASLAM_META_H_ 25 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/types.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_TYPES_H_ 2 | #define ASLAM_TYPES_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace aslam { 8 | 9 | /// Interpolation method used for image undistortion. (Wrapper for OpenCV types) 10 | enum class InterpolationMethod : int { 11 | /// A bilinear interpolation (used by default). 12 | Linear = cv::INTER_LINEAR, 13 | /// A bicubic interpolation over 4x4 pixel neighborhood. 14 | Cubic = cv::INTER_CUBIC, 15 | /// A Lanczos interpolation over 8x8 pixel neighborhood. 16 | Lanczos = cv::INTER_LANCZOS4, 17 | /// A nearest-neighbor interpolation. 18 | NearestNeighbor = cv::INTER_NEAREST, 19 | /// Resampling using pixel area relation. It may be a prefered method for image decimation, 20 | /// as it gives moire-free results. But when the image is zoomed, it is similar to the 21 | /// INTER_NEAREST method. 22 | Area = cv::INTER_AREA 23 | }; 24 | 25 | } // namespace aslam 26 | 27 | #endif //ASLAM_TYPES_H_ 28 | -------------------------------------------------------------------------------- /aslam_cv_geometric_vision/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_geometric_vision) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(HEADERS 11 | include/aslam/geometric-vision/match-outlier-rejection-twopt.h 12 | include/aslam/geometric-vision/pnp-pose-estimator.h 13 | ) 14 | 15 | set(SOURCES 16 | src/match-outlier-rejection-twopt.cc 17 | src/pnp-pose-estimator.cc 18 | ) 19 | 20 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 21 | add_doxygen(NOT_AUTOMATIC) 22 | 23 | add_definitions(-std=c++11) 24 | SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") 25 | 26 | ######### 27 | # TESTS # 28 | ######### 29 | catkin_add_gtest(test_pnp_pose_estimator_test 30 | test/test_pnp_pose_estimator_test.cc) 31 | target_link_libraries(test_pnp_pose_estimator_test ${PROJECT_NAME}) 32 | 33 | ########## 34 | # EXPORT # 35 | ########## 36 | cs_install() 37 | cs_export() 38 | -------------------------------------------------------------------------------- /aslam_cv_calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_calibration) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | 11 | set(HEADERS 12 | include/aslam/calibration/target-base.h 13 | ) 14 | 15 | set(SOURCES 16 | src/focallength-initializers.cc 17 | src/target-algorithms.cc 18 | src/target-aprilgrid.cc 19 | src/target-base.cc 20 | ) 21 | 22 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 23 | add_doxygen(NOT_AUTOMATIC) 24 | 25 | ########## 26 | # GTESTS # 27 | ########## 28 | catkin_add_gtest(test_init_intrinsics test/test-init-intrinsics.cc) 29 | target_link_libraries(test_init_intrinsics ${PROJECT_NAME}) 30 | 31 | catkin_add_gtest(test_target_observation test/test-target-observation.cc) 32 | target_link_libraries(test_target_observation ${PROJECT_NAME}) 33 | 34 | ########## 35 | # EXPORT # 36 | ########## 37 | cs_install_scripts(python/create-target-pdf.py) 38 | cs_install() 39 | cs_export() 40 | -------------------------------------------------------------------------------- /aslam_cv_common/src/reader-first-reader-writer-lock.cc: -------------------------------------------------------------------------------- 1 | #include "aslam/common/reader-first-reader-writer-lock.h" 2 | 3 | namespace aslam { 4 | 5 | ReaderFirstReaderWriterMutex::ReaderFirstReaderWriterMutex() 6 | : ReaderWriterMutex() {} 7 | 8 | ReaderFirstReaderWriterMutex::~ReaderFirstReaderWriterMutex() {} 9 | 10 | void ReaderFirstReaderWriterMutex::acquireReadLock() { 11 | std::unique_lock lock(mutex_); 12 | while (current_writer_) { 13 | cv_writer_finished_.wait(lock); 14 | } 15 | ++num_readers_; 16 | } 17 | 18 | void ReaderFirstReaderWriterMutex::acquireWriteLock() { 19 | std::unique_lock lock(mutex_); 20 | while (true) { 21 | while (num_readers_ > (pending_upgrade_ ? 1 : 0)) { 22 | cv_readers_.wait(lock); 23 | } 24 | if (!current_writer_ && !pending_upgrade_) { 25 | break; 26 | } else { 27 | while (current_writer_ || pending_upgrade_) { 28 | cv_writer_finished_.wait(lock); 29 | } 30 | } 31 | } 32 | current_writer_ = true; 33 | } 34 | 35 | } // namespace aslam 36 | -------------------------------------------------------------------------------- /aslam_cv_common/test/reader_writer_mutex_fixture.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_READER_WRITER_MUTEX_FIXTURE_H_ 2 | #define ASLAM_COMMON_READER_WRITER_MUTEX_FIXTURE_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | constexpr int kMagicNumber = 29845; 11 | constexpr int kNumCycles = 1000; 12 | 13 | namespace aslam { 14 | 15 | class ReaderWriterMutexFixture : public ::testing::Test { 16 | private: 17 | int value_; 18 | std::atomic num_writes_; 19 | std::atomic num_upgrade_failures_; 20 | 21 | protected: 22 | virtual void SetUp(); 23 | 24 | void reader(); 25 | void writer(); 26 | void delayedReader(); 27 | void readerUpgrade(); 28 | 29 | int value() { return value_; } 30 | int num_writes() { return num_writes_; } 31 | int num_upgrade_failures() { return num_upgrade_failures_; } 32 | 33 | ReaderWriterMutex value_mutex_; 34 | }; 35 | 36 | } // namespace aslam 37 | 38 | #include "./reader_writer_mutex_fixture_inl.h" 39 | 40 | #endif // ASLAM_COMMON_READER_WRITER_MUTEX_FIXTURE_H_ 41 | -------------------------------------------------------------------------------- /aslam_cv_calibration/include/aslam/calibration/target-algorithms.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CALIBRATION_TARGET_ALGORITHMS_H 2 | #define ASLAM_CALIBRATION_TARGET_ALGORITHMS_H 3 | 4 | #include 5 | #include 6 | 7 | #include "aslam/calibration/target-observation.h" 8 | 9 | namespace aslam { 10 | namespace calibration { 11 | 12 | // Estimates the target transform from the given corner observations. 13 | bool estimateTargetTransformation( 14 | const TargetObservation& target_observation, 15 | const aslam::Camera::ConstPtr& camera_ptr, aslam::Transformation* T_G_C); 16 | 17 | // Estimates the target transform from the given corner observations (with 18 | // additional options). 19 | bool estimateTargetTransformation( 20 | const TargetObservation& target_observation, 21 | const aslam::Camera::ConstPtr& camera_ptr, aslam::Transformation* T_G_C, 22 | const bool run_nonlinear_refinement, const double ransac_pixel_sigma, 23 | const int ransac_max_iters); 24 | 25 | } // namespace calibration 26 | } // namespace aslam 27 | 28 | #endif // ASLAM_CALIBRATION_TARGET_ALGORITHMS_H 29 | -------------------------------------------------------------------------------- /aslam_cv_tracker/include/aslam/tracker/tracking-helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_TRACKING_HELPERS_H_ 2 | #define ASLAM_TRACKING_HELPERS_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace aslam { 14 | class VisualFrame; 15 | 16 | /// Insert a list of OpenCV keypoints and descriptors into an empty VisualFrame. 17 | void insertCvKeypointsAndDescriptorsIntoEmptyVisualFrame( 18 | const std::vector& new_cv_keypoints, const cv::Mat& new_cv_descriptors, 19 | const double fixed_keypoint_uncertainty_px, aslam::VisualFrame* frame); 20 | 21 | /// Append a list of OpenCV kepoints and descriptors to a VisualFrame. 22 | void insertAdditionalCvKeypointsAndDescriptorsToVisualFrame( 23 | const std::vector& new_cv_keypoints, const cv::Mat& new_cv_descriptors, 24 | const double fixed_keypoint_uncertainty_px, aslam::VisualFrame* frame); 25 | 26 | } // namespace aslam 27 | 28 | #endif // ASLAM_TRACKING_HELPERS_H_ 29 | -------------------------------------------------------------------------------- /aslam_cv_triangulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_triangulation) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(HEADERS 11 | include/aslam/triangulation/triangulation.h 12 | ) 13 | 14 | set(SOURCES 15 | src/triangulation.cc 16 | ) 17 | 18 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 19 | 20 | # TODO(tcies) reenable once sm timing fixed in tango 21 | # cs_add_executable(triangulation-benchmark benchmark/triangulation-benchmark.cc) 22 | # target_link_libraries(triangulation-benchmark ${PROJECT_NAME} gtest pthread) 23 | 24 | add_doxygen(NOT_AUTOMATIC) 25 | 26 | SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") 27 | 28 | ########## 29 | # GTESTS # 30 | ########## 31 | catkin_add_gtest(test_triangulation 32 | test/test-triangulation.cc 33 | include/aslam/triangulation/test/triangulation-fixture.h 34 | ) 35 | target_link_libraries(test_triangulation ${PROJECT_NAME}) 36 | 37 | ########## 38 | # EXPORT # 39 | ########## 40 | cs_install() 41 | cs_export() 42 | -------------------------------------------------------------------------------- /aslam_cv_matcher/include/aslam/matcher/match.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_MATCH_H_ 2 | #define ASLAM_MATCH_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace aslam { 11 | 12 | /// \brief A struct to encapsulate a match between two lists. 13 | /// The matches are indices into these lists. 14 | struct FrameToFrameMatch : std::pair { 15 | FrameToFrameMatch() = default; 16 | FrameToFrameMatch(int first_, int second_) 17 | : std::pair(first_, second_) {} 18 | virtual ~FrameToFrameMatch() = default; 19 | int getKeypointIndexInFrameA() const { 20 | return first; 21 | } 22 | void setKeypointIndexInFrameA(int first_) { 23 | first = first_; 24 | } 25 | int getKeypointIndexInFrameB() const { 26 | return second; 27 | } 28 | void setKeypointIndexInFrameB(int second_) { 29 | second = second_; 30 | } 31 | }; 32 | 33 | typedef Aligned FrameToFrameMatches; 34 | typedef Aligned FrameToFrameMatchesList; 35 | 36 | } // namespace aslam 37 | 38 | #endif // ASLAM_MATCH_H_ 39 | -------------------------------------------------------------------------------- /aslam_cv_common/cmake/export_flags.cmake: -------------------------------------------------------------------------------- 1 | include(${CMAKE_CURRENT_LIST_DIR}/detect_simd.cmake) 2 | 3 | include(CheckCXXCompilerFlag) 4 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 5 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 6 | if(COMPILER_SUPPORTS_CXX14) 7 | set(CMAKE_CXX14_EXTENSION_COMPILE_OPTION -std=c++14) 8 | set(CMAKE_CXX_STANDARD 14) 9 | elseif(COMPILER_SUPPORTS_CXX11) 10 | add_definitions(--std=c++11) 11 | else() 12 | message(FATAL_ERROR "Compiler does not support C++11 nor C++14!") 13 | endif() 14 | 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 16 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") 18 | endif() 19 | 20 | set(ENABLE_TIMING FALSE CACHE BOOL "Set to TRUE to enable timing") 21 | message(STATUS "Timers enabled? ${ENABLE_TIMING}") 22 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_TIMING=${ENABLE_TIMING}") 23 | 24 | set(ENABLE_STATISTICS FALSE CACHE BOOL "Set to TRUE to enable statistics") 25 | message(STATUS "Statistics enabled? ${ENABLE_STATISTICS}") 26 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_STATISTICS=${ENABLE_STATISTICS}") 27 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/crtp-clone.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CV_CRTP_CLONE_H_ 2 | #define ASLAM_CV_CRTP_CLONE_H_ 3 | 4 | namespace aslam { 5 | 6 | /// \class Cloneable 7 | /// \brief Implements the cloneable concept using the CRTP and constructor inheritance. 8 | /// Example: 9 | /// @code 10 | /// class Vehicle { 11 | /// Vehicle() = delete; 12 | /// Vehicle(int speed) : speed_(speed) {}; 13 | /// virtual Vehicle* clone() const = 0; // Make sure to add the pure virtual function clone 14 | /// const int speed_; 15 | /// }; 16 | /// 17 | /// class Car : public aslam::Cloneable { 18 | /// Car(int speed) : Base(speed) {}; // Use the "Base" typedef defined by the Cloneable class 19 | /// }; 20 | /// @endcode 21 | template 22 | class Cloneable : public BaseType { 23 | public: 24 | typedef Cloneable Base; 25 | using BaseType::BaseType; 26 | 27 | /// Method to clone this instance 28 | virtual BaseType* clone() const { 29 | return new DerivedType(static_cast(*this)); 30 | }; 31 | 32 | virtual ~Cloneable() {}; 33 | }; 34 | 35 | } // namespace aslam 36 | 37 | #endif // ASLAM_CV_CRTP_CLONE_H_ 38 | -------------------------------------------------------------------------------- /aslam_cv_visualization/include/aslam/visualization/basic-visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_VISUALIZATION_BASIC_VISUALIZATION_H_ 2 | #define ASLAM_VISUALIZATION_BASIC_VISUALIZATION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace aslam_cv_visualization { 14 | 15 | // Takes a frame and draws keypoints on it. 16 | // Does not draw the raw image, but takes it from frame. 17 | void drawKeypoints( 18 | const aslam::VisualFrame& frame, cv::Mat* image, int descriptor_type); 19 | 20 | // Takes two frames and a list of matches between them and draws the matches. 21 | // Does not draw the raw image, but takes it from frame_kp1. 22 | void drawKeypointMatches( 23 | const aslam::VisualFrame& frame_kp1, const aslam::VisualFrame& frame_k, 24 | const aslam::FrameToFrameMatches& matches_kp1_k, int descriptor_type, 25 | cv::Mat* image); 26 | 27 | } // namespace aslam_cv_visualization 28 | 29 | #endif // ASLAM_VISUALIZATION_BASIC_VISUALIZATION_H_ 30 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/pose-types.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_POSE_TYPES_H_ 2 | #define ASLAM_COMMON_POSE_TYPES_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace aslam { 14 | 15 | typedef kindr::minimal::QuatTransformation Transformation; 16 | typedef Aligned TransformationVector; 17 | typedef Eigen::Matrix TransformationCovariance; 18 | typedef Aligned 19 | TransformationCovarianceList; 20 | typedef kindr::minimal::RotationQuaternion Quaternion; 21 | typedef kindr::minimal::AngleAxis AngleAxis; 22 | typedef kindr::minimal::Position Position3D; 23 | 24 | // Types used in ceres error terms, where templating for Jet types 25 | // is necessary. 26 | template 27 | using QuaternionTemplate = kindr::minimal::RotationQuaternionTemplate; 28 | 29 | template 30 | using PositionTemplate = kindr::minimal::PositionTemplate; 31 | 32 | } // namespace aslam 33 | 34 | #endif // ASLAM_COMMON_POSE_TYPES_H_ 35 | -------------------------------------------------------------------------------- /aslam_cv_cameras/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_cameras) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(SOURCES 11 | src/camera-3d-lidar.cc 12 | src/camera-factory.cc 13 | src/camera-pinhole.cc 14 | src/camera-unified-projection.cc 15 | src/camera.cc 16 | src/distortion-equidistant.cc 17 | src/distortion-fisheye.cc 18 | src/distortion-radtan.cc 19 | src/distortion.cc 20 | src/ncamera.cc 21 | src/random-camera-generator.cc 22 | ) 23 | 24 | cs_add_library(${PROJECT_NAME} ${SOURCES}) 25 | 26 | add_doxygen(NOT_AUTOMATIC) 27 | 28 | ########## 29 | # GTESTS # 30 | ########## 31 | catkin_add_gtest(test_cameras test/test-cameras.cc) 32 | target_link_libraries(test_cameras ${PROJECT_NAME}) 33 | 34 | catkin_add_gtest(test_camera_3d_lidar test/test-camera-3d-lidar.cc) 35 | target_link_libraries(test_camera_3d_lidar ${PROJECT_NAME}) 36 | 37 | catkin_add_gtest(test_distortions test/test-distortions.cc) 38 | target_link_libraries(test_distortions ${PROJECT_NAME}) 39 | 40 | catkin_add_gtest(test_ncamera test/test-ncamera.cc) 41 | target_link_libraries(test_ncamera ${PROJECT_NAME}) 42 | 43 | ########## 44 | # EXPORT # 45 | ########## 46 | cs_install() 47 | cs_export() 48 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/include/aslam/pipeline/visual-pipeline-null.h: -------------------------------------------------------------------------------- 1 | #ifndef NULL_VISUAL_PIPELINE_H 2 | #define NULL_VISUAL_PIPELINE_H 3 | 4 | #include "visual-pipeline.h" 5 | 6 | namespace aslam { 7 | 8 | /// \class NullVisualPipeline 9 | /// \brief A visual pipeline that does not transform the image. 10 | class NullVisualPipeline : public VisualPipeline { 11 | public: 12 | ASLAM_POINTER_TYPEDEFS(NullVisualPipeline); 13 | ASLAM_DISALLOW_EVIL_CONSTRUCTORS(NullVisualPipeline); 14 | 15 | /// \param[in] camera The camera that produces the images. 16 | /// \param[in] copyImages If true, images passed in are cloned before storing 17 | /// in the frame. 18 | NullVisualPipeline(const Camera::ConstPtr& camera, bool copyImages); 19 | 20 | virtual ~NullVisualPipeline() {}; 21 | 22 | protected: 23 | /// \brief Process the frame and fill the results into the frame variable 24 | /// 25 | /// The top level function will already fill in the timestamps and the output camera. 26 | /// \param[in] image The image data. 27 | /// \param[in/out] frame The visual frame. This will be constructed before calling. 28 | virtual void processFrameImpl(const cv::Mat& /* image */, 29 | VisualFrame* /* frame */) const { } 30 | 31 | }; 32 | 33 | } // namespace aslam 34 | 35 | #endif // NULL_VISUAL_PIPELINE_H 36 | -------------------------------------------------------------------------------- /aslam_cv_common/test/test-hash-id.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace aslam; 11 | 12 | TEST(HashIdTest, Different) { 13 | HashId a(HashId::random()), b(HashId::random()); 14 | EXPECT_NE(a, b); 15 | } 16 | 17 | TEST(HashIdTest, Validity) { 18 | HashId a, b; 19 | EXPECT_FALSE(a.isValid()); 20 | EXPECT_FALSE(b.isValid()); 21 | EXPECT_EQ(a,b); 22 | generateId(&a); 23 | EXPECT_TRUE(a.isValid()); 24 | } 25 | 26 | TEST(HashIdTest, String) { 27 | HashId a(HashId::random()), b(HashId::random()); 28 | std::string as(a.hexString()), bs(b.hexString()); 29 | EXPECT_NE(as, bs); 30 | EXPECT_EQ(as.length(), 32u); 31 | EXPECT_EQ(bs.length(), 32u); 32 | } 33 | 34 | TEST(HashIdTest, Deserialize) { 35 | HashId a; 36 | std::string as(a.hexString()); 37 | HashId b; 38 | EXPECT_TRUE(b.fromHexString(as)); 39 | EXPECT_EQ(a, b); 40 | } 41 | 42 | TEST(HashIdTest, StdHash) { 43 | std::unordered_set hashes; 44 | HashId needle(HashId::random()); 45 | hashes.insert(needle); 46 | hashes.insert(HashId::random()); 47 | std::unordered_set::iterator found = hashes.find(needle); 48 | EXPECT_TRUE(found != hashes.end()); 49 | EXPECT_EQ(*found, needle); 50 | } 51 | 52 | ASLAM_UNITTEST_ENTRYPOINT 53 | -------------------------------------------------------------------------------- /aslam_cv_matcher/include/aslam/matcher/match-helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_MATCHER_MATCH_HELPERS_H_ 2 | #define ASLAM_MATCHER_MATCH_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "aslam/matcher/match.h" 9 | 10 | namespace aslam { 11 | 12 | /// Return the normalized bearing vectors for a list of single camera matches. 13 | void getBearingVectorsFromMatches( 14 | const VisualFrame& frame_kp1, const VisualFrame& frame_k, 15 | const FrameToFrameMatches& matches_kp1_k, int descriptor_type, 16 | Aligned* bearing_vectors_kp1, 17 | Aligned* bearing_vectors_k); 18 | 19 | /// Rotate keypoints from a VisualFrame using a specified rotation. Note that if the back-, 20 | /// projection fails or the keypoint leaves the image region, the predicted keypoint will be left 21 | /// unchanged and the prediction_success will be set to false. 22 | void predictKeypointsByRotation(const VisualFrame& frame_k, 23 | const aslam::Quaternion& q_Ckp1_Ck, 24 | Eigen::Matrix2Xd* predicted_keypoints_kp1, 25 | std::vector* prediction_success, 26 | int descriptor_type = 0); 27 | 28 | } // namespace aslam 29 | 30 | #endif // ASLAM_MATCHER_MATCH_HELPERS_H_ 31 | -------------------------------------------------------------------------------- /aslam_cv_common/test/reader_writer_lock_test.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "aslam/common/entrypoint.h" 10 | #include "./reader_writer_mutex_fixture.h" 11 | 12 | constexpr int kNumThreads = 20; 13 | 14 | namespace aslam { 15 | 16 | TEST_F(ReaderWriterMutexFixture, ReaderWriterLock) { 17 | aslam::ReaderWriterMutex mutex; 18 | std::vector threads; 19 | for (int i = 0; i < kNumThreads; ++i) { 20 | threads.emplace_back([this]() { reader(); }); 21 | threads.emplace_back([this]() { writer(); }); 22 | } 23 | for (std::thread& thread : threads) { 24 | thread.join(); 25 | } 26 | EXPECT_EQ(0, value() % kMagicNumber); 27 | } 28 | 29 | TEST_F(ReaderWriterMutexFixture, UpgradeReaderLock) { 30 | std::vector threads; 31 | for (int i = 0; i < kNumThreads; ++i) { 32 | threads.emplace_back([this]() { delayedReader(); }); 33 | threads.emplace_back([this]() { readerUpgrade(); }); 34 | } 35 | for (std::thread& thread : threads) { 36 | thread.join(); 37 | } 38 | VLOG(3) << "Number of writes after upgrade: " << num_writes(); 39 | VLOG(3) << "Number of failed upgrades: " << num_upgrade_failures(); 40 | EXPECT_NE(0, value()); 41 | EXPECT_NE(0, num_writes()); 42 | EXPECT_EQ(value(), num_writes() * kMagicNumber); 43 | } 44 | 45 | } // namespace aslam 46 | 47 | ASLAM_UNITTEST_ENTRYPOINT 48 | -------------------------------------------------------------------------------- /aslam_cv_geometric_vision/include/aslam/geometric-vision/match-outlier-rejection-twopt.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_MATCH_OUTLIER_REJECTION_TWOPT_H_ 2 | #define ASLAM_MATCH_OUTLIER_REJECTION_TWOPT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace aslam { 12 | class VisualFrame; 13 | 14 | namespace geometric_vision { 15 | using BearingVectors = 16 | std::vector>; 17 | 18 | // RANSAC threshold can be defined as: 1 - cos(max_ray_disparity_angle). 19 | bool rejectOutlierFeatureMatchesTranslationRotationSAC( 20 | const aslam::VisualFrame& frame_kp1, const aslam::VisualFrame& frame_k, 21 | const aslam::Quaternion& q_Ckp1_Ck, int descriptor_type, 22 | const aslam::FrameToFrameMatches& matches_kp1_k, 23 | bool fix_random_seed, double ransac_threshold, size_t ransac_max_iterations, 24 | aslam::FrameToFrameMatches* inlier_matches_kp1_k, 25 | aslam::FrameToFrameMatches* outlier_matches_kp1_k); 26 | 27 | bool rejectOutlierFeatureMatchesTranslationRotationSAC( 28 | const BearingVectors& bearing_vectors_kp1, 29 | const BearingVectors& bearing_vectors_k, 30 | const aslam::Quaternion& q_Ckp1_Ck, bool fix_random_seed, 31 | double ransac_threshold, size_t ransac_max_iterations, 32 | std::unordered_set* inlier_indices); 33 | 34 | } // namespace geometric_vision 35 | 36 | } // namespace aslam 37 | #endif // ASLAM_MATCH_OUTLIER_REJECTION_TWOPT_H_ 38 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/yaml-file-serialization.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CV_COMMON_YAML_FILE_SERIALIZATION_H_ 2 | #define ASLAM_CV_COMMON_YAML_FILE_SERIALIZATION_H_ 3 | 4 | #include // NOLINT 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace aslam { 12 | class YamlFileSerializable { 13 | public: 14 | virtual ~YamlFileSerializable() = default; 15 | 16 | virtual void serialize(YAML::Node* yaml_node) const = 0; 17 | virtual bool deserialize(const YAML::Node& yaml_node) = 0; 18 | 19 | // Existing files get overwritten. 20 | void serializeToFile(const std::string& file_name) const { 21 | YAML::Node yaml_node; 22 | serialize(&yaml_node); 23 | CHECK(yaml_node.IsDefined()); 24 | 25 | std::ofstream output_file_stream(file_name); 26 | CHECK(output_file_stream.is_open()) 27 | << "Failed to open file " << file_name << " for writing."; 28 | output_file_stream << yaml_node; 29 | output_file_stream.close(); 30 | } 31 | 32 | bool deserializeFromFile(const std::string& file_name) { 33 | YAML::Node yaml_node; 34 | try { 35 | yaml_node = YAML::LoadFile(file_name.c_str()); 36 | } catch (const std::exception& ex) { // NOLINT 37 | LOG(ERROR) << "Failed to load YAML node from file " << file_name 38 | << " with the error: " << ex.what(); 39 | return false; 40 | } 41 | 42 | return deserialize(yaml_node); 43 | } 44 | }; 45 | } // namespace aslam 46 | 47 | #endif // ASLAM_CV_COMMON_YAML_FILE_SERIALIZATION_H_ 48 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_cv_pipeline) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | ############# 8 | # LIBRARIES # 9 | ############# 10 | set(HEADERS 11 | include/aslam/pipeline/test/convert-maps-legacy.h 12 | include/aslam/pipeline/undistorter.h 13 | include/aslam/pipeline/undistorter-mapped.h 14 | include/aslam/pipeline/undistorter-mapped-inl.h 15 | include/aslam/pipeline/visual-npipeline.h 16 | include/aslam/pipeline/visual-pipeline.h 17 | include/aslam/pipeline/visual-pipeline-brisk.h 18 | include/aslam/pipeline/visual-pipeline-freak.h 19 | include/aslam/pipeline/visual-pipeline-null.h 20 | ) 21 | 22 | set(SOURCES 23 | src/test/convert-maps-legacy.cc 24 | src/undistorter.cc 25 | src/undistorter-mapped.cc 26 | src/visual-npipeline.cc 27 | src/visual-pipeline-brisk.cc 28 | src/visual-pipeline-freak.cc 29 | src/visual-pipeline-null.cc 30 | src/visual-pipeline.cc 31 | ) 32 | 33 | cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 34 | 35 | add_doxygen(NOT_AUTOMATIC) 36 | 37 | SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} -lpthread") 38 | 39 | ########## 40 | # GTESTS # 41 | ########## 42 | catkin_add_gtest(test_undistorters test/test-undistorters.cc) 43 | target_link_libraries(test_undistorters ${PROJECT_NAME}) 44 | 45 | catkin_add_gtest(test_visual-npipeline test/test-visual-npipeline.cc) 46 | target_link_libraries(test_visual-npipeline ${PROJECT_NAME}) 47 | 48 | ########## 49 | # EXPORT # 50 | ########## 51 | cs_install() 52 | cs_export() 53 | -------------------------------------------------------------------------------- /aslam_cv_detector/src/kaze/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2012, Pablo Fernández Alcantarilla 2 | All Rights Reserved 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holders nor the names of its contributors 15 | may be used to endorse or promote products derived from this software without 16 | specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 19 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 20 | OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 21 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 23 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 24 | BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 26 | WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /aslam_cv_common/test/test-channels.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | DECLARE_CHANNEL(TEST, Eigen::Matrix2Xd) 8 | 9 | TEST(Channel, FailUnavailableChannel) { 10 | aslam::channels::ChannelGroup channels; 11 | EXPECT_DEATH(aslam::channels::get_TEST_Data(channels), "^"); 12 | } 13 | 14 | TEST(Channel, AddChannel) { 15 | aslam::channels::ChannelGroup channels; 16 | EXPECT_DEATH(aslam::channels::get_TEST_Data(channels), "^"); 17 | aslam::channels::add_TEST_Channel(&channels); 18 | aslam::channels::get_TEST_Data(channels); 19 | } 20 | 21 | TEST(Channel, DoubleAddChannelDeath) { 22 | aslam::channels::ChannelGroup channels; 23 | EXPECT_FALSE(aslam::channels::has_TEST_Channel(channels)); 24 | aslam::channels::add_TEST_Channel(&channels); 25 | EXPECT_DEATH(aslam::channels::add_TEST_Channel(&channels), "^"); 26 | } 27 | 28 | TEST(Channel, HasChannelAddChannelExists) { 29 | aslam::channels::ChannelGroup channels; 30 | EXPECT_FALSE(aslam::channels::has_TEST_Channel(channels)); 31 | aslam::channels::add_TEST_Channel(&channels); 32 | EXPECT_TRUE(aslam::channels::has_TEST_Channel(channels)); 33 | } 34 | 35 | TEST(Channel, SetRetrieveChannel) { 36 | aslam::channels::ChannelGroup channels; 37 | aslam::channels::add_TEST_Channel(&channels); 38 | Eigen::Matrix2Xd& data = aslam::channels::get_TEST_Data(channels); 39 | data.resize(Eigen::NoChange, 3); 40 | data.setRandom(); 41 | Eigen::Matrix2Xd data2 = data; 42 | Eigen::Matrix2Xd& data3 = aslam::channels::get_TEST_Data(channels); 43 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(data3, data2, 1e-8)); 44 | } 45 | 46 | ASLAM_UNITTEST_ENTRYPOINT 47 | 48 | -------------------------------------------------------------------------------- /aslam_cv_common/test/reader_writer_mutex_fixture_inl.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_READER_WRITER_MUTEX_FIXTURE_INL_H_ 2 | #define ASLAM_COMMON_READER_WRITER_MUTEX_FIXTURE_INL_H_ 3 | 4 | #include 5 | 6 | #include "./reader_writer_mutex_fixture.h" 7 | 8 | namespace aslam { 9 | 10 | void ReaderWriterMutexFixture::SetUp() { 11 | ::testing::Test::SetUp(); 12 | value_ = 0; 13 | num_writes_ = 0; 14 | num_upgrade_failures_ = 0; 15 | } 16 | 17 | void ReaderWriterMutexFixture::reader() { 18 | for (int i = 0; i < kNumCycles; ++i) { 19 | aslam::ScopedReadLock lock(&value_mutex_); 20 | EXPECT_EQ(0, value_ % kMagicNumber); 21 | } 22 | } 23 | 24 | void ReaderWriterMutexFixture::writer() { 25 | for (int i = 0; i < kNumCycles; ++i) { 26 | aslam::ScopedWriteLock lock(&value_mutex_); 27 | value_ = i * kMagicNumber; 28 | } 29 | } 30 | 31 | void ReaderWriterMutexFixture::delayedReader() { 32 | for (int i = 0; i < kNumCycles; ++i) { 33 | value_mutex_.acquireReadLock(); 34 | usleep(5); 35 | EXPECT_EQ(value_, (num_writes_) * kMagicNumber); 36 | value_mutex_.releaseReadLock(); 37 | } 38 | } 39 | 40 | void ReaderWriterMutexFixture::readerUpgrade() { 41 | for (int i = 0; i < kNumCycles; ++i) { 42 | value_mutex_.acquireReadLock(); 43 | EXPECT_EQ(0, value_ % kMagicNumber); 44 | int read_value = value_; 45 | usleep(5); 46 | if (value_mutex_.upgradeToWriteLock()) { 47 | value_ = read_value + kMagicNumber; 48 | ++(num_writes_); 49 | value_mutex_.releaseWriteLock(); 50 | } else { 51 | ++num_upgrade_failures_; 52 | } 53 | } 54 | } 55 | 56 | } // namespace aslam 57 | 58 | #endif // ASLAM_COMMON_READER_WRITER_MUTEX_FIXTURE_INL_H_ 59 | -------------------------------------------------------------------------------- /aslam_cv_cameras/include/aslam/cameras/random-camera-generator.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_RANDOM_CAMERA_GENERATOR_H_ 2 | #define ASLAM_RANDOM_CAMERA_GENERATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace aslam { 22 | 23 | /// Create a test NCamera object for unit testing. 24 | NCamera::Ptr createTestNCamera(size_t num_cameras); 25 | NCamera::UniquePtr createUniqueTestNCamera(size_t num_cameras); 26 | 27 | /// Creates an artificial 4-camera rig in a plane with a camera pointing in 28 | /// each direction. (similar to the V-Charge or JanETH camera system) 29 | NCamera::Ptr createSurroundViewTestNCamera(); 30 | NCamera::UniquePtr createSurroundViewUniqueTestNCamera(); 31 | 32 | /// \brief Create a test camera object for intrinsics unit testing. (with null 33 | /// distortion) 34 | template 35 | PinholeCamera::Ptr createIntrinsicsPinholeTestCamera() { 36 | Distortion::UniquePtr zeros = DistortionType::createZeroTestDistortion(); 37 | PinholeCamera::Ptr camera( 38 | new PinholeCamera(400, 400, 319.5, 239.5, 640, 480, zeros)); 39 | CameraId id; 40 | generateId(&id); 41 | camera->setId(id); 42 | return camera; 43 | } 44 | 45 | } // namespace aslam 46 | 47 | #endif /* ASLAM_RANDOM_CAMERA_GENERATOR_H_ */ 48 | -------------------------------------------------------------------------------- /aslam_cv_tracker/include/aslam/tracker/feature-tracker.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_FEATURE_TRACKER_BASE_H_ 2 | #define ASLAM_FEATURE_TRACKER_BASE_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace aslam { 14 | class VisualFrame; 15 | } 16 | 17 | namespace aslam { 18 | /// \class FeatureTracker 19 | /// \brief Base class defining the interface for feature trackers. 20 | class FeatureTracker { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | ASLAM_POINTER_TYPEDEFS(FeatureTracker); 24 | ASLAM_DISALLOW_EVIL_CONSTRUCTORS(FeatureTracker); 25 | 26 | protected: 27 | FeatureTracker() = default; 28 | public: 29 | virtual ~FeatureTracker() {} 30 | 31 | /// Track features and return the matches. The matches are not written to the TrackId channels 32 | /// and should be written to the track id channels using a TrackManager (after e.g. outlier 33 | /// filtering). 34 | virtual void track( 35 | const Quaternion& q_Ckp1_Ck, const VisualFrame& frame_k, aslam::VisualFrame* frame_kp1, 36 | FrameToFrameMatches* matches_with_score_kp1_k) = 0; 37 | 38 | /// Set a list of keypoint indices that should be aborted during the next call to track. 39 | /// The vector is swapped and invalidates the source vector. 40 | virtual void swapKeypointIndicesToAbort(const FrameId& /*frame_id*/, 41 | std::unordered_set* /*keypoint_indices_to_abort*/) { 42 | LOG(FATAL) << "FeatureTracker does not support track abortion."; 43 | } 44 | }; 45 | 46 | } // namespace aslam 47 | 48 | #endif // ASLAM_FEATURE_TRACKER_BASE_H_ 49 | -------------------------------------------------------------------------------- /aslam_cv_common/cmake/setup_openmp.cmake: -------------------------------------------------------------------------------- 1 | # Usage: 2 | # 1. add 'setup_openmp()' after catkin_simple command 3 | # 2. add ${OpenMP_LIBS} to target_link_libraries of your library 4 | # 3. add ${OpenMP_FLAGS} to target_compile_options( PRIVATE ${OpenMP_FLAGS}) of your library 5 | macro(setup_openmp) 6 | if(APPLE) 7 | set(LLVM_PATH "/usr/local/opt/llvm") 8 | find_package(LLVM HINTS ${LLVM_PATH}/lib/cmake/llvm) 9 | if (LLVM_FOUND) 10 | message(STATUS "Building with LLVM clang and omp support.") 11 | set(CMAKE_C_COMPILER ${LLVM_TOOLS_BINARY_DIR}/clang) 12 | set(CMAKE_CXX_COMPILER ${LLVM_TOOLS_BINARY_DIR}/clang++) 13 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fopenmp -L${LLVM_PATH}/lib -Wl,-rpath,${LLVM_PATH}/lib") 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp -L${LLVM_PATH}/lib -Wl,-rpath,${LLVM_PATH}/lib") 15 | else() 16 | message(STATUS "LLVM not found, building without omp support.") 17 | endif() 18 | else() 19 | find_package(OpenMP) 20 | if(OPENMP_FOUND) 21 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL Clang) 22 | set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) 23 | set(OpenMP_LIBS omp) 24 | # add_definitions(-Wno-unknown-pragmas -fopenmp) 25 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL GNU) 26 | set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) 27 | set(OpenMP_LIBS gomp) 28 | # add_definitions(-Wno-unknown-pragmas -fopenmp) 29 | else() 30 | message(WARNING "Unrecognized C++ compiler: ${CMAKE_CXX_COMPILER} ID: ${CMAKE_CXX_COMPILER_ID}. Compiling without openMP support.") 31 | endif() 32 | else() 33 | message(WARNING "OpenMP not found! Compiling without openMP support.") 34 | endif() 35 | endif() 36 | endmacro() 37 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/macros.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_MACROS_H_ 2 | #define ASLAM_COMMON_MACROS_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #define ASLAM_DISALLOW_EVIL_CONSTRUCTORS(TypeName) \ 9 | TypeName(const TypeName&) = delete; \ 10 | void operator=(const TypeName&) = delete 11 | 12 | #define ASLAM_POINTER_TYPEDEFS(TypeName) \ 13 | typedef AlignedUniquePtr UniquePtr; \ 14 | typedef std::shared_ptr Ptr; \ 15 | typedef std::shared_ptr ConstPtr 16 | 17 | /// Extract the type from an expression which wraps a type inside braces. This 18 | /// is done to protect the commas in some types. 19 | template struct ArgumentType; 20 | template struct ArgumentType { typedef U Type; }; 21 | #define GET_TYPE(TYPE) ArgumentType::Type 22 | 23 | #define ASLAM_FORWARD_MEMBER_CONST_ITERATORS(TypeName, member_variable) \ 24 | typedef TypeName::value_type value_type; \ 25 | typedef TypeName::const_iterator const_iterator; \ 26 | TypeName::const_iterator begin() const { \ 27 | return member_variable.begin(); \ 28 | } \ 29 | TypeName::const_iterator end() const { \ 30 | return member_variable.end(); \ 31 | } 32 | 33 | namespace aslam { 34 | namespace common { 35 | namespace macros { 36 | 37 | constexpr double kEpsilon = 1e-8; 38 | 39 | } // namespace macros 40 | } // namespace common 41 | } // namespace aslam 42 | 43 | #endif // ASLAM_COMMON_MACROS_H_ 44 | -------------------------------------------------------------------------------- /aslam_cv_common/src/channel.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | namespace aslam { 8 | namespace channels { 9 | 10 | template<> 11 | bool Channel::operator==(const Channel& other) { 12 | return cv::countNonZero(value_ != other.value_) == 0; 13 | } 14 | 15 | ChannelGroup cloneChannelGroup(const ChannelGroup& channels) { 16 | std::lock_guard lock(channels.m_channels_); 17 | ChannelGroup cloned_group; 18 | for (const ChannelMap::value_type& channel : channels.channels_) { 19 | CHECK(channel.second); 20 | cloned_group.channels_.emplace(channel.first, 21 | std::shared_ptr(channel.second->clone())); 22 | } 23 | return cloned_group; 24 | } 25 | 26 | bool isChannelGroupEqual(const ChannelGroup& left_channels, const ChannelGroup& right_channels) { 27 | // Early exit if both groups are the same. 28 | if (&left_channels == &right_channels) { 29 | return true; 30 | } 31 | std::lock_guard lock_left(left_channels.m_channels_); 32 | std::lock_guard lock_right(right_channels.m_channels_); 33 | 34 | if (left_channels.channels_.size() != right_channels.channels_.size()) { 35 | return false; 36 | } 37 | for (const ChannelMap::value_type& left_channel_pair : left_channels.channels_) { 38 | ChannelMap::const_iterator it_right = right_channels.channels_.find(left_channel_pair.first); 39 | if (it_right == right_channels.channels_.end()) { 40 | return false; 41 | } 42 | if (!CHECK_NOTNULL(it_right->second.get())->compare(*left_channel_pair.second)) { 43 | return false; 44 | } 45 | } 46 | return true; 47 | } 48 | 49 | } // namespace channels 50 | } // namespace aslam 51 | -------------------------------------------------------------------------------- /aslam_cv_calibration/include/aslam/calibration/focallength-initializers.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CALIBRATION_CAMERA_INITIALIZER_H 2 | #define ASLAM_CALIBRATION_CAMERA_INITIALIZER_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "aslam/calibration/target-observation.h" 9 | #include "aslam/calibration/helpers.h" 10 | 11 | namespace aslam { 12 | namespace calibration { 13 | 14 | // Initializes the intrinsics vector based on one views of a calibration targets. 15 | // On success it returns true. These functions are based on functions from Lionel Heng and 16 | // the excellent camodocal: https://github.com/hengli/camodocal. 17 | // This algorithm can be used with high distortion lenses. 18 | // 19 | // C. Hughes, P. Denny, M. Glavin, and E. Jones, 20 | // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point 21 | // Extraction, PAMI 2010 22 | // Find circles from rows of chessboard corners, and for each pair 23 | // of circles, find vanishing points: v1 and v2. 24 | // f = ||v1 - v2|| / PI; 25 | bool initFocalLengthVanishingPoints( 26 | const std::vector& observations, 27 | Eigen::VectorXd* intrinsics); 28 | 29 | // Initializes the intrinsics vector based on one view of a gridded calibration target. 30 | // On success it returns true. These functions are based on functions from Lionel Heng 31 | // and the excellent camodocal https://github.com/hengli/camodocal. 32 | // 33 | // Z. Zhang 34 | // A Flexible New Technique for Camera Calibration, 35 | // Extraction, PAMI 2000 36 | // Intrinsics estimation with image of absolute conic; 37 | bool initFocalLengthAbsoluteConic( 38 | const std::vector& observations, 39 | Eigen::VectorXd* intrinsics); 40 | 41 | } // namespace calibration 42 | } // namespace aslam 43 | 44 | #include "internal/focallength-initializers-traits-inl.h" 45 | 46 | #endif // ASLAM_CALIBRATION_CAMERA_INITIALIZER_H 47 | -------------------------------------------------------------------------------- /aslam_cv_detector/src/line-segment-detector.cc: -------------------------------------------------------------------------------- 1 | #include "aslam/detectors/line-segment-detector.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace aslam { 11 | 12 | LineSegmentDetector::LineSegmentDetector(const Options& options) 13 | : options_(options) { 14 | line_detector_ = aslamcv::createLineSegmentDetectorPtr( 15 | cv::LSD_REFINE_STD, 0.8, 0.6, 2.0, 16.5, 0.0, 0.65, 1024); 16 | } 17 | 18 | LineSegmentDetector::~LineSegmentDetector() {} 19 | 20 | void LineSegmentDetector::detect(const cv::Mat& image, Lines* lines) { 21 | CHECK_NOTNULL(lines)->clear(); 22 | CHECK(!line_detector_.empty()); 23 | 24 | std::vector raw_lines; 25 | line_detector_->detect(image, raw_lines); 26 | line_detector_->filterSize(raw_lines, raw_lines, options_.min_segment_length_px); 27 | 28 | lines->reserve(raw_lines.size()); 29 | for (size_t line_idx = 0u; line_idx < raw_lines.size(); ++line_idx) { 30 | lines->emplace_back(static_cast(raw_lines[line_idx](0)), 31 | static_cast(raw_lines[line_idx](1)), 32 | static_cast(raw_lines[line_idx](2)), 33 | static_cast(raw_lines[line_idx](3))); 34 | } 35 | } 36 | 37 | void LineSegmentDetector::drawLines(const Lines& lines, cv::Mat* image) { 38 | CHECK_NOTNULL(image); 39 | CHECK_EQ(image->channels(), 3) << "Color image required."; 40 | 41 | cv::RNG rng(12345); 42 | for (size_t idx = 0u; idx < lines.size(); ++idx) { 43 | cv::Point2d start_point(lines[idx].start_point(0), lines[idx].start_point(1)); 44 | cv::Point2d end_point(lines[idx].end_point(0), lines[idx].end_point(1)); 45 | 46 | cv::Scalar color(rng.uniform(0,255), rng.uniform(0, 255), rng.uniform(0, 255)); 47 | cv::line(*image, start_point, end_point, color, 2, CV_AA); 48 | } 49 | } 50 | 51 | } // namespace aslam 52 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/reader-writer-lock.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_COMMON_READER_WRITER_LOCK_H_ 2 | #define ASLAM_COMMON_READER_WRITER_LOCK_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | // Adapted from http://www.paulbridger.com/read_write_lock/ 10 | namespace aslam { 11 | class ReaderWriterMutex { 12 | public: 13 | ReaderWriterMutex(); 14 | virtual ~ReaderWriterMutex(); 15 | 16 | virtual void acquireReadLock(); 17 | void releaseReadLock(); 18 | 19 | virtual void acquireWriteLock(); 20 | virtual void releaseWriteLock(); 21 | 22 | // Attempt upgrade. If upgrade fails, relinquish read lock. 23 | virtual bool upgradeToWriteLock(); 24 | 25 | // Returns true if there are any active readers, writers, or threads that are waiting for read or 26 | // write access. 27 | bool isInUse(); 28 | 29 | protected: 30 | ReaderWriterMutex(const ReaderWriterMutex&) = delete; 31 | ReaderWriterMutex& operator=(const ReaderWriterMutex&) = delete; 32 | 33 | std::mutex mutex_; 34 | unsigned int pending_readers_; 35 | unsigned int num_readers_; 36 | std::condition_variable cv_readers_; 37 | unsigned int num_pending_writers_; 38 | bool current_writer_; 39 | std::condition_variable cv_writer_finished_; 40 | bool pending_upgrade_; 41 | }; 42 | 43 | class ScopedReadLock { 44 | public: 45 | explicit ScopedReadLock(ReaderWriterMutex* rw_lock); 46 | ScopedReadLock(ScopedReadLock&& other); 47 | ~ScopedReadLock(); 48 | 49 | private: 50 | ScopedReadLock(const ScopedReadLock&) = delete; 51 | ScopedReadLock& operator=(const ScopedReadLock&) = delete; 52 | ReaderWriterMutex* rw_lock_; 53 | }; 54 | 55 | class ScopedWriteLock { 56 | public: 57 | explicit ScopedWriteLock(ReaderWriterMutex* rw_lock); 58 | ScopedWriteLock(ScopedWriteLock&& other); 59 | ~ScopedWriteLock(); 60 | 61 | private: 62 | ScopedWriteLock(const ScopedWriteLock&) = delete; 63 | ScopedWriteLock& operator=(const ScopedWriteLock&) = delete; 64 | ReaderWriterMutex* rw_lock_; 65 | }; 66 | 67 | } // namespace aslam 68 | #endif // ASLAM_COMMON_READER_WRITER_LOCK_H_ 69 | -------------------------------------------------------------------------------- /aslam_cv_pipeline/src/visual-pipeline.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace aslam { 10 | 11 | VisualPipeline::VisualPipeline(const Camera::ConstPtr& input_camera, 12 | const Camera::ConstPtr& output_camera, bool copy_images) 13 | : input_camera_(input_camera), output_camera_(output_camera), 14 | copy_images_(copy_images) { 15 | CHECK(input_camera); 16 | CHECK(output_camera); 17 | } 18 | 19 | 20 | VisualPipeline::VisualPipeline(std::unique_ptr& preprocessing, bool copy_images) 21 | : preprocessing_(std::move(preprocessing)), 22 | copy_images_(copy_images) { 23 | CHECK_NOTNULL(preprocessing_.get()); 24 | input_camera_ = preprocessing_->getInputCameraShared(); 25 | output_camera_ = preprocessing_->getOutputCameraShared(); 26 | } 27 | 28 | std::shared_ptr VisualPipeline::processImage(const cv::Mat& raw_image, 29 | int64_t timestamp) const { 30 | CHECK_EQ(input_camera_->imageWidth(), static_cast(raw_image.cols)); 31 | CHECK_EQ(input_camera_->imageHeight(), static_cast(raw_image.rows)); 32 | 33 | // \TODO(PTF) Eventually we can put timestamp correction policies in here. 34 | std::shared_ptr frame(new VisualFrame); 35 | frame->setTimestampNanoseconds(timestamp); 36 | frame->setRawCameraGeometry(input_camera_); 37 | frame->setCameraGeometry(output_camera_); 38 | FrameId id; 39 | generateId(&id); 40 | frame->setId(id); 41 | if(copy_images_) { 42 | frame->setRawImage(raw_image.clone()); 43 | } else { 44 | frame->setRawImage(raw_image); 45 | } 46 | 47 | cv::Mat image; 48 | if(preprocessing_) { 49 | preprocessing_->processImage(raw_image, &image); 50 | } else { 51 | image = raw_image; 52 | } 53 | /// Send the image to the derived class for processing 54 | processFrameImpl(image, frame.get()); 55 | 56 | return frame; 57 | } 58 | 59 | } // namespace aslam 60 | -------------------------------------------------------------------------------- /aslam_cv_calibration/src/target-algorithms.cc: -------------------------------------------------------------------------------- 1 | #include "aslam/calibration/target-algorithms.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace aslam { 7 | namespace calibration { 8 | 9 | bool estimateTargetTransformation( 10 | const TargetObservation& target_observation, 11 | const aslam::Camera::ConstPtr& camera_ptr, aslam::Transformation* T_G_C) { 12 | CHECK(camera_ptr); 13 | CHECK_NOTNULL(T_G_C); 14 | constexpr bool kRunNonlinearRefinement = true; 15 | constexpr double kRansacPixelSigma = 1.0; 16 | constexpr int kRansacMaxIters = 200; 17 | return estimateTargetTransformation( 18 | target_observation, camera_ptr, T_G_C, kRunNonlinearRefinement, 19 | kRansacPixelSigma, kRansacMaxIters); 20 | } 21 | 22 | bool estimateTargetTransformation( 23 | const TargetObservation& target_observation, 24 | const aslam::Camera::ConstPtr& camera_ptr, aslam::Transformation* T_G_C, 25 | const bool run_nonlinear_refinement, const double ransac_pixel_sigma, 26 | const int ransac_max_iters) { 27 | CHECK(camera_ptr); 28 | CHECK_GT(ransac_pixel_sigma, 0.0); 29 | CHECK_GT(ransac_max_iters, 0); 30 | CHECK_NOTNULL(T_G_C); 31 | const Eigen::Matrix2Xd& observed_corners = 32 | target_observation.getObservedCorners(); 33 | // Corner positions in target coordinates (global frame). 34 | const Eigen::Matrix3Xd corner_positions_G = 35 | target_observation.getCorrespondingTargetPoints(); 36 | aslam::geometric_vision::PnpPoseEstimator pnp(run_nonlinear_refinement); 37 | std::vector inliers; 38 | int num_iters = 0; 39 | bool pnp_success = pnp.absolutePoseRansacPinholeCam( 40 | observed_corners, corner_positions_G, ransac_pixel_sigma, 41 | ransac_max_iters, camera_ptr, T_G_C, &inliers, &num_iters); 42 | if (pnp_success) { 43 | VLOG(4) << "Found " << inliers.size() << "/" << observed_corners.cols() 44 | << " inliers in" << num_iters << " iterations."; 45 | } else { 46 | LOG(WARNING) << "Target transformation estimation failed."; 47 | } 48 | return pnp_success; 49 | } 50 | 51 | } // namespace calibration 52 | } // namespace aslam 53 | -------------------------------------------------------------------------------- /aslam_cv_common/include/aslam/common/channel-definitions.h: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_CV_COMMON_CHANNEL_DEFINITIONS_H_ 2 | #define ASLAM_CV_COMMON_CHANNEL_DEFINITIONS_H_ 3 | 4 | #include 5 | #include 6 | 7 | /// Coordinates of the raw keypoints. (keypoint detector output) 8 | /// (cols are keypoints) 9 | DECLARE_CHANNEL(VISUAL_KEYPOINT_MEASUREMENTS, Eigen::Matrix2Xd) 10 | 11 | /// Keypoint coordinate uncertainties of the raw keypoints. (keypoint detector output) 12 | /// (cols are uncertainties) 13 | DECLARE_CHANNEL(VISUAL_KEYPOINT_MEASUREMENT_UNCERTAINTIES, Eigen::VectorXd) 14 | 15 | /// Keypoint orientation from keypoint extractor. (keypoint detector output) 16 | /// Computed orientation of the keypoint (-1 if not applicable); 17 | /// it's in [0,360) degrees and measured relative to image coordinate system, ie in clockwise. 18 | DECLARE_CHANNEL(VISUAL_KEYPOINT_ORIENTATIONS, Eigen::VectorXd) 19 | 20 | /// Diameter of the meaningful keypoint neighborhood. (keypoint detector output) 21 | DECLARE_CHANNEL(VISUAL_KEYPOINT_SCALES, Eigen::VectorXd) 22 | 23 | /// The score by which the most strong keypoints have been selected. Can be used for the further 24 | /// sorting or subsampling. (keypoint detector output) 25 | DECLARE_CHANNEL(VISUAL_KEYPOINT_SCORES, Eigen::VectorXd) 26 | 27 | // 3D position of the 2D keypoint as provided by a depth sensor and also a time 28 | // offset for the point with respect to the time in the frame 29 | DECLARE_CHANNEL(VISUAL_KEYPOINT_3D_POSITIONS, Eigen::Matrix3Xd) 30 | DECLARE_CHANNEL(VISUAL_KEYPOINT_TIME_OFFSETS, Eigen::VectorXi) 31 | 32 | /// The keypoint descriptors. (extractor output) 33 | /// (cols are descriptors) 34 | DECLARE_CHANNEL(DESCRIPTORS, 35 | std::vector>) 36 | DECLARE_CHANNEL(DESCRIPTOR_TYPES, Eigen::VectorXi) 37 | 38 | /// Track IDs for tracked features. (-1 if not tracked); (feature tracker output) 39 | DECLARE_CHANNEL(TRACK_IDS, Eigen::VectorXi) 40 | 41 | /// The raw image. 42 | DECLARE_CHANNEL(RAW_IMAGE, cv::Mat) 43 | DECLARE_CHANNEL(CV_MAT, cv::Mat) 44 | 45 | #endif // ASLAM_CV_COMMON_CHANNEL_DEFINITIONS_H_ 46 | -------------------------------------------------------------------------------- /aslam_cv_cameras/include/aslam/cameras/distortion-fisheye-inl.h: -------------------------------------------------------------------------------- 1 | #ifndef FISHEYE_DISTORTION_INL_H_ 2 | #define FISHEYE_DISTORTION_INL_H_ 3 | 4 | #include 5 | 6 | namespace aslam { 7 | namespace jet_trigonometric { 8 | 9 | inline double tan(double x) { 10 | return std::tan(x); 11 | } 12 | 13 | inline double atan(double x) { 14 | return std::atan(x); 15 | } 16 | 17 | template