├── .gitattributes ├── src ├── view │ ├── gui │ │ ├── IFormsPayload.cpp │ │ ├── MoveFormsPayload.cpp │ │ ├── FormsPayload.cpp │ │ ├── IFormsPayload.h │ │ ├── CMakeLists.txt │ │ ├── FormsPayload.h │ │ ├── MoveFormsPayload.h │ │ └── SwagGUI.h │ ├── CMakeLists.txt │ └── Visualizer.h ├── controller │ ├── move │ │ ├── MoveMethod.h │ │ ├── CMakeLists.txt │ │ ├── MoveControllerGUI.h │ │ ├── MoveController.cpp │ │ ├── MoveControllerGUI.cpp │ │ └── MoveController.h │ ├── IController.cpp │ ├── IControllerGUI.cpp │ ├── home │ │ ├── CMakeLists.txt │ │ ├── HomeController.cpp │ │ └── HomeController.h │ ├── edit │ │ ├── CMakeLists.txt │ │ ├── EditingControllerGUI.cpp │ │ └── EditingControllerGUI.h │ ├── scan │ │ ├── CMakeLists.txt │ │ ├── ScanControllerGUI.h │ │ ├── ScanController.h │ │ ├── ScanController.cpp │ │ └── ScanControllerGUI.cpp │ ├── manager │ │ ├── CMakeLists.txt │ │ ├── ControllerManager.h │ │ ├── ControllerManager.cpp │ │ └── ControllerManagerCache.h │ ├── processing │ │ ├── CMakeLists.txt │ │ ├── ProcessingControllerGUI.h │ │ ├── ProcessingController.cpp │ │ ├── ProcessingControllerGUI.cpp │ │ └── ProcessingController.h │ ├── calibration │ │ ├── CMakeLists.txt │ │ ├── CalibrationControllerGUI.h │ │ ├── CalibrationControllerGUI.cpp │ │ ├── CalibrationController.h │ │ └── CalibrationController.cpp │ ├── CMakeLists.txt │ ├── IController.h │ └── IControllerGUI.h ├── cli │ ├── CMakeLists.txt │ ├── CLIParser.h │ └── CLIParser.cpp ├── model │ ├── arduino │ │ ├── CMakeLists.txt │ │ ├── Arduino.h │ │ └── Arduino.cpp │ ├── scan │ │ ├── CMakeLists.txt │ │ ├── ScanModel.cpp │ │ └── ScanModel.h │ ├── calibration │ │ ├── CMakeLists.txt │ │ └── CalibrationModel.h │ ├── processing │ │ ├── CMakeLists.txt │ │ ├── ProcessingModel.h │ │ └── ProcessingModel.cpp │ ├── camera │ │ ├── CMakeLists.txt │ │ ├── ICamera.h │ │ └── SR305.h │ └── CMakeLists.txt ├── types │ ├── CMakeLists.txt │ ├── equations │ │ ├── Point.cpp │ │ ├── CMakeLists.txt │ │ ├── Point.h │ │ ├── Normal.cpp │ │ ├── Plane.cpp │ │ ├── Equations.h │ │ ├── Normal.h │ │ └── Plane.h │ ├── CameraTypes.cpp │ ├── CameraTypes.h │ └── CloudType.h ├── utils │ ├── CMakeLists.txt │ ├── Constants.h │ ├── Logger.h │ ├── Algorithms.h │ └── Algorithms.cpp ├── file │ ├── CMakeLists.txt │ ├── CalibrationFileHandler.h │ ├── ScanFileHandler.h │ ├── CalibrationFileHandler.cpp │ └── IFileHandler.h ├── main.cpp └── CMakeLists.txt ├── doc └── img │ ├── GUI.png │ ├── sponge.png │ └── swaggg.png ├── test ├── processingTests │ ├── visual │ │ └── CMakeLists.txt │ ├── CMakeLists.txt │ └── README.md ├── research │ ├── calibration │ │ ├── data │ │ │ ├── 0.pcd │ │ │ └── 20.pcd │ │ ├── CMakeLists.txt │ │ └── Calibration.cpp │ ├── registration │ │ ├── data │ │ │ ├── 0.pcd │ │ │ ├── 100.pcd │ │ │ ├── 120.pcd │ │ │ ├── 140.pcd │ │ │ ├── 160.pcd │ │ │ ├── 180.pcd │ │ │ ├── 20.pcd │ │ │ ├── 200.pcd │ │ │ ├── 220.pcd │ │ │ ├── 240.pcd │ │ │ ├── 260.pcd │ │ │ ├── 280.pcd │ │ │ ├── 300.pcd │ │ │ ├── 320.pcd │ │ │ ├── 340.pcd │ │ │ ├── 40.pcd │ │ │ ├── 60.pcd │ │ │ └── 80.pcd │ │ └── CMakeLists.txt │ ├── segmentation │ │ ├── data │ │ │ ├── 0.pcd │ │ │ └── 20.pcd │ │ ├── CMakeLists.txt │ │ └── Segmentation.cpp │ ├── depthFiltering │ │ ├── data │ │ │ ├── sponge.pcd │ │ │ ├── fixture_1.pcd │ │ │ ├── fixture_2.pcd │ │ │ ├── fixture_3.pcd │ │ │ ├── fixture_raw.pcd │ │ │ ├── raw_cloud.pcd │ │ │ └── calibration_cloud.pcd │ │ ├── README.md │ │ ├── CMakeLists.txt │ │ └── CompareDepthFiltering.cpp │ ├── README.md │ └── CMakeLists.txt ├── utilsTests │ ├── CMakeLists.txt │ ├── README.md │ └── AlgosTests.cpp ├── calibrationTests │ ├── visual │ │ ├── CMakeLists.txt │ │ └── CalibrationTestsVisual.cpp │ ├── CMakeLists.txt │ ├── README.md │ └── CalibrationTests.cpp ├── equationsTests │ ├── EquationsTests.cpp │ ├── README.md │ ├── CMakeLists.txt │ ├── NormalTests.cpp │ └── PlaneTests.cpp ├── typesTests │ ├── README.md │ ├── CMakeLists.txt │ ├── CloudTypeTests.cpp │ └── CameraTypesTests.cpp ├── cameraTests │ ├── CMakeLists.txt │ └── CameraTests.cpp ├── fileTests │ ├── CMakeLists.txt │ ├── README.md │ ├── ScanFileHandlerTests.cpp │ └── ScanFileHandlerPhysicalTests.cpp ├── README.md ├── main.cpp └── CMakeLists.txt ├── .gitmodules ├── license ├── CMakeLists.txt ├── .gitignore ├── .travis.yml └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | *.pcd filter=lfs diff=lfs merge=lfs -text 2 | *.png filter=lfs diff=lfs merge=lfs -text 3 | -------------------------------------------------------------------------------- /src/view/gui/IFormsPayload.cpp: -------------------------------------------------------------------------------- 1 | #include "IFormsPayload.h" 2 | 3 | IFormsPayload::IFormsPayload() {} 4 | -------------------------------------------------------------------------------- /doc/img/GUI.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3013c65fab71aa09051378ab50c7058162dc0a2bf413778a25d83758d98f80a8 3 | size 803005 4 | -------------------------------------------------------------------------------- /doc/img/sponge.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:6351172773eebd12f021bf5ec5c1d40360c84fba4bdc86992f5472828d54e632 3 | size 1562487 4 | -------------------------------------------------------------------------------- /doc/img/swaggg.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e1464810f546943c8af9ed258377a5623cb21cf198eb256c73fb937f65a0682e 3 | size 24805 4 | -------------------------------------------------------------------------------- /test/processingTests/visual/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(${TEST_MAIN} PRIVATE 2 | ) 3 | 4 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/research/calibration/data/0.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:990adec8a548044b78547959a0f996300c6fae5aab4e3d7ab67f8f26a316cfe7 3 | size 3968886 4 | -------------------------------------------------------------------------------- /test/research/calibration/data/20.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:8ebf18285389a2ea5bd32f111e73abe53c22d133d68ecd6687a8b97614395690 3 | size 3971839 4 | -------------------------------------------------------------------------------- /test/research/registration/data/0.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:5b559a92f453e0b5f717a7b400d512940ab2ecf62a6e0ba3181936dd156eb162 3 | size 4672551 4 | -------------------------------------------------------------------------------- /test/research/registration/data/100.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3d5dde4297bca04002def859c321c8b9b6ec7aed747ee5b322eaa62a024d2dda 3 | size 4550153 4 | -------------------------------------------------------------------------------- /test/research/registration/data/120.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:cde950eaefe484b29c2a8c327df9a79a39e8263597020ccc2d5796758f1408ee 3 | size 4569082 4 | -------------------------------------------------------------------------------- /test/research/registration/data/140.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:9c4ebe4e9b1924266cc3fddd66841d03e12ab4aa7a5dab76595e637ee337316f 3 | size 4594307 4 | -------------------------------------------------------------------------------- /test/research/registration/data/160.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:03f13163b8be39b1efbfd96f2f6573b21bcf4bd03632a54e8618f0a24e156581 3 | size 4601375 4 | -------------------------------------------------------------------------------- /test/research/registration/data/180.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:598cd2cecd69ed112213c403e80f0b93c83a6b6c0db20c3bb6fc7c940dc57499 3 | size 4601134 4 | -------------------------------------------------------------------------------- /test/research/registration/data/20.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:118f8b7cca1eda2f64386bad3d89c6b97063981b45c5380a6c03f60461cb053a 3 | size 4546526 4 | -------------------------------------------------------------------------------- /test/research/registration/data/200.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:69cd8f19047a3489fb09661e808bde4c793c56d1a0ff780a73932cf9deb484c3 3 | size 4603874 4 | -------------------------------------------------------------------------------- /test/research/registration/data/220.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:35f88dccdc3766a4354ce9a3dc16157f947c97424113cb1afbabafccc1d9023c 3 | size 4591681 4 | -------------------------------------------------------------------------------- /test/research/registration/data/240.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:482c1c9d214d568452370c9d866536dcbb08b63b6064dd2a8733136afb2e8acd 3 | size 4564493 4 | -------------------------------------------------------------------------------- /test/research/registration/data/260.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:84662993527403ea0f2b34fc14e2512a8b89b0b944b226aca63e6dca9f9d6f6c 3 | size 4552037 4 | -------------------------------------------------------------------------------- /test/research/registration/data/280.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:278c6a59c8471718ed725759f4f3bc2e3a85b9e4595d64d21fb112f896f2b5c2 3 | size 4542198 4 | -------------------------------------------------------------------------------- /test/research/registration/data/300.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2e93e0ab12f779875750b446adac7b440402162106cc3b8c5c30ed9583f62a27 3 | size 4549299 4 | -------------------------------------------------------------------------------- /test/research/registration/data/320.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:dec10e6fa89c869000bd13405e9298c92a38bb25d924d2f70acb07b40178d2e8 3 | size 4554239 4 | -------------------------------------------------------------------------------- /test/research/registration/data/340.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:050761c19f3e6c7aa184c1dcd3eedc9dcefa31c648dd2d3135404bf2dece6b27 3 | size 4553117 4 | -------------------------------------------------------------------------------- /test/research/registration/data/40.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:650b1389456972f92da9a7165ad3c53107b776a525d3f19e102c0eb660c6bb87 3 | size 4553844 4 | -------------------------------------------------------------------------------- /test/research/registration/data/60.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3def29dfd6459bee2a5ed579fba102d52fef05c447a32210250e6459507d1bcc 3 | size 4551215 4 | -------------------------------------------------------------------------------- /test/research/registration/data/80.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3caed5a2eebd860c76195aec90643a0f1d2ce90d5fefc734422385ce189ac309 3 | size 4551352 4 | -------------------------------------------------------------------------------- /test/research/segmentation/data/0.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:20121828cb15d18f58f777a302479a3abd769d503fb231dd413eed5fe104618e 3 | size 1507284 4 | -------------------------------------------------------------------------------- /test/research/segmentation/data/20.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:83040436fa0c9c22f7b54919912115049b774fedc62894b6664a715a930d3da9 3 | size 1459248 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/sponge.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:bf2bfa5d042583d2ab0e74df411fd43e677a9d4559fb8c27e1974eefa79e3148 3 | size 6475367 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/fixture_1.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:6814bd1a64ec0315166a040a43267d683dbb9de773aba7a5caa72fedcf6221ad 3 | size 1569965 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/fixture_2.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:05742f68575a852a43d6f9cc8877f2cb6ca92e40a3963622b073696226be6a3a 3 | size 1610034 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/fixture_3.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b97787023fd2b5358f6efacb7ec35ec7d7df906dc72d2a5b61a42b221efea9d4 3 | size 1599671 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/fixture_raw.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:058fbf62119d222cf7975874a8da17a6ae4b8e51831746bd15a3db26680ebdcc 3 | size 6182153 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/raw_cloud.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:f7d13954c3b03efde9b53b1fd699ca0430ed7f969089accf23b29f1373848fed 3 | size 7267623 4 | -------------------------------------------------------------------------------- /test/research/depthFiltering/data/calibration_cloud.pcd: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:811151d627f3f17d51228196f85aa462ea6c56ac7aea712913104e2190c35f9e 3 | size 1224212 4 | -------------------------------------------------------------------------------- /src/view/gui/MoveFormsPayload.cpp: -------------------------------------------------------------------------------- 1 | #include "MoveFormsPayload.h" 2 | 3 | MoveFormsPayload::MoveFormsPayload(const MoveMethod &move_method, int deg) : 4 | move_method(move_method), deg(deg) {} 5 | 6 | -------------------------------------------------------------------------------- /test/utilsTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(${TEST_MAIN} PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/AlgosTests.cpp 3 | ) 4 | 5 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/move/MoveMethod.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_MOVEMETHOD_H 2 | #define SWAG_SCANNER_MOVEMETHOD_H 3 | 4 | enum class MoveMethod { 5 | TO, 6 | BY 7 | }; 8 | 9 | #endif //SWAG_SCANNER_MOVEMETHOD_H 10 | -------------------------------------------------------------------------------- /test/utilsTests/README.md: -------------------------------------------------------------------------------- 1 | # utilsTests 2 | 3 | This folder contains tests for verifying different utility classes. 4 | 5 | * [AlgosTests.cpp](./AlgosTests.cpp) : Verifies mathematical and functional accuracy of handmade algorithms 6 | -------------------------------------------------------------------------------- /src/controller/IController.cpp: -------------------------------------------------------------------------------- 1 | #include "IController.h" 2 | #include 3 | 4 | void controller::IController::run() { 5 | // do nothing 6 | } 7 | 8 | controller::IController::~IController() { 9 | // do nothing 10 | } 11 | -------------------------------------------------------------------------------- /src/view/gui/FormsPayload.cpp: -------------------------------------------------------------------------------- 1 | #include "FormsPayload.h" 2 | 3 | FormsPayload::FormsPayload(const QString &name, int angle, int rotations) : 4 | angle(angle), rotations(rotations) { 5 | this->name = name.toUtf8().constData(); 6 | } 7 | -------------------------------------------------------------------------------- /test/calibrationTests/visual/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(${TEST_MAIN} PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationTestsVisual.cpp 3 | ) 4 | 5 | target_include_directories(${TEST_MAIN} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/processingTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if (BUILD_TESTS_VISUAL) 2 | add_subdirectory(visual) 3 | endif() 4 | 5 | target_sources(${TEST_MAIN} PRIVATE 6 | ) 7 | 8 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/research/README.md: -------------------------------------------------------------------------------- 1 | # research 2 | 3 | This folder contains tests and experiments for improving scan quality. 4 | 5 | note: because this folder is visualization heavy, ```BUILD_TESTS_VISUAL``` must be set to ```ON``` 6 | to build these tests. 7 | 8 | -------------------------------------------------------------------------------- /test/equationsTests/EquationsTests.cpp: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_EQUATIONSTESTS_CPP 2 | #define SWAG_SCANNER_EQUATIONSTESTS_CPP 3 | 4 | #include 5 | #include 6 | #include "Equations.h" 7 | 8 | 9 | #endif //SWAG_SCANNER_EQUATIONSTESTS_CPP -------------------------------------------------------------------------------- /test/typesTests/README.md: -------------------------------------------------------------------------------- 1 | # typesTests 2 | 3 | This folder contains tests for verifying created types. 4 | 5 | * [CameraTypesTests.cpp](./CameraTypesTests.cpp) : Verifies CameraTypes 6 | * [CameraTypesTests.cpp](./CameraTypesTests.cpp) : Verifies CloudTypes 7 | -------------------------------------------------------------------------------- /src/cli/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/CLIParser.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/CLIParser.h 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/model/arduino/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/Arduino.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/Arduino.h 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/model/scan/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanModel.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanModel.h 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/equationsTests/README.md: -------------------------------------------------------------------------------- 1 | # equationsTests 2 | 3 | This folder contains tests for verifying custom equations:: types. 4 | 5 | * [EquationsTests.cpp](./EquationsTests.cpp) : 6 | * [NormalTests.cpp](./NormalTests.cpp) : 7 | * [PlaneTests.cpp](./PlaneTests.cpp) : 8 | -------------------------------------------------------------------------------- /src/controller/IControllerGUI.cpp: -------------------------------------------------------------------------------- 1 | #include "IControllerGUI.h" 2 | #include "SwagGUI.h" 3 | #include 4 | 5 | controller::IControllerGUI::IControllerGUI(std::shared_ptr gui) : 6 | gui(std::move(gui)) { 7 | qRegisterMetaType(); 8 | } 9 | -------------------------------------------------------------------------------- /test/typesTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(${TEST_MAIN} PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/CameraTypesTests.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/CloudTypeTests.cpp 4 | ) 5 | 6 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/home/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/HomeController.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/HomeController.h 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/view/gui/IFormsPayload.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_IFORMSPAYLOAD_H 2 | #define SWAG_SCANNER_IFORMSPAYLOAD_H 3 | 4 | class IFormsPayload { 5 | public: 6 | IFormsPayload(); 7 | 8 | virtual ~IFormsPayload() = default; 9 | }; 10 | 11 | #endif //SWAG_SCANNER_IFORMSPAYLOAD_H 12 | -------------------------------------------------------------------------------- /src/model/calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationModel.h 3 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationModel.cpp 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/model/processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ProcessingModel.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/ProcessingModel.h 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/cameraTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #if (BUILD_TESTS_VISUAL) 2 | # add_subdirectory(visual) 3 | #endif() 4 | 5 | target_sources(${TEST_MAIN} PRIVATE 6 | ${CMAKE_CURRENT_SOURCE_DIR}/CameraTests.cpp 7 | ) 8 | 9 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/fileTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(${TEST_MAIN} PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanFileHandlerPhysicalTests.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanFileHandlerTests.cpp 4 | ) 5 | 6 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/edit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/EditingControllerGUI.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/EditingControllerGUI.h 4 | ) 5 | 6 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/view/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(gui) 2 | 3 | target_sources(swag_scanner_lib PRIVATE 4 | ${CMAKE_CURRENT_SOURCE_DIR}/Visualizer.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/Visualizer.h 6 | ) 7 | 8 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/calibrationTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if (BUILD_TESTS_VISUAL) 2 | add_subdirectory(visual) 3 | endif() 4 | 5 | target_sources(${TEST_MAIN} PRIVATE 6 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationTests.cpp 7 | ) 8 | 9 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/model/camera/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ICamera.h 3 | ${CMAKE_CURRENT_SOURCE_DIR}/SR305.cpp 4 | ${CMAKE_CURRENT_SOURCE_DIR}/SR305.h 5 | ) 6 | 7 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/research/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(depthFiltering) 2 | add_subdirectory(registration) 3 | add_subdirectory(segmentation) 4 | add_subdirectory(calibration) 5 | 6 | target_sources(${TEST_MAIN} PRIVATE 7 | 8 | ) 9 | 10 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/equationsTests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(${TEST_MAIN} PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/EquationsTests.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/NormalTests.cpp 4 | ${CMAKE_CURRENT_SOURCE_DIR}/PlaneTests.cpp 5 | ) 6 | 7 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(equations) 2 | 3 | target_sources(swag_scanner_lib PRIVATE 4 | ${CMAKE_CURRENT_SOURCE_DIR}/CameraTypes.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/CameraTypes.h 6 | ${CMAKE_CURRENT_SOURCE_DIR}/CloudType.h 7 | ) 8 | 9 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/Algorithms.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/Algorithms.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/Constants.h 5 | ${CMAKE_CURRENT_SOURCE_DIR}/Logger.h 6 | ) 7 | 8 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | # Tests 2 | 3 | SwagScanner has a wide suite of testing to verify math, behavior, and control flow of its 4 | functionalities. 5 | 6 | * the fun stuff happens in the [research](./research) folder 7 | 8 | Key words: 9 | * physical - means the tests are being run against actual files 10 | * visual - means the tests will produce a visual result 11 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "extern/googletest"] 2 | path = extern/googletest 3 | url = https://github.com/google/googletest.git 4 | [submodule "extern/json"] 5 | path = extern/json 6 | url = https://github.com/nlohmann/json 7 | [submodule "extern/SwagScannerWidgets"] 8 | path = extern/SwagScannerWidgets 9 | url = https://github.com/seanngpack/qt-collapsible-section 10 | -------------------------------------------------------------------------------- /src/model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(arduino) 2 | add_subdirectory(calibration) 3 | add_subdirectory(camera) 4 | add_subdirectory(processing) 5 | add_subdirectory(scan) 6 | 7 | 8 | target_sources(swag_scanner_lib PRIVATE 9 | ${CMAKE_CURRENT_SOURCE_DIR}/IModel.h 10 | ) 11 | 12 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/calibrationTests/README.md: -------------------------------------------------------------------------------- 1 | # calibrationTests 2 | 3 | This folder contains tests for verifying mathematical and visual accuracy 4 | of the calibration algorithm. 5 | 6 | * [CalibrationTests.cpp](./CalibrationTests.cpp) : Verifies building the calibration matrices and center point calculation 7 | * [CalibrationTestsVisual.cpp](visual/CalibrationTestsVisual.cpp) : Visual tests 8 | -------------------------------------------------------------------------------- /src/controller/home/HomeController.cpp: -------------------------------------------------------------------------------- 1 | #include "IFileHandler.h" 2 | #include "HomeController.h" 3 | 4 | using json = nlohmann::json; 5 | 6 | void controller::HomeController::run() { 7 | json settings_json = file::IFileHandler::load_swag_scanner_info_json(); 8 | settings_json["current_position"] = 0; 9 | file::IFileHandler::write_swag_scanner_info_json(settings_json); 10 | } 11 | -------------------------------------------------------------------------------- /src/types/equations/Point.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Point.h" 3 | 4 | equations::Point::Point(double x, double y, double z) : x(x), y(y), z(z) {} 5 | 6 | equations::Point::Point(const std::vector &in) : x(in[0]), y(in[1]), z(in[2]) { 7 | if (in.size() < 3) { 8 | throw std::invalid_argument("Cannot construct a point with fewer than three values"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /src/controller/scan/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanController.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanController.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanControllerGUI.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanControllerGUI.h 6 | ) 7 | 8 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Logger.h" 2 | #include 3 | #include 4 | 5 | // Run all the tests that were declared with TEST() 6 | int main(int argc, char **argv){ 7 | auto default_logger = logger::setup_default_logger(); 8 | spdlog::set_level(spdlog::level::level_enum::debug); 9 | testing::InitGoogleTest(&argc, argv); 10 | return RUN_ALL_TESTS(); 11 | } 12 | -------------------------------------------------------------------------------- /src/controller/manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ControllerManager.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/ControllerManager.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/ControllerManagerCache.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/ControllerManagerCache.h 6 | ) 7 | 8 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 9 | -------------------------------------------------------------------------------- /test/fileTests/README.md: -------------------------------------------------------------------------------- 1 | # fileTests 2 | 3 | This folder contains tests for verifying file handler behavior. Most of these tests 4 | depend on having infrastructure on your computer set up. 5 | 6 | * [ScanFileHandlerPhysicalTests.cpp](./ScanFileHandlerPhysicalTests.cpp) : Verifies Scan file handler constructors 7 | * [ScanFileHandlerTests.cpp](./ScanFileHandlerTests.cpp) : Verifies the handler creates folders and files correctly 8 | -------------------------------------------------------------------------------- /src/controller/processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/ProcessingController.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/ProcessingController.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/ProcessingControllerGUI.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/ProcessingControllerGUI.h 6 | ) 7 | 8 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationController.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationController.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationControllerGUI.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationControllerGUI.h 6 | ) 7 | 8 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/move/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveController.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveController.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveControllerGUI.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveControllerGUI.h 6 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveMethod.h 7 | ) 8 | 9 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/file/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationFileHandler.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/CalibrationFileHandler.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/IFileHandler.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/IFileHandler.h 6 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanFileHandler.cpp 7 | ${CMAKE_CURRENT_SOURCE_DIR}/ScanFileHandler.h 8 | ) 9 | 10 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/types/equations/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/Equations.h 3 | ${CMAKE_CURRENT_SOURCE_DIR}/Normal.cpp 4 | ${CMAKE_CURRENT_SOURCE_DIR}/Normal.h 5 | ${CMAKE_CURRENT_SOURCE_DIR}/Plane.cpp 6 | ${CMAKE_CURRENT_SOURCE_DIR}/Plane.h 7 | ${CMAKE_CURRENT_SOURCE_DIR}/Point.cpp 8 | ${CMAKE_CURRENT_SOURCE_DIR}/Point.h 9 | ) 10 | 11 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/types/equations/Point.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_POINT_H 2 | #define SWAG_SCANNER_POINT_H 3 | 4 | #include 5 | 6 | namespace equations { 7 | 8 | /** 9 | * Simple object representing a point in 3d space. 10 | */ 11 | typedef struct Point { 12 | public: 13 | double x,y,z; 14 | 15 | Point(double x, double y, double z); 16 | 17 | Point(const std::vector &in); 18 | 19 | ~Point() = default; 20 | } Point; 21 | } 22 | 23 | #endif //SWAG_SCANNER_POINT_H 24 | -------------------------------------------------------------------------------- /src/controller/home/HomeController.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_HOMECONTROLLER_H 2 | #define SWAG_SCANNER_HOMECONTROLLER_H 3 | 4 | #include "IController.h" 5 | 6 | namespace controller { 7 | 8 | /** 9 | * This controller handles homing of the scanner. 10 | */ 11 | class HomeController : public IController { 12 | public: 13 | HomeController() = default; 14 | 15 | /** 16 | * Go to home position. 17 | */ 18 | void run() override; 19 | }; 20 | } 21 | 22 | #endif //SWAG_SCANNER_HOMECONTROLLER_H 23 | -------------------------------------------------------------------------------- /test/research/depthFiltering/README.md: -------------------------------------------------------------------------------- 1 | # Results from CompareDepthFiltering.cpp 2 | 3 | In this experiment, I aimed to compare realsense's ```spatial edge-preserving filter``` against 4 | PCL's ```FastBilateralFilter```. Tuning different parameters and treating the clouds 5 | through decimation filters, realsense's implementation clearly shows better results than PCL's implementation. 6 | 7 | So far, I have found that minimal bilateral filtering leads to best plane detection results, 8 | achieving only .179% error on this dataset. 9 | 10 | ![section opening and closing](example.gif) -------------------------------------------------------------------------------- /src/controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(calibration) 2 | add_subdirectory(edit) 3 | add_subdirectory(manager) 4 | add_subdirectory(home) 5 | add_subdirectory(move) 6 | add_subdirectory(processing) 7 | add_subdirectory(scan) 8 | 9 | target_sources(swag_scanner_lib PRIVATE 10 | ${CMAKE_CURRENT_SOURCE_DIR}/IController.h 11 | ${CMAKE_CURRENT_SOURCE_DIR}/IController.cpp 12 | ${CMAKE_CURRENT_SOURCE_DIR}/IControllerGUI.h 13 | ${CMAKE_CURRENT_SOURCE_DIR}/IControllerGUI.cpp 14 | ) 15 | 16 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/typesTests/CloudTypeTests.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "../../src/types/CloudType.h" 3 | 4 | 5 | TEST(CloudTypeTests, TestEnumMap) { 6 | ASSERT_EQ(CloudType::String(CloudType::Type::RAW), "raw"); 7 | } 8 | 9 | TEST(CloudTypeTests, TestEnumForLoop) { 10 | std::vector v; 11 | for ( const auto e : CloudType::All ) 12 | v.push_back(CloudType::String(e)); 13 | 14 | ASSERT_EQ(v[0], "raw"); 15 | ASSERT_EQ(v[1], "filtered"); 16 | ASSERT_EQ(v[2], "processed"); 17 | ASSERT_EQ(v[3], "normal"); 18 | ASSERT_EQ(v[4], "calibration"); 19 | 20 | } -------------------------------------------------------------------------------- /src/cli/CLIParser.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CLIPARSER_H 2 | #define SWAG_SCANNER_CLIPARSER_H 3 | 4 | 5 | #include 6 | 7 | namespace cli { 8 | /** 9 | * This class keeps track of shared resources and uses the manager to create 10 | * new controllers; 11 | */ 12 | class CLIParser { 13 | public: 14 | 15 | CLIParser(); 16 | 17 | boost::program_options::variables_map get_variables_map(int argc, char *argv[]); 18 | 19 | 20 | private: 21 | boost::program_options::options_description desc; 22 | 23 | 24 | 25 | 26 | 27 | }; 28 | 29 | }; 30 | 31 | #endif //SWAG_SCANNER_CLIPARSER_H 32 | -------------------------------------------------------------------------------- /src/types/equations/Normal.cpp: -------------------------------------------------------------------------------- 1 | #include "Normal.h" 2 | 3 | equations::Normal::Normal(double A, double B, double C) : A(A), B(B), C(C) {} 4 | 5 | equations::Normal::Normal(const std::vector &in) : A(in[0]), B(in[1]), C(in[2]) { 6 | if (in.size() < 3) { 7 | throw std::invalid_argument("cannot create normal, input vector size too small"); 8 | } 9 | } 10 | 11 | equations::Normal::Normal(const pcl::ModelCoefficients &in) : A(in.values[0]), B(in.values[1]), C(in.values[2]) {} 12 | 13 | equations::Normal equations::Normal::operator+(const equations::Normal &n2) const { 14 | return equations::Normal(A + n2.A, B + n2.B, C + n2.C); 15 | } 16 | 17 | -------------------------------------------------------------------------------- /src/view/gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources(swag_scanner_lib PRIVATE 2 | ${CMAKE_CURRENT_SOURCE_DIR}/SwagGUI.cpp 3 | ${CMAKE_CURRENT_SOURCE_DIR}/SwagGUI.h 4 | ${CMAKE_CURRENT_SOURCE_DIR}/IFormsPayload.cpp 5 | ${CMAKE_CURRENT_SOURCE_DIR}/IFormsPayload.h 6 | ${CMAKE_CURRENT_SOURCE_DIR}/FormsPayload.cpp 7 | ${CMAKE_CURRENT_SOURCE_DIR}/FormsPayload.h 8 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveFormsPayload.cpp 9 | ${CMAKE_CURRENT_SOURCE_DIR}/MoveFormsPayload.h 10 | ${CMAKE_CURRENT_SOURCE_DIR}/swagscannerview.ui 11 | ) 12 | 13 | target_include_directories(swag_scanner_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/IController.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_ICONTROLLER_H 2 | #define SWAG_SCANNER_ICONTROLLER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace controller { 8 | /** 9 | * Represents an abstract base for controllers. 10 | * Have to inherit from QObject and QRunnable here because Qt requires these 11 | * to be at the base class. SMH. 12 | */ 13 | class IController : public QObject, public QRunnable { 14 | public: 15 | /** 16 | * Run the controller 17 | */ 18 | virtual void run(); 19 | 20 | virtual ~IController(); 21 | }; 22 | } 23 | 24 | #endif //SWAG_SCANNER_ICONTROLLER_H 25 | -------------------------------------------------------------------------------- /test/research/calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(calibration_target temp) 2 | add_custom_command(OUTPUT 3 | ${CMAKE_CURRENT_BINARY_DIR}/research/calibration/data/${calibration_target} 4 | COMMAND ${CMAKE_COMMAND} -E copy_directory 5 | ${CMAKE_SOURCE_DIR}/test/research/calibration/data 6 | ${CMAKE_CURRENT_BINARY_DIR}/data 7 | DEPENDS ${TEST_MAIN} 8 | ) 9 | 10 | add_custom_target(copy_calibration_files ALL 11 | DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/research/calibration/data/${calibration_target} 12 | ) 13 | 14 | target_sources(${TEST_MAIN} PRIVATE 15 | ${CMAKE_CURRENT_SOURCE_DIR}/calibration.cpp 16 | ) 17 | 18 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/processingTests/README.md: -------------------------------------------------------------------------------- 1 | # processingTests 2 | 3 | This folder contains tests for verifying mathematical and visual accuracy 4 | of processing algorithms. 5 | 6 | * [DepthTests.cpp](./DepthTests.cpp) : Verifies depth related methods such as creating pointclouds with depth frames. 7 | * [ModelTests.cpp](./ModelTests.cpp) : Verifies model methods 8 | * [ModelTests.cpp](./ModelTests.cpp) : Verifies model methods 9 | * [RegistrationPhysicalTests.cpp](./RegistrationPhysicalTests.cpp) : Verifies registration methods using premade example files in folder 10 | * [ModelTestsVisual.cpp](./visual/ModelTestsVisual.cpp) : Verifies model methods visually 11 | * [ModelTestsVisual.cpp](./visual/SegmentationTestsVisual.cpp) : Verifies segmentation methods visually 12 | -------------------------------------------------------------------------------- /test/research/registration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(registration_target temp) 2 | add_custom_command(OUTPUT 3 | ${CMAKE_CURRENT_BINARY_DIR}/research/registration/data/${registration_target} 4 | COMMAND ${CMAKE_COMMAND} -E copy_directory 5 | ${CMAKE_SOURCE_DIR}/test/research/registration/data 6 | ${CMAKE_CURRENT_BINARY_DIR}/data 7 | DEPENDS ${TEST_MAIN} 8 | ) 9 | 10 | add_custom_target(copy_registration_files ALL 11 | DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/research/registration/data/${registration_target} 12 | ) 13 | 14 | target_sources(${TEST_MAIN} PRIVATE 15 | ${CMAKE_CURRENT_SOURCE_DIR}/Registration.cpp 16 | ) 17 | 18 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /test/research/segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(segmentation_target temp) 2 | add_custom_command(OUTPUT 3 | ${CMAKE_CURRENT_BINARY_DIR}/research/segmentation/data/${segmentation_target} 4 | COMMAND ${CMAKE_COMMAND} -E copy_directory 5 | ${CMAKE_SOURCE_DIR}/test/research/segmentation/data 6 | ${CMAKE_CURRENT_BINARY_DIR}/data 7 | DEPENDS ${TEST_MAIN} 8 | ) 9 | 10 | add_custom_target(copy_segmentation_files ALL 11 | DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/research/segmentation/data/${segmentation_target} 12 | ) 13 | 14 | target_sources(${TEST_MAIN} PRIVATE 15 | ${CMAKE_CURRENT_SOURCE_DIR}/Segmentation.cpp 16 | ) 17 | 18 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/view/gui/FormsPayload.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_FORMSPAYLOAD_H 2 | #define SWAG_SCANNER_FORMSPAYLOAD_H 3 | 4 | #include "IFormsPayload.h" 5 | #include 6 | #include 7 | 8 | /** 9 | * Represents an object containing information from the forms. 10 | */ 11 | struct FormsPayload : public IFormsPayload { 12 | /** 13 | * This constructor creates a new FormsPayload object. Does conversions from QString to std::string and int. 14 | * @param name name field. 15 | * @param deg degree field. 16 | * @param rot rotation field. 17 | */ 18 | explicit FormsPayload(const QString &name, int angle, int rotations); 19 | 20 | std::string name; 21 | int angle; 22 | int rotations; 23 | }; 24 | 25 | #endif //SWAG_SCANNER_FORMSPAYLOAD_H 26 | -------------------------------------------------------------------------------- /src/view/gui/MoveFormsPayload.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_MOVEFORMSPAYLOAD_H 2 | #define SWAG_SCANNER_MOVEFORMSPAYLOAD_H 3 | 4 | #include "IFormsPayload.h" 5 | #include "MoveMethod.h" 6 | #include 7 | #include 8 | 9 | /** 10 | * Represents an object containing information from the move forms. 11 | */ 12 | struct MoveFormsPayload : public IFormsPayload { 13 | /** 14 | * This constructor creates a new MoveFormsPayload object. 15 | * If both the to and by forms are filled, then it will use the to form. 16 | * 17 | * @param method either to or by. 18 | * @param deg the number of degrees. 19 | */ 20 | explicit MoveFormsPayload(const MoveMethod &method, int deg); 21 | 22 | MoveMethod move_method; 23 | int deg; 24 | 25 | }; 26 | 27 | #endif //SWAG_SCANNER_MOVEFORMSPAYLOAD_H 28 | -------------------------------------------------------------------------------- /src/controller/processing/ProcessingControllerGUI.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_PROCESSINGCONTROLLERGUI_H 2 | #define SWAG_SCANNER_PROCESSINGCONTROLLERGUI_H 3 | 4 | #include "ProcessingController.h" 5 | #include "IControllerGUI.h" 6 | 7 | namespace controller { 8 | class ProcessingControllerGUI : public ProcessingController, public IControllerGUI { 9 | public: 10 | ProcessingControllerGUI(std::shared_ptr model, 11 | std::shared_ptr gui); 12 | 13 | /** 14 | * This run method is just like the base's run method, but it fetches move info before running. 15 | */ 16 | void run() override; 17 | 18 | void update(const IFormsPayload &payload) override; 19 | }; 20 | 21 | 22 | } 23 | 24 | #endif //SWAG_SCANNER_PROCESSINGCONTROLLERGUI_H 25 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.17) 2 | project(tests) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(TEST_MAIN unit_tests) 6 | 7 | include_directories( 8 | ${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} 9 | ${gmock_SOURCE_DIR}/include ${gmock_SOURCE_DIR} 10 | ) 11 | 12 | add_executable(${TEST_MAIN} main.cpp) 13 | target_link_libraries(${TEST_MAIN} PRIVATE swag_scanner_lib 14 | gtest 15 | gtest_main) 16 | 17 | add_subdirectory(calibrationTests) 18 | add_subdirectory(cameraTests) 19 | add_subdirectory(equationsTests) 20 | add_subdirectory(fileTests) 21 | add_subdirectory(processingTests) 22 | add_subdirectory(typesTests) 23 | add_subdirectory(utilsTests) 24 | 25 | if (BUILD_TESTS_VISUAL) 26 | message("building visual tests") 27 | add_subdirectory(research) 28 | 29 | endif() 30 | 31 | add_test(${TEST_MAIN} ${TEST_MAIN}) 32 | -------------------------------------------------------------------------------- /src/controller/processing/ProcessingController.cpp: -------------------------------------------------------------------------------- 1 | #include "ProcessingController.h" 2 | #include "Visualizer.h" 3 | #include "ProcessingModel.h" 4 | #include "Logger.h" 5 | #include 6 | #include 7 | 8 | using json = nlohmann::json; 9 | 10 | controller::ProcessingController::ProcessingController(std::shared_ptr model) : 11 | model(std::move(model)) {} 12 | 13 | void controller::ProcessingController::run() { 14 | logger::info("starting processing"); 15 | logger::info("[TRANSFORMING]"); 16 | model->transform_clouds_to_world(); 17 | logger::info("[FINISHED TRANSFORMING]"); 18 | logger::info("[FILTERING]"); 19 | model->filter(); 20 | logger::info("[FINISHED FILTERING]"); 21 | logger::info("[REGISTERING]"); 22 | model->register_clouds(); 23 | logger::info("[FINISHED REGISTERING]"); 24 | } 25 | -------------------------------------------------------------------------------- /src/controller/move/MoveControllerGUI.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_MOVECONTROLLERGUI_H 2 | #define SWAG_SCANNER_MOVECONTROLLERGUI_H 3 | 4 | #include "MoveController.h" 5 | #include "IControllerGUI.h" 6 | 7 | namespace arduino { 8 | class Arduino; 9 | } 10 | 11 | class SwagGUI; 12 | 13 | class IFormsPayload; 14 | 15 | namespace controller { 16 | class MoveControllerGUI : public MoveController, public IControllerGUI { 17 | public: 18 | MoveControllerGUI(std::shared_ptr arduino, 19 | std::shared_ptr gui); 20 | 21 | /** 22 | * This run method is just like the base's run method, but it fetches move info before running. 23 | */ 24 | void run() override; 25 | 26 | void update(const IFormsPayload &payload) override; 27 | 28 | private: 29 | 30 | }; 31 | } 32 | 33 | #endif //SWAG_SCANNER_MOVECONTROLLERGUI_H 34 | -------------------------------------------------------------------------------- /src/controller/scan/ScanControllerGUI.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_SCANCONTROLLERGUI_H 2 | #define SWAG_SCANNER_SCANCONTROLLERGUI_H 3 | 4 | #include "ScanController.h" 5 | #include "IControllerGUI.h" 6 | 7 | 8 | namespace controller { 9 | class ScanControllerGUI : public ScanController, public IControllerGUI { 10 | public: 11 | ScanControllerGUI(std::shared_ptr camera, 12 | std::shared_ptr arduino, 13 | std::shared_ptr model, 14 | std::shared_ptr gui); 15 | 16 | /** 17 | * This run method is just like the base's run method, but it fetches move info before running. 18 | */ 19 | void run() override; 20 | 21 | void update(const IFormsPayload &payload) override; 22 | }; 23 | } 24 | 25 | #endif //SWAG_SCANNER_SCANCONTROLLERGUI_H 26 | -------------------------------------------------------------------------------- /test/equationsTests/NormalTests.cpp: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_NORMALTESTS_CPP 2 | #define SWAG_SCANNER_NORMALTESTS_CPP 3 | 4 | #include 5 | #include 6 | #include "Normal.h" 7 | 8 | TEST(NormalTests, TestConstructor1) { 9 | equations::Normal n(1.1, 1.3, -1.1); 10 | EXPECT_FLOAT_EQ(n.A, 1.1); 11 | EXPECT_FLOAT_EQ(n.B, 1.3); 12 | EXPECT_FLOAT_EQ(n.C, -1.1); 13 | } 14 | 15 | TEST(NormalTests, TestConstructor2) { 16 | std::vector v{1.1, 1.3, -1.1}; 17 | equations::Normal n(v); 18 | EXPECT_FLOAT_EQ(n.A, 1.1); 19 | EXPECT_FLOAT_EQ(n.B, 1.3); 20 | EXPECT_FLOAT_EQ(n.C, -1.1); 21 | } 22 | 23 | TEST(NormalTests, TestAdditionOverload) { 24 | std::vector v{1.1, 1.3, -1.1}; 25 | equations::Normal n1(v); 26 | equations::Normal n2(v); 27 | equations::Normal n3 = n1 + n2; 28 | EXPECT_DOUBLE_EQ(n3.A, 2.2); 29 | EXPECT_DOUBLE_EQ(n3.B, 2.6); 30 | EXPECT_DOUBLE_EQ(n3.C, -2.2); 31 | } 32 | 33 | #endif //SWAG_SCANNER_NORMALTESTS_CPP -------------------------------------------------------------------------------- /src/types/equations/Plane.cpp: -------------------------------------------------------------------------------- 1 | #include "Plane.h" 2 | #include "Normal.h" 3 | 4 | equations::Plane::Plane(double A, double B, double C, double D) : A(A), B(B), C(C), D(D) {} 5 | 6 | equations::Plane::Plane(const std::vector &in) : A(in[0]), B(in[1]), C(in[2]), D(in[3]) {} 7 | 8 | equations::Plane::Plane(const std::shared_ptr &in) : A(in->values[0]), B(in->values[1]), 9 | C(in->values[2]), D(in->values[3]) {} 10 | 11 | equations::Plane::Plane(const pcl::ModelCoefficients &in) : A(in.values[0]), B(in.values[1]), 12 | C(in.values[2]), D(in.values[3]) {} 13 | 14 | equations::Normal equations::Plane::get_normal() const { 15 | return equations::Normal(this->A, this->B, this->C); 16 | } 17 | 18 | equations::Plane equations::Plane::operator+(const equations::Plane &p2) const { 19 | return equations::Plane(A + p2.A, B + p2.B, C + p2.C, D + p2.D); 20 | } 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/controller/processing/ProcessingControllerGUI.cpp: -------------------------------------------------------------------------------- 1 | #include "ProcessingControllerGUI.h" 2 | #include "ProcessingController.h" 3 | #include "ProcessingModel.h" 4 | #include "FormsPayload.h" 5 | #include "SwagGUI.h" 6 | 7 | 8 | controller::ProcessingControllerGUI::ProcessingControllerGUI(std::shared_ptr model, 9 | std::shared_ptr gui) : 10 | ProcessingController(std::move(model)), 11 | IControllerGUI(std::move(gui)) {} 12 | 13 | void controller::ProcessingControllerGUI::run() { 14 | emit update_console("Starting processing"); 15 | ProcessingController::run(); 16 | 17 | gui->display_cloud(model->get_cloud("REGISTERED.pcd")); 18 | emit update_console("Processing done!"); 19 | } 20 | 21 | void controller::ProcessingControllerGUI::update(const IFormsPayload &payload) { 22 | const auto &p = dynamic_cast(payload); 23 | model->clear_clouds(); 24 | model->set_scan(p.name); 25 | } 26 | 27 | -------------------------------------------------------------------------------- /src/controller/move/MoveController.cpp: -------------------------------------------------------------------------------- 1 | #include "MoveController.h" 2 | #include "Arduino.h" 3 | #include "IFileHandler.h" 4 | #include 5 | 6 | using json = nlohmann::json; 7 | 8 | controller::MoveController::MoveController(std::shared_ptr arduino) : 9 | arduino(std::move(arduino)) {} 10 | 11 | void controller::MoveController::run() { 12 | if (move_method == MoveMethod::TO) { 13 | arduino->rotate_to(deg); 14 | } else if (move_method == MoveMethod::BY) { 15 | arduino->rotate_by(deg); 16 | } 17 | } 18 | 19 | void controller::MoveController::set_deg(int degs) { 20 | this->deg = degs; 21 | } 22 | 23 | void controller::MoveController::set_home() { 24 | json settings_json = file::IFileHandler::load_swag_scanner_info_json(); 25 | settings_json["current_position"] = 0; 26 | file::IFileHandler::write_swag_scanner_info_json(settings_json); 27 | } 28 | 29 | void controller::MoveController::set_move_method(const MoveMethod &move_method) { 30 | this->move_method = move_method; 31 | } 32 | -------------------------------------------------------------------------------- /test/research/depthFiltering/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #file(COPY data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) 2 | 3 | # https://stackoverflow.com/questions/40294146/alternative-to-cmake-post-build-command-when-target-is-in-subdirectory 4 | # used the stackoverflow for the logic behind custom command outside of target definition directory 5 | 6 | 7 | set(temp_file temp) 8 | add_custom_command(OUTPUT 9 | ${CMAKE_CURRENT_BINARY_DIR}/research/DepthFiltering/data/${temp_file} 10 | COMMAND ${CMAKE_COMMAND} -E copy_directory 11 | ${CMAKE_SOURCE_DIR}/test/research/DepthFiltering/data 12 | ${CMAKE_CURRENT_BINARY_DIR}/data 13 | DEPENDS ${TEST_MAIN} 14 | ) 15 | 16 | message("make it here ") 17 | add_custom_target(copy_files ALL 18 | DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/research/DepthFiltering/data/${temp_file} 19 | ) 20 | 21 | target_sources(${TEST_MAIN} PRIVATE 22 | ${CMAKE_CURRENT_SOURCE_DIR}/CompareDepthFiltering.cpp 23 | ) 24 | 25 | target_include_directories(${TEST_MAIN} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -------------------------------------------------------------------------------- /src/controller/edit/EditingControllerGUI.cpp: -------------------------------------------------------------------------------- 1 | #include "EditingControllerGUI.h" 2 | #include "ProcessingModel.h" 3 | #include "SwagGUI.h" 4 | #include "CloudType.h" 5 | #include "FormsPayload.h" 6 | #include 7 | 8 | controller::EditingControllerGUI::EditingControllerGUI(std::shared_ptr model, 9 | std::shared_ptr gui) : 10 | model(std::move(model)), 11 | IControllerGUI(std::move(gui)) {} 12 | 13 | void controller::EditingControllerGUI::run() { 14 | std::shared_ptr> cloud = model->load_cloud("REGISTERED.pcd", 15 | CloudType::Type::REGISTERED); 16 | gui->display_cloud(cloud); 17 | } 18 | 19 | void controller::EditingControllerGUI::update(const IFormsPayload &payload) { 20 | const auto &p = dynamic_cast(payload); 21 | std::filesystem::path scan_path = p.name; 22 | model->set_scan(scan_path.filename().string()); 23 | } 24 | -------------------------------------------------------------------------------- /src/types/equations/Equations.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_EQUATIONS_H 2 | #define SWAG_SCANNER_EQUATIONS_H 3 | 4 | #include "Normal.h" 5 | 6 | namespace equations { 7 | 8 | /** 9 | * Cross product of two 1x3 Normals. 10 | * @param n1 first normal. 11 | * @param n2 second normal. 12 | * @return cross product result. 13 | */ 14 | inline Normal cross(Normal n1, Normal n2) { 15 | return Normal(-n2.B * n1.C + n1.B * n2.C, n2.A * n1.C - n1.A * n2.C, -n2.A * n1.B + n1.A * n2.B); 16 | } 17 | 18 | /** 19 | * Take the norm of given normal. 20 | * @param n the normal. 21 | * @return scalar result of norm. 22 | */ 23 | inline double norm(Normal n) { 24 | return sqrt(pow(n.A, 2) + pow(n.B, 2) + pow(n.C, 2)); 25 | } 26 | 27 | /** 28 | * Calculate norm(n1)/norm(cross(n1,n2)) 29 | * @param n1 normal. 30 | * @param n2 normal. 31 | * @return norm(n1)/norm(cross(n1,n2)). 32 | */ 33 | inline double coeff(Normal n1, Normal n2) { 34 | return norm(n1) / norm(cross(n1, n2)); 35 | } 36 | } 37 | 38 | #endif //SWAG_SCANNER_EQUATIONS_H -------------------------------------------------------------------------------- /license: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) [2020] [sean ng pack] 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /src/controller/move/MoveControllerGUI.cpp: -------------------------------------------------------------------------------- 1 | #include "MoveControllerGUI.h" 2 | #include "Arduino.h" 3 | #include "SwagGUI.h" 4 | #include "MoveMethod.h" 5 | #include "MoveFormsPayload.h" 6 | 7 | controller::MoveControllerGUI::MoveControllerGUI(std::shared_ptr arduino, 8 | std::shared_ptr gui) : 9 | MoveController(std::move(arduino)), 10 | IControllerGUI(std::move(gui)) {} 11 | 12 | void controller::MoveControllerGUI::run() { 13 | if (move_method == MoveMethod::TO) { 14 | emit update_console("moving to position: " + std::to_string(deg)); 15 | arduino->rotate_to(deg); 16 | emit update_console("done moving"); 17 | } else if (move_method == MoveMethod::BY) { 18 | emit update_console("moving by: " + std::to_string(deg) + " degrees"); 19 | arduino->rotate_by(deg); 20 | emit update_console("done moving"); 21 | } 22 | } 23 | 24 | void controller::MoveControllerGUI::update(const IFormsPayload &payload) { 25 | const auto &p = dynamic_cast(payload); 26 | set_move_method(p.move_method); 27 | set_deg(p.deg); 28 | } 29 | -------------------------------------------------------------------------------- /test/typesTests/CameraTypesTests.cpp: -------------------------------------------------------------------------------- 1 | #include "../../src/types/CameraTypes.h" 2 | #include "gtest/gtest.h" 3 | 4 | 5 | class CameraTypesFixture : public ::testing::Test { 6 | 7 | 8 | protected: 9 | camera::intrinsics *intrinsics; 10 | 11 | virtual void SetUp() { 12 | float distortion[5] = {.139, .124, .0043, .00067, -.034}; 13 | intrinsics = new camera::intrinsics(100, 100, 14 | 10.0, 10.0, 100.0, 100.0, 15 | RS2_DISTORTION_INVERSE_BROWN_CONRADY, 16 | distortion, .001); 17 | } 18 | 19 | virtual void TearDown() { 20 | delete intrinsics; 21 | } 22 | }; 23 | 24 | 25 | TEST_F(CameraTypesFixture, TestToString) { 26 | ASSERT_EQ(intrinsics->to_string(), "width: 100\n" 27 | "height: 100\n" 28 | "fx: 10.000000\n" 29 | "fy: 10.000000\n" 30 | "ppx: 100.000000\n" 31 | "ppy: 100.000000\n" 32 | "depth scale: 0.001000"); 33 | } 34 | -------------------------------------------------------------------------------- /src/controller/processing/ProcessingController.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_PROCESSINGCONTROLLER_H 2 | #define SWAG_SCANNER_PROCESSINGCONTROLLER_H 3 | 4 | #include "IController.h" 5 | #include "CloudType.h" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace visual { 11 | class Visualizer; 12 | } 13 | 14 | namespace model { 15 | class ProcessingModel; 16 | } 17 | 18 | namespace file { 19 | class ScanFileHandler; 20 | } 21 | 22 | 23 | namespace controller { 24 | 25 | /** 26 | * This controller handles data processing commands. 27 | */ 28 | class ProcessingController : public virtual IController { 29 | public: 30 | explicit ProcessingController(std::shared_ptr model); 31 | 32 | /** 33 | * Process the data. Filters, segments, and rotates the clouds. 34 | */ 35 | void run() override; 36 | 37 | 38 | protected: 39 | std::shared_ptr model; 40 | std::shared_ptr viewer; 41 | std::shared_ptr file_handler; 42 | }; 43 | 44 | 45 | } 46 | 47 | #endif //SWAG_SCANNER_PROCESSINGCONTROLLER_H 48 | -------------------------------------------------------------------------------- /src/controller/edit/EditingControllerGUI.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_EDITINGCONTROLLERGUI_H 2 | #define SWAG_SCANNER_EDITINGCONTROLLERGUI_H 3 | 4 | #include "IController.h" 5 | #include "IControllerGUI.h" 6 | 7 | namespace model { 8 | class ProcessingModel; 9 | } 10 | 11 | namespace pcl { 12 | class PointXYZ; 13 | 14 | template 15 | class PointCloud; 16 | } 17 | 18 | namespace controller { 19 | 20 | class EditingControllerGUI : public IControllerGUI { 21 | public: 22 | EditingControllerGUI(std::shared_ptr model, 23 | std::shared_ptr gui); 24 | 25 | /** 26 | * Display the cloud on the screen. 27 | * 28 | * Note: current only displays the registered cloud 29 | */ 30 | void run() override; 31 | 32 | /** 33 | * TBH, this doesn't really need to anything either, but need to override to conform to superclass. 34 | */ 35 | void update(const IFormsPayload &payload) override; 36 | 37 | protected: 38 | std::shared_ptr model; 39 | std::string cloud_path; 40 | }; 41 | } 42 | 43 | #endif //SWAG_SCANNER_EDITINGCONTROLLERGUI_H 44 | -------------------------------------------------------------------------------- /src/utils/Constants.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CONSTANTS_H 2 | #define SWAG_SCANNER_CONSTANTS_H 3 | 4 | namespace constants { 5 | inline constexpr float BED_DIAMETER = .180; 6 | inline constexpr float MAX_SCAN_HEIGHT = .3; 7 | inline constexpr int SENSOR_ANGLE = 45; 8 | inline constexpr float CENTER_TO_SENSOR_H = .180; 9 | inline constexpr float CENTER_TO_SENSOR_V = .180; 10 | 11 | // crop coefficients for calibration 12 | // can make them a bit bigger because of robust calibration plane detection 13 | inline constexpr float cal_min_x = -.10; 14 | inline constexpr float cal_max_x = .10; 15 | inline constexpr float cal_min_y = - 100; 16 | inline constexpr float cal_max_y = .13; 17 | inline constexpr float cal_min_z = -100; 18 | inline constexpr float cal_max_z = .49; 19 | 20 | // dimensions of scanning box 21 | // crop coefficients after calibration is done 22 | inline constexpr float scan_min_x = -.11; 23 | inline constexpr float scan_max_x = .11; 24 | inline constexpr float scan_min_y = - .11; 25 | inline constexpr float scan_max_y = .11; 26 | inline constexpr float scan_min_z = 0; 27 | inline constexpr float scan_max_z = .17; 28 | 29 | } 30 | 31 | #endif //SWAG_SCANNER_CONSTANTS_H 32 | -------------------------------------------------------------------------------- /src/controller/manager/ControllerManager.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CONTROLLERMANAGER_H 2 | #define SWAG_SCANNER_CONTROLLERMANAGER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace controller { 8 | class IController; 9 | class IControllerGUI; 10 | class ControllerManagerCache; 11 | } 12 | 13 | class SwagGUI; 14 | 15 | namespace controller { 16 | class ControllerManager { 17 | public: 18 | 19 | ControllerManager(); 20 | 21 | ~ControllerManager(); 22 | 23 | std::shared_ptr get_controller(const boost::program_options::variables_map &vm); 24 | 25 | /** 26 | * Yes, I'm using string comparison because the input is from the CLI, so it will be a string anyways. 27 | */ 28 | std::shared_ptr get_controller(const std::string &name); 29 | 30 | /** 31 | * This on the other hand, I can use an enum for later. 32 | */ 33 | std::shared_ptr get_gui_controller(const std::string &name); 34 | 35 | std::shared_ptr get_gui(); 36 | 37 | 38 | private: 39 | std::unique_ptr cache; 40 | 41 | }; 42 | } 43 | 44 | #endif //SWAG_SCANNER_CONTROLLERMANAGER_H 45 | -------------------------------------------------------------------------------- /src/controller/calibration/CalibrationControllerGUI.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CALIBRATIONCONTROLLERGUI_H 2 | #define SWAG_SCANNER_CALIBRATIONCONTROLLERGUI_H 3 | 4 | #include "CalibrationController.h" 5 | #include "IControllerGUI.h" 6 | 7 | class SwagGUI; 8 | 9 | class IFormsPayload; 10 | 11 | namespace controller { 12 | class CalibrationControllerGUI : public CalibrationController, public IControllerGUI { 13 | 14 | public: 15 | CalibrationControllerGUI(std::shared_ptr camera, 16 | std::shared_ptr arduino, 17 | std::shared_ptr model, 18 | std::shared_ptr gui); 19 | 20 | /** 21 | * This run method is a little different than CalibrationController's run method. This will 22 | * fetch the values from the view before proceeding. 23 | */ 24 | void run() override; 25 | 26 | 27 | /** 28 | * Update params using given payload. 29 | * 30 | * @param payload payload containing cloud path info. 31 | */ 32 | void update(const IFormsPayload &payload) override; 33 | 34 | }; 35 | 36 | } 37 | 38 | #endif //SWAG_SCANNER_CALIBRATIONCONTROLLERGUI_H 39 | -------------------------------------------------------------------------------- /test/equationsTests/PlaneTests.cpp: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_PLANETESTS_CPP 2 | #define SWAG_SCANNER_PLANETESTS_CPP 3 | 4 | #include 5 | #include 6 | #include "Normal.h" 7 | #include "Plane.h" 8 | 9 | TEST(PlaneTests, TestConstructor1) { 10 | equations::Plane p(1.1, 1.3, -1.1, 3.1); 11 | EXPECT_FLOAT_EQ(p.A, 1.1); 12 | EXPECT_FLOAT_EQ(p.B, 1.3); 13 | EXPECT_FLOAT_EQ(p.C, -1.1); 14 | EXPECT_FLOAT_EQ(p.D, 3.1); 15 | } 16 | 17 | TEST(PlaneTests, TestConstructor2) { 18 | std::vector v{1.1, 1.3, -1.1, 3.1}; 19 | equations::Plane p(v); 20 | EXPECT_FLOAT_EQ(p.A, 1.1); 21 | EXPECT_FLOAT_EQ(p.B, 1.3); 22 | EXPECT_FLOAT_EQ(p.C, -1.1); 23 | EXPECT_FLOAT_EQ(p.D, 3.1); 24 | } 25 | 26 | TEST(PlaneTests, TestGetNormal) { 27 | std::vector v{1.1, 1.3, -1.1, 3.1}; 28 | equations::Plane p(v); 29 | equations::Normal n = p.get_normal(); 30 | EXPECT_FLOAT_EQ(n.A, 1.1); 31 | EXPECT_FLOAT_EQ(n.B, 1.3); 32 | EXPECT_FLOAT_EQ(n.C, -1.1); 33 | } 34 | 35 | TEST(PlaneTests, TestAdditionOverload) { 36 | std::vector v{1.1, 1.3, -1.1, 3.1}; 37 | equations::Plane p1(v); 38 | equations::Plane p2(v); 39 | equations::Plane p3 = p1 + p2; 40 | EXPECT_DOUBLE_EQ(p3.A, 2.2); 41 | EXPECT_DOUBLE_EQ(p3.B, 2.6); 42 | EXPECT_DOUBLE_EQ(p3.C, -2.2); 43 | EXPECT_DOUBLE_EQ(p3.D, 6.2); 44 | } 45 | 46 | #endif //SWAG_SCANNER_PLANETESTS_CPP -------------------------------------------------------------------------------- /src/controller/move/MoveController.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_MOVECONTROLLER_H 2 | #define SWAG_SCANNER_MOVECONTROLLER_H 3 | 4 | #include "IController.h" 5 | #include "MoveMethod.h" 6 | #include 7 | 8 | namespace arduino { 9 | class Arduino; 10 | } 11 | 12 | namespace controller { 13 | /** 14 | * This controller allows for manual rotation of the table using commands from the commandline. 15 | */ 16 | class MoveController : public virtual IController { 17 | public: 18 | 19 | explicit MoveController(std::shared_ptr arduino); 20 | 21 | /** 22 | * Move table to position, or by given amount. 23 | */ 24 | void run() override; 25 | 26 | /** 27 | * Set the degrees. 28 | * @param deg degrees. 29 | */ 30 | void set_deg(int deg); 31 | 32 | /** 33 | * Set the current position to the home position. Writes this to settings.json. 34 | */ 35 | void set_home(); 36 | 37 | /** 38 | * Set the controller to move the table either move TO a position, or BY a degree amount. 39 | * 40 | * @param input either "to" or "by" command. 41 | */ 42 | void set_move_method(const MoveMethod &move_method); 43 | 44 | 45 | protected: 46 | std::shared_ptr arduino; 47 | MoveMethod move_method; 48 | int deg = 0; 49 | }; 50 | } 51 | 52 | #endif //SWAG_SCANNER_MOVECONTROLLER_H 53 | -------------------------------------------------------------------------------- /src/controller/IControllerGUI.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_ICONTROLLERGUI_H 2 | #define SWAG_SCANNER_ICONTROLLERGUI_H 3 | 4 | #include "IController.h" 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | class SwagGUI; 11 | 12 | class IFormsPayload; 13 | 14 | Q_DECLARE_METATYPE(std::string) 15 | 16 | namespace controller { 17 | /** 18 | * Represents a controller for GUIs. 19 | */ 20 | class IControllerGUI : public IController { 21 | Q_OBJECT 22 | 23 | public: 24 | 25 | explicit IControllerGUI(std::shared_ptr gui); 26 | 27 | virtual ~IControllerGUI() {} 28 | 29 | 30 | /** 31 | * This method connects gui to this controller. Must run this method whenever 32 | * swapping controllers for GUI. Call this method in the manager class. 33 | * 34 | */ 35 | // void setup_gui(); 36 | 37 | /** 38 | * Update the controller with the given payload. 39 | * 40 | * @param payload payload from GUI. 41 | */ 42 | virtual void update(const IFormsPayload &payload) = 0; 43 | 44 | 45 | signals: 46 | 47 | /** 48 | * Write message to the GUI console. 49 | * 50 | * @param info message you want to write. 51 | */ 52 | void update_console(const std::string &info); 53 | 54 | 55 | protected: 56 | std::shared_ptr gui; 57 | 58 | }; 59 | 60 | } 61 | 62 | #endif //SWAG_SCANNER_ICONTROLLERGUI_H 63 | -------------------------------------------------------------------------------- /src/controller/scan/ScanController.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_SCANCONTROLLER_H 2 | #define SWAG_SCANNER_SCANCONTROLLER_H 3 | 4 | #include "IController.h" 5 | #include 6 | 7 | namespace visual { 8 | class Visualizer; 9 | } 10 | 11 | namespace model { 12 | class ScanModel; 13 | } 14 | 15 | namespace file { 16 | class ScanFileHandler; 17 | } 18 | 19 | namespace arduino { 20 | class Arduino; 21 | } 22 | 23 | namespace camera { 24 | class ICamera; 25 | } 26 | 27 | namespace controller { 28 | /** 29 | * This controller handles data acquisition. 30 | */ 31 | class ScanController : public virtual IController { 32 | public: 33 | ScanController(std::shared_ptr camera, 34 | std::shared_ptr arduino, 35 | std::shared_ptr model); 36 | 37 | void run() override; 38 | 39 | void set_deg(int deg); 40 | 41 | void set_num_rot(int num_rot); 42 | 43 | /** 44 | * Write folders, run the scan and collect data. 45 | * @param degs number of degrees per rotation. 46 | * @param num_rot number of rotations. 47 | * the scan to (RAW, CALIBRATION, etc) 48 | */ 49 | void scan(); 50 | 51 | 52 | protected: 53 | std::shared_ptr camera; 54 | std::shared_ptr arduino; 55 | std::shared_ptr model; 56 | int deg = 20; 57 | int num_rot = 18; 58 | 59 | }; 60 | } 61 | 62 | #endif //SWAG_SCANNER_SCANCONTROLLER_H 63 | -------------------------------------------------------------------------------- /src/model/scan/ScanModel.cpp: -------------------------------------------------------------------------------- 1 | #include "ScanModel.h" 2 | #include "Logger.h" 3 | #include 4 | 5 | 6 | namespace fs = std::filesystem; 7 | 8 | model::ScanModel::ScanModel() : file_handler() {} 9 | 10 | void model::ScanModel::set_scan(const std::string &scan_name) { 11 | file_handler.set_scan(scan_name); 12 | } 13 | 14 | void model::ScanModel::save_cloud(const std::string &cloud_name, const CloudType::Type &cloud_type) { 15 | auto cloud = clouds[clouds_map[cloud_name]]; 16 | file_handler.save_cloud(cloud, cloud_name, cloud_type); 17 | } 18 | 19 | void model::ScanModel::save_cloud(std::shared_ptr> cloud, 20 | const std::string &cloud_name, 21 | const CloudType::Type &cloud_type) { 22 | file_handler.save_cloud(cloud, cloud_name, cloud_type); 23 | } 24 | 25 | // TODO: later add functionality where you can set whatever calibration you want to use 26 | // this applies to the processing model not scan model!! 27 | void model::ScanModel::update_info_json(int deg, int num_rot) { 28 | auto t = std::time(nullptr); 29 | auto tm = *std::localtime(&t); 30 | std::ostringstream oss; 31 | oss << std::put_time(&tm, "%m-%d-%Y %H:%M:%S"); 32 | auto date = oss.str(); 33 | 34 | fs::path latest_cal_path = file_handler.find_latest_calibration(); 35 | std::string info_json_path = latest_cal_path.string() 36 | + "/" + latest_cal_path.filename().string() + ".json";; 37 | file_handler.update_info_json(date, deg, num_rot, info_json_path); 38 | } 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/cli/CLIParser.cpp: -------------------------------------------------------------------------------- 1 | #include "CLIParser.h" 2 | #include "Logger.h" 3 | #include 4 | 5 | namespace po = boost::program_options; 6 | 7 | cli::CLIParser::CLIParser() { 8 | logger::setup_file_logger(); 9 | // desc = po::options_description 10 | // desc("SwagScanner options"); 11 | desc.add_options() 12 | ("help, h", "lol no help here") 13 | // main commands 14 | ("scan", "scan object") 15 | ("calibrate", "calibrate scanner") 16 | ("process", "process scanned data") 17 | ("move", "move calibration bed") 18 | ("set_home", "move calibration bed") 19 | 20 | // gui 21 | ("gui", "start gui application") 22 | 23 | // arguments for main commands 24 | ("name", po::value(), "set name of scan") 25 | ("deg", po::value(), "degrees") 26 | ("rot", po::value(), "number of rotations") 27 | ("s_alpha", po::value(), "smooth alpha") 28 | ("s_delta", po::value(), "smooth delta") 29 | ("d_mag", po::value(), "decimation filter magnitude") 30 | ("s_mag", po::value(), "spatial filter magnitude") 31 | ("to", po::value(), "move to a position") 32 | ("by", po::value(), "move by degrees") 33 | ("home", "move to 0 position"); 34 | } 35 | 36 | po::variables_map cli::CLIParser::get_variables_map(int argc, char *argv[]) { 37 | po::variables_map vm; 38 | po::store(po::parse_command_line(argc, argv, desc), vm); 39 | po::notify(vm); 40 | return vm; 41 | } 42 | 43 | -------------------------------------------------------------------------------- /src/types/equations/Normal.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_NORMAL_H 2 | #define SWAG_SCANNER_NORMAL_H 3 | 4 | #include 5 | 6 | 7 | namespace equations { 8 | /** 9 | * Class represents the normal equation of a plane. 10 | */ 11 | typedef struct Normal { 12 | double A = 0.0; 13 | double B = 0.0; 14 | double C = 0.0; 15 | 16 | /** 17 | * Default constructor. Don't forget to set values for A,B,C. 18 | */ 19 | Normal() = default; 20 | 21 | /** 22 | * Create a normal object given the coefficients of a plane. 23 | * @param A Coefficient 24 | * @param B Coefficient 25 | * @param C Coefficient 26 | */ 27 | Normal(double A, double B, double C); 28 | 29 | /** 30 | * Initialize a normal given a vector of three coefficients. 31 | * @param in vector of three floats. 32 | * @throws invalid_argument if the vector contains too few coefficients. 33 | */ 34 | Normal(const std::vector &in); 35 | 36 | 37 | /** 38 | * Initialize a normal given PCL ModelCoefficients. 39 | * @param in PCL coefficients of four coefficients (A,B,C,D) but will only use A,B,C 40 | */ 41 | Normal(const pcl::ModelCoefficients &in); 42 | 43 | /** 44 | * Overloaded addition + operator. 45 | * @param n2 other normal to add. 46 | * @return new normal, addition of this and other. 47 | */ 48 | Normal operator+(const Normal &n2) const; 49 | 50 | ~Normal() = default; 51 | } Normal; 52 | } 53 | 54 | #endif //SWAG_SCANNER_NORMAL_H 55 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "CLIParser.h" 2 | #include "IController.h" 3 | #include "ControllerManager.h" 4 | #include "ControllerManagerCache.h" 5 | #include "IFileHandler.h" 6 | #include "SwagGUI.h" 7 | #include "Logger.h" 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | int main(int argc, char *argv[]) { 14 | file::IFileHandler::check_program_folder(); 15 | 16 | auto default_logger = logger::setup_default_logger(); 17 | std::unique_ptr cli_parser = std::make_unique(); 18 | boost::program_options::variables_map vm = cli_parser->get_variables_map(argc, argv); 19 | 20 | if (vm.count("gui")) { 21 | auto file_logger = logger::setup_file_logger(); 22 | // this is sloppy, but i need it to initialize the logger. I should make a function that deletes the generated file. 23 | logger::set_file_logger_location(file::IFileHandler::swag_scanner_path.string() + "/settings/log_init.txt"); 24 | spdlog::register_logger(file_logger); 25 | spdlog::set_level(spdlog::level::level_enum::debug); 26 | file_logger->flush_on(spdlog::level::info); 27 | 28 | QApplication app(argc, argv); 29 | controller::ControllerManager manager; 30 | std::shared_ptr gui = manager.get_gui(); 31 | gui->show(); 32 | return app.exec(); 33 | } else { 34 | controller::ControllerManager manager; 35 | std::shared_ptr controller = manager.get_controller(vm); 36 | controller->run(); 37 | return 0; 38 | } 39 | // pretty sure this does nothing 40 | spdlog::shutdown(); 41 | } 42 | -------------------------------------------------------------------------------- /src/controller/scan/ScanController.cpp: -------------------------------------------------------------------------------- 1 | #include "ScanController.h" 2 | #include "ScanModel.h" 3 | #include "Arduino.h" 4 | #include "SR305.h" 5 | #include "Visualizer.h" 6 | #include "ScanFileHandler.h" 7 | #include "Logger.h" 8 | #include 9 | #include 10 | 11 | namespace fs = std::filesystem; 12 | 13 | controller::ScanController::ScanController(std::shared_ptr camera, 14 | std::shared_ptr arduino, 15 | std::shared_ptr model) : 16 | camera(std::move(camera)), 17 | arduino(std::move(arduino)), 18 | model(std::move(model)) {} 19 | 20 | void controller::ScanController::run() { 21 | scan(); 22 | } 23 | 24 | void controller::ScanController::set_deg(int deg) { 25 | deg = deg; 26 | } 27 | 28 | void controller::ScanController::set_num_rot(int num_rot) { 29 | num_rot = num_rot; 30 | } 31 | 32 | void controller::ScanController::scan() { 33 | model->update_info_json(deg, num_rot); 34 | camera->scan(); 35 | const camera::intrinsics intrin = camera->get_intrinsics(); 36 | logger::info("started scanning..."); 37 | for (int i = 0; i < num_rot; i++) { 38 | std::string name = std::to_string(i * deg) + ".pcd"; 39 | camera->scan(); 40 | std::vector depth_frame_raw = camera->get_depth_frame(); 41 | std::shared_ptr> cloud_raw = camera->create_point_cloud(depth_frame_raw, intrin); 42 | model->add_cloud(cloud_raw, name); 43 | model->save_cloud(name, CloudType::Type::RAW); 44 | arduino->rotate_by(deg); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /src/types/CameraTypes.cpp: -------------------------------------------------------------------------------- 1 | #include "CameraTypes.h" 2 | 3 | camera::intrinsics::intrinsics(rs2_intrinsics intrin, float depth_scale) : 4 | width(intrin.width), height(intrin.height), fx(intrin.fx), fy(intrin.fy), 5 | ppx(intrin.ppx), ppy(intrin.ppy), model(intrin.model), 6 | depth_scale(depth_scale) { 7 | for (int i = 0; i < 5; i++) { 8 | coeffs[i] = intrin.coeffs[i]; 9 | } 10 | } 11 | 12 | camera::intrinsics::intrinsics(int width, 13 | int height, 14 | float fx, 15 | float fy, 16 | float ppx, 17 | float ppy, 18 | rs2_distortion model, 19 | float coeffs[5], 20 | float depth_scale) : width(width), height(height), fx(fx), 21 | fy(fy), ppx(ppx), ppy(ppy), model(model), 22 | depth_scale(depth_scale) { 23 | for (int i = 0; i < 5; i++) { 24 | this->coeffs[i] = coeffs[i]; 25 | } 26 | } 27 | 28 | std::string camera::intrinsics::to_string() const { 29 | return "width: " + std::to_string(width) + "\n" + 30 | "height: " + std::to_string(height) + "\n" + 31 | "fx: " + std::to_string(fx) + "\n" + 32 | "fy: " + std::to_string(fy) + "\n" + 33 | "ppx: " + std::to_string(ppx) + "\n" + 34 | "ppy: " + std::to_string(ppy) + "\n" + 35 | "depth scale: " + std::to_string(depth_scale); 36 | } 37 | 38 | camera::intrinsics::~intrinsics() {} 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/types/CameraTypes.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #ifndef SWAG_SCANNER_CAMERATYPES_H 5 | #define SWAG_SCANNER_CAMERATYPES_H 6 | 7 | namespace camera { 8 | /** 9 | * Intrinsics for camera. 10 | */ 11 | typedef struct intrinsics { 12 | int width; /** width of image in pixels */ 13 | int height; /** height of image in pixels */ 14 | float fx; /** focal length of image, as a multiple of pixel width & height */ 15 | float fy; /** focal length of image, as a multiple of pixel width & height */ 16 | float ppx; /** pixel coordinates of the principal point (center of projection) */ 17 | float ppy; /** pixel coordinates of the principal point (center of projection) */ 18 | rs2_distortion model; /** model used to calibrate the image */ 19 | float coeffs[5]; /** coefficients describing the distortion model */ 20 | float depth_scale; /** multiply by camera value to get depth in meters */ 21 | 22 | intrinsics() = default; 23 | 24 | intrinsics(rs2_intrinsics intrin, float depth_scale); 25 | 26 | intrinsics(int width, 27 | int height, 28 | float fx, 29 | float fy, 30 | float ppx, 31 | float ppy, 32 | rs2_distortion model, 33 | float coeffs[5], 34 | float depth_scale); 35 | 36 | std::string to_string() const; 37 | 38 | ~intrinsics(); 39 | 40 | 41 | } ss_intrinsics; 42 | } 43 | 44 | #endif //SWAG_SCANNER_CAMERATYPES_H 45 | -------------------------------------------------------------------------------- /test/fileTests/ScanFileHandlerTests.cpp: -------------------------------------------------------------------------------- 1 | //#include 2 | //#include "ScanFileHandler.h" 3 | //#include 4 | // 5 | //using json = nlohmann::json; 6 | //using namespace testing; 7 | // 8 | // 9 | //class FileHandlerFixture : public ::testing::Test { 10 | // 11 | // 12 | //protected: 13 | // file::ScanFileHandler *handler; 14 | // 15 | // virtual void SetUp() { 16 | // handler = new file::ScanFileHandler(); 17 | // } 18 | // 19 | // virtual void TearDown() { 20 | // delete handler; 21 | // } 22 | //}; 23 | // 24 | //// remember to watch out for creating a brand new directory from scratch. See if 25 | //// all folders are being created, files, etc. 26 | // 27 | // 28 | ///** 29 | // * Test to see if passing "true" to the constructor will make a new folder 30 | // * Skip this test when not in use. 31 | // */ 32 | //TEST_F(FileHandlerFixture, TestConstructor2) { 33 | // GTEST_SKIP(); 34 | // file::IFileHandler *handler = new file::ScanFileHandler(true); 35 | //} 36 | // 37 | ///** 38 | // * Test to see if passing a scan name will work. 39 | // */ 40 | //TEST_F(FileHandlerFixture, TestConstructor3) { 41 | // GTEST_SKIP(); 42 | // file::IFileHandler *handler = new file::ScanFileHandler("swagg"); 43 | //} 44 | // 45 | ///** 46 | // * Note: disable this test when not in use. 47 | // * Test setting the current folder to something valid. 48 | // * Also see if it creates new folders within it. 49 | // */ 50 | //TEST_F(FileHandlerFixture, TestSetFolderPath) { 51 | //// handler->set_scan_folder_path("/Users/seanngpack/Programming Stuff"); 52 | //// EXPECT_EQ(handler->get_scan_folder_path(), 53 | //// "/Users/seanngpack/Programming Stuff/Projects/scanner_files/testing"); 54 | //} 55 | // 56 | -------------------------------------------------------------------------------- /src/model/scan/ScanModel.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_SCANMODEL_H 2 | #define SWAG_SCANNER_SCANMODEL_H 3 | 4 | #include "IModel.h" 5 | #include "ScanFileHandler.h" 6 | 7 | namespace pcl { 8 | class PointXYZ; 9 | 10 | template 11 | class PointCloud; 12 | } 13 | 14 | namespace model { 15 | /** 16 | * Represents a model for scanning. 17 | */ 18 | class ScanModel : public IModel { 19 | public: 20 | ScanModel(); 21 | 22 | ~ScanModel() = default; 23 | 24 | 25 | /** 26 | * Set the scan to the input. This triggers the filehandler to set the current working directory 27 | * to the given input. This will also clear any existing clouds in the model. 28 | * 29 | * @param scan_name scan name. 30 | */ 31 | void set_scan(const std::string &scan_name); 32 | 33 | /** 34 | * Save scan cloud. 35 | */ 36 | void save_cloud(const std::string &cloud_name, const CloudType::Type &cloud_type); 37 | 38 | /** 39 | * Save given cloud. 40 | */ 41 | void save_cloud(std::shared_ptr> cloud, 42 | const std::string &cloud_name, 43 | const CloudType::Type &cloud_type); 44 | 45 | /** 46 | * Update the info.json file with the current time, number of degrees, and number of rotations. 47 | * 48 | * @param deg angle in degrees. 49 | * @num_rot number of rotations. 50 | */ 51 | void update_info_json(int deg, int num_rot); 52 | 53 | private: 54 | std::shared_ptr logger; 55 | file::ScanFileHandler file_handler; 56 | 57 | }; 58 | } 59 | 60 | #endif //SWAG_SCANNER_SCANMODEL_H 61 | -------------------------------------------------------------------------------- /src/types/CloudType.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CLOUDTYPE_H 2 | #define SWAG_SCANNER_CLOUDTYPE_H 3 | 4 | #include 5 | 6 | /** 7 | * Enumeration for point calibration states. 8 | * RAW = calibration that has had no processing done to it yet. 9 | * FILTERED = cloud that has bilateral filtering, nan points removed, and point cropping done to it. 10 | * REGISTERED = registered clouds. 11 | * NORMAL = normal clouds. 12 | * CALIBRATION = calibration clouds. 13 | */ 14 | namespace CloudType { 15 | enum class Type { 16 | NONE, 17 | RAW, 18 | FILTERED, 19 | // PROCESSED, 20 | REGISTERED, 21 | // NORMAL, 22 | CALIBRATION 23 | }; 24 | 25 | static const Type All[] = {CloudType::Type::RAW, 26 | CloudType::Type::FILTERED, 27 | // CloudType::Type::PROCESSED, 28 | CloudType::Type::REGISTERED, 29 | // CloudType::Type::NORMAL, 30 | CloudType::Type::CALIBRATION}; 31 | 32 | inline std::string String(CloudType::Type type) { 33 | switch (type) { 34 | case CloudType::Type::RAW: 35 | return "raw"; 36 | case CloudType::Type::FILTERED: 37 | return "filtered"; 38 | // case CloudType::Type::PROCESSED: 39 | // return "processed"; 40 | // case CloudType::Type::NORMAL: 41 | // return "normal"; 42 | case CloudType::Type::REGISTERED: 43 | return "registered"; 44 | case CloudType::Type::CALIBRATION: 45 | return "calibration"; 46 | default: 47 | return "error, enum not defined for cloudtype"; 48 | } 49 | } 50 | } 51 | 52 | #endif //SWAG_SCANNER_CLOUDTYPE_H 53 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # -------------------------------------------------------------------------------- 2 | # Add library 3 | # -------------------------------------------------------------------------------- 4 | add_library(swag_scanner_lib "") 5 | 6 | # -------------------------------------------------------------------------------- 7 | # Link libraries to library 8 | # -------------------------------------------------------------------------------- 9 | # consider changing this to public for spdlog 10 | target_link_libraries(swag_scanner_lib PRIVATE 11 | feeling-blue 12 | ${PCL_LIBRARIES} 13 | ${Boost_PROGRAM_OPTIONS_LIBRARY} 14 | ${realsense2_LIBRARY} 15 | Qt5::Quick Qt5::Core Qt5::Widgets Qt5::Designer 16 | nlohmann_json::nlohmann_json 17 | spdlog::spdlog 18 | ${VTK_LIBRARIES} 19 | calibration_basic_widget 20 | scan_basic_widget 21 | ) 22 | 23 | # -------------------------------------------------------------------------------- 24 | # Add subdirectories 25 | # -------------------------------------------------------------------------------- 26 | add_subdirectory(cli) 27 | add_subdirectory(controller) 28 | add_subdirectory(file) 29 | add_subdirectory(model) 30 | add_subdirectory(types) 31 | add_subdirectory(utils) 32 | add_subdirectory(view) 33 | 34 | 35 | 36 | # -------------------------------------------------------------------------------- 37 | # Add executable and link library 38 | # -------------------------------------------------------------------------------- 39 | add_executable(swag_scanner main.cpp) 40 | target_link_libraries(swag_scanner PRIVATE swag_scanner_lib 41 | Qt5::Quick Qt5::Core Qt5::Widgets Qt5::Designer 42 | spdlog::spdlog 43 | nlohmann_json::nlohmann_json) 44 | -------------------------------------------------------------------------------- /test/calibrationTests/visual/CalibrationTestsVisual.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "Algorithms.h" 6 | #include "Visualizer.h" 7 | 8 | 9 | // This shows visually rotating a calibration about a line. 10 | // This test is currently not working because the folder does not exist. 11 | TEST(CalibrationTestsVisual, VisualizePlaneCalibration) { 12 | std::string folder_path = "/Users/seanngpack/Programming Stuff/Projects/scanner_files/18"; 13 | auto cloudIn = std::make_shared>(); 14 | auto cloudOut = std::make_shared>(); 15 | auto transformed = std::make_shared>(); 16 | pcl::io::loadPCDFile(folder_path + "/filtered/" + "1.pcd", *cloudIn); 17 | pcl::io::loadPCDFile(folder_path + "/filtered/" + "4.pcd", *cloudOut); 18 | // std::vector coefs = mod->get_plane_coefs(cloudIn); 19 | // mod->get_plane_coefs(cloudOut); 20 | 21 | std::vector origin = {-0.000213082, 0.0298714, 0.42673}; 22 | std::vector direction = {-0.0158, -0.8661, -0.4996}; 23 | 24 | // from 1.pcd -> 4.pcd 25 | float theta = 0.15708; 26 | 27 | transformed->resize(cloudIn->size()); 28 | 29 | for (int i = 0; i < cloudIn->size(); i++) { 30 | transformed->points[i] = algos::rotate_point_about_line(cloudIn->points[i], 31 | origin, 32 | direction, 33 | theta); 34 | } 35 | 36 | 37 | visual::Visualizer visualizer; 38 | std::vector>> clouds{cloudOut, transformed}; 39 | visualizer.simpleVis(clouds); 40 | 41 | } -------------------------------------------------------------------------------- /src/controller/calibration/CalibrationControllerGUI.cpp: -------------------------------------------------------------------------------- 1 | #include "CalibrationControllerGUI.h" 2 | #include "CalibrationFileHandler.h" 3 | #include "CalibrationModel.h" 4 | #include "Normal.h" 5 | #include "Point.h" 6 | #include "SR305.h" 7 | #include "Arduino.h" 8 | #include "Visualizer.h" 9 | #include "SwagGUI.h" 10 | #include "FormsPayload.h" 11 | #include "Logger.h" 12 | 13 | controller::CalibrationControllerGUI::CalibrationControllerGUI(std::shared_ptr camera, 14 | std::shared_ptr arduino, 15 | std::shared_ptr model, 16 | std::shared_ptr gui) : 17 | CalibrationController(std::move(camera), 18 | std::move(arduino), 19 | std::move(model)), 20 | IControllerGUI(std::move(gui)) {} 21 | 22 | 23 | void controller::CalibrationControllerGUI::run() { 24 | emit update_console("Starting scan"); 25 | logger::info("[STARTED CALIBRATION SCANNING]"); 26 | scan(); 27 | logger::info("[CALIBRATION SCANNING COMPLETE]"); 28 | emit update_console("Scan complete"); 29 | emit update_console("Calculating center point..."); 30 | model->calculate_center_point(); 31 | emit update_console("Center point calculation complete"); 32 | emit update_console("Refining center point calculation..."); 33 | model->refine_center_point(); 34 | emit update_console("Refined"); 35 | model->update_calibration_json(); 36 | logger::info("[CALIBRATION COMPLETE]"); 37 | emit update_console("Calibration done"); 38 | } 39 | 40 | 41 | void controller::CalibrationControllerGUI::update(const IFormsPayload &payload) { 42 | const auto &p = dynamic_cast(payload); 43 | model->set_calibration(p.name); 44 | this->deg = p.angle; 45 | this->num_rot = p.rotations; 46 | } 47 | -------------------------------------------------------------------------------- /src/controller/calibration/CalibrationController.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CALIBRATIONCONTROLLER_H 2 | #define SWAG_SCANNER_CALIBRATIONCONTROLLER_H 3 | 4 | #include "IController.h" 5 | #include "Plane.h" 6 | #include 7 | 8 | namespace controller { 9 | class ScanController; 10 | } 11 | 12 | namespace file { 13 | class CalibrationFileHandler; 14 | } 15 | 16 | namespace arduino { 17 | class Arduino; 18 | } 19 | 20 | namespace model { 21 | class CalibrationModel; 22 | } 23 | 24 | namespace visual { 25 | class Visualizer; 26 | } 27 | 28 | namespace camera { 29 | class ICamera; 30 | } 31 | 32 | namespace pcl { 33 | class PointXYZ; 34 | 35 | template 36 | class PointCloud; 37 | } 38 | 39 | 40 | namespace controller { 41 | /** 42 | * This controller handles calibration. 43 | */ 44 | class CalibrationController : public virtual IController { 45 | public: 46 | CalibrationController(std::shared_ptr camera, 47 | std::shared_ptr arduino, 48 | std::shared_ptr model); 49 | 50 | /** 51 | * Scan calibration fixture with member info for degs and # of rotations into a new 52 | * calibration folder. Calculate configuration properties and save to that folder. 53 | */ 54 | void run() override; 55 | 56 | void set_deg(int deg); 57 | 58 | void set_num_rot(int rot); 59 | 60 | 61 | protected: 62 | std::shared_ptr camera; 63 | std::shared_ptr arduino; 64 | std::shared_ptr model; 65 | int deg = 15; 66 | int num_rot = 8; 67 | std::vector>> clouds; 68 | 69 | 70 | /** 71 | * Scan the calibration clouds and save them into current folder. 72 | */ 73 | void scan(); 74 | 75 | 76 | }; 77 | } 78 | 79 | #endif //SWAG_SCANNER_CALIBRATIONCONTROLLER_H 80 | -------------------------------------------------------------------------------- /src/controller/calibration/CalibrationController.cpp: -------------------------------------------------------------------------------- 1 | #include "CalibrationController.h" 2 | #include "CalibrationModel.h" 3 | #include "Constants.h" 4 | #include "Point.h" 5 | #include "SR305.h" 6 | #include "Arduino.h" 7 | #include "Visualizer.h" 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | controller::CalibrationController::CalibrationController(std::shared_ptr camera, 14 | std::shared_ptr arduino, 15 | std::shared_ptr model) : 16 | camera(std::move(camera)), arduino(std::move(arduino)), model(std::move(model)) {} 17 | 18 | void controller::CalibrationController::run() { 19 | scan(); 20 | 21 | model->calculate_center_point(); 22 | model->refine_center_point(); 23 | model->update_calibration_json(); 24 | // viewer->ptVis(cloud_vector[0], pcl::PointXYZ(center.x, center.y, center.z)); 25 | } 26 | 27 | void controller::CalibrationController::scan() { 28 | using namespace constants; 29 | 30 | const camera::intrinsics intrin = camera->get_intrinsics(); 31 | for (int i = 0; i < num_rot; i++) { 32 | std::string cloud_name = std::to_string(i * deg) + ".pcd"; 33 | camera->scan(); 34 | std::vector depth_frame = camera->get_depth_frame(); 35 | std::shared_ptr> cloud = camera->create_point_cloud(depth_frame, intrin); 36 | 37 | model->crop_cloud(cloud, cal_min_x, cal_max_x, cal_min_y, cal_max_y, cal_min_z, cal_max_z); 38 | model->bilateral_filter(cloud, 10, .001); 39 | model->voxel_grid_filter(cloud, .001); 40 | model->add_cloud(cloud, cloud_name); 41 | model->save_cloud(cloud_name); 42 | 43 | arduino->rotate_by(deg); 44 | } 45 | arduino->rotate_to(0); 46 | 47 | } 48 | 49 | void controller::CalibrationController::set_deg(int deg) { 50 | this->deg = deg; 51 | } 52 | 53 | void controller::CalibrationController::set_num_rot(int rot) { 54 | this->num_rot = rot; 55 | } 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/model/camera/ICamera.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CPP_ICAMERA_H 2 | #define SWAG_SCANNER_CPP_ICAMERA_H 3 | 4 | #include "CameraTypes.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace pcl { 10 | class PointXYZ; 11 | 12 | template 13 | class PointCloud; 14 | } 15 | 16 | namespace camera { 17 | 18 | /** 19 | * Interface for a camera. Contains methods to get depth maps and return them as 1d vectors. 20 | */ 21 | class ICamera { 22 | public: 23 | 24 | /** 25 | * Get copy of camera intrin. 26 | * @return a pointer to ss_camera struct of the camera intrin. 27 | */ 28 | virtual camera::intrinsics get_intrinsics() = 0; 29 | 30 | /** 31 | * Get copy of intrin after processing is applied. Only applies to realsense 32 | * sensors. 33 | * @return intrin after decimation changes the resolution of image. 34 | */ 35 | virtual camera::intrinsics get_intrinsics_processed() = 0; 36 | 37 | /** 38 | * Get depth image and set to class variable. 39 | */ 40 | virtual void scan() = 0; 41 | 42 | /** 43 | * Get depth frame vector. 44 | * @return the depth map. 45 | */ 46 | virtual std::vector get_depth_frame() = 0; 47 | 48 | /** 49 | * Get the depth frame vector after filtering. 50 | * @return filtered depth frame vector. 51 | */ 52 | virtual std::vector get_depth_frame_processed() = 0; 53 | 54 | /** 55 | * Create new pointcloud given depth frame and intrinsics. 56 | */ 57 | virtual std::shared_ptr> 58 | create_point_cloud(const std::vector &depth_frame, 59 | const camera::intrinsics &intrinsics) = 0; 60 | 61 | /** 62 | * Virtual destructor, must be defined or else it will never call the base class's destructor. 63 | */ 64 | virtual ~ICamera() {} 65 | 66 | 67 | protected: 68 | intrinsics intrin; 69 | }; 70 | 71 | } 72 | 73 | 74 | #endif //SWAG_SCANNER_CPP_ICAMERA_H 75 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.17) 2 | project(swag_scanner) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | # find pcl installed with brew 6 | set(Boost_USE_STATIC_LIBS OFF) 7 | set(Boost_USE_MULTITHREADED ON) 8 | set(Boost_USE_STATIC_RUNTIME OFF) 9 | set(JSON_BuildTests OFF CACHE INTERNAL "") 10 | # these are for qt .ui and .qrc files 11 | set(CMAKE_AUTOMOC ON) 12 | set(CMAKE_AUTOUIC ON) 13 | set(CMAKE_AUTORCC ON) 14 | set(PCL_DIR /usr/local/Cellar/pcl/1.11.1_5/share/pcl-1.11/) 15 | if(NOT Qt5_DIR) 16 | set(Qt5_DIR "/usr/local/opt/qt/lib/cmake/Qt5") 17 | endif() 18 | 19 | option(BUILD_TESTS "Build the tests" OFF) 20 | option(BUILD_TESTS_VISUAL "Build the visual tests" OFF) 21 | 22 | 23 | # -------------------------------------------------------------------------------- 24 | # Find libraries 25 | # -------------------------------------------------------------------------------- 26 | find_package(PCL 1.11 REQUIRED) 27 | find_package(VTK REQUIRED) 28 | find_package(Qt5 COMPONENTS Core Widgets Quick Designer REQUIRED) 29 | find_package(Boost 1.72.0 COMPONENTS program_options REQUIRED) 30 | find_package(OpenCV REQUIRED) 31 | find_package(feeling-blue REQUIRED) 32 | find_package(realsense2 REQUIRED) 33 | find_package(spdlog REQUIRED) 34 | 35 | 36 | add_definitions(${PCL_DEFINITIONS}) 37 | include_directories( 38 | ${PCL_INCLUDE_DIRS} 39 | ${realsense_INCLUDE_DIR}) 40 | 41 | # -------------------------------------------------------------------------------- 42 | # Subdirectories 43 | # -------------------------------------------------------------------------------- 44 | 45 | add_subdirectory(extern/json) 46 | add_subdirectory(extern/SwagScannerWidgets) 47 | include_directories(extern/SwagScannerWidgets/swagscanner_widgets/include) 48 | add_subdirectory(src) 49 | 50 | # -------------------------------------------------------------------------------- 51 | # Build tests 52 | # -------------------------------------------------------------------------------- 53 | 54 | if (BUILD_TESTS) 55 | include(CTest) 56 | enable_testing() 57 | add_subdirectory(${PROJECT_SOURCE_DIR}/extern/googletest) 58 | add_subdirectory(test) 59 | endif () 60 | -------------------------------------------------------------------------------- /src/types/equations/Plane.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_PLANE_H 2 | #define SWAG_SCANNER_PLANE_H 3 | 4 | #include 5 | #include 6 | 7 | namespace equations { 8 | 9 | class Normal; 10 | 11 | /** 12 | * Class represents the equation of a plane. Contains methods to convert vector or pcl::ModelCoefficients 13 | * to this object. 14 | */ 15 | typedef struct Plane { 16 | public: 17 | 18 | double A = 0; 19 | double B = 0; 20 | double C = 0; 21 | double D = 0; 22 | 23 | /** 24 | * Default constructor for plane. Don't forget to set A,B,C,D 25 | */ 26 | Plane() = default; 27 | 28 | /** 29 | * Create a plane object given the coefficients of a plane. 30 | * @param A Coefficient 31 | * @param B Coefficient 32 | * @param C Coefficient 33 | * @param D Coefficient 34 | */ 35 | Plane(double A, double B, double C, double D); 36 | 37 | /** 38 | * Initialize a plane given a vector. 39 | * Note: I can use reference because primitive will get copied anyway. 40 | * @param in vector of four coefficients (A,B,C,D) 41 | * @throws invalid_argument if the vector is missing a coefficient. 42 | */ 43 | Plane(const std::vector &in); 44 | 45 | /** 46 | * Initialize a plane given PCL ModelCoefficients. 47 | * @param in PCL coefficients of four coefficients (A,B,C,D) 48 | */ 49 | Plane(const std::shared_ptr &in); 50 | 51 | /** 52 | * Initialize a plane given PCL ModelCoefficients. 53 | * @param in PCL coefficients of four coefficients (A,B,C,D) 54 | */ 55 | Plane(const pcl::ModelCoefficients &in); 56 | 57 | /** 58 | * Generate normal from this plane. 59 | * @return the normal. 60 | */ 61 | Normal get_normal() const; 62 | 63 | /** 64 | * Overloaded addition + operator. 65 | * @param p2 other plane to add. 66 | * @return new plane, addition of this and other. 67 | */ 68 | Plane operator+(const Plane &p2) const; 69 | 70 | ~Plane() = default; 71 | 72 | private: 73 | 74 | } Plane; 75 | } 76 | 77 | #endif //SWAG_SCANNER_PLANE_H 78 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 35 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 36 | 37 | # User-specific stuff 38 | .idea/**/workspace.xml 39 | .idea/**/tasks.xml 40 | .idea/**/usage.statistics.xml 41 | .idea/**/dictionaries 42 | .idea/**/shelf 43 | 44 | # Generated files 45 | .idea/**/contentModel.xml 46 | 47 | # Sensitive or high-churn files 48 | .idea/**/dataSources/ 49 | .idea/**/dataSources.ids 50 | .idea/**/dataSources.local.xml 51 | .idea/**/sqlDataSources.xml 52 | .idea/**/dynamic.xml 53 | .idea/**/uiDesigner.xml 54 | .idea/**/dbnavigator.xml 55 | 56 | # Gradle 57 | .idea/**/gradle.xml 58 | .idea/**/libraries 59 | 60 | # Gradle and Maven with auto-import 61 | # When using Gradle or Maven with auto-import, you should exclude module files, 62 | # since they will be recreated, and may cause churn. Uncomment if using 63 | # auto-import. 64 | # .idea/artifacts 65 | # .idea/compiler.xml 66 | # .idea/jarRepositories.xml 67 | # .idea/modules.xml 68 | # .idea/*.iml 69 | # .idea/modules 70 | # *.iml 71 | # *.ipr 72 | 73 | # CMake 74 | cmake-build-*/ 75 | 76 | # Mongo Explorer plugin 77 | .idea/**/mongoSettings.xml 78 | 79 | # File-based project format 80 | *.iws 81 | 82 | # IntelliJ 83 | out/ 84 | 85 | # mpeltonen/sbt-idea plugin 86 | .idea_modules/ 87 | 88 | # JIRA plugin 89 | atlassian-ide-plugin.xml 90 | 91 | # Cursive Clojure plugin 92 | .idea/replstate.xml 93 | 94 | # Crashlytics plugin (for Android Studio and IntelliJ) 95 | com_crashlytics_export_strings.xml 96 | crashlytics.properties 97 | crashlytics-build.properties 98 | fabric.properties 99 | 100 | # Editor-based Rest Client 101 | .idea/httpRequests 102 | 103 | # Android studio 3.1+ serialized cache file 104 | .idea/caches/build_file_checksums.ser 105 | 106 | .DS_Store -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | branches: 4 | only: 5 | - master 6 | - model 7 | - gui-implementation 8 | - registration-algo-dev 9 | 10 | matrix: 11 | include: 12 | - os: osx 13 | python: 3.7 14 | osx_image: xcode11.6 15 | cache: 16 | directories: 17 | - $TRAVIS_BUILD_DIR/target 18 | - $HOME/Library/Caches/Homebrew 19 | - /usr/local/Homebrew 20 | - $HOME/local_bottle_metadata 21 | addons: 22 | homebrew: 23 | update: true 24 | packages: 25 | - vtk 26 | - librealsense 27 | - opencv 28 | - spdlog 29 | - pcl 30 | 31 | dist: trusty 32 | 33 | # https://gist.github.com/gfreezy/52cb2afe807d59b821c82a50cd183f4b 34 | before_install: | 35 | if [ -n "$IS_OSX" ]; then 36 | TAPS="$(brew --repository)/Library/Taps" 37 | if [ -e "$TAPS/caskroom/homebrew-cask" -a -e "$TAPS/homebrew/homebrew-cask" ]; then 38 | rm -rf "$TAPS/caskroom/homebrew-cask" 39 | fi 40 | find "$TAPS" -type d -name .git -exec \ 41 | bash -xec ' 42 | cd $(dirname '\''{}'\'') || echo "status: $?" 43 | git clean -fxd || echo "status: $?" 44 | sleep 1 || echo "status: $?" 45 | git status || echo "status: $?"' \; || echo "status: $?" 46 | brew_cache_cleanup 47 | fi 48 | 49 | 50 | before_cache: | 51 | # Cleanup dirs to be cached 52 | set -e; set -x 53 | if [ -n "$IS_OSX" ]; then 54 | # When Taps is cached, this dir causes "Error: file exists" on `brew update` 55 | if [ -e "$(brew --repository)/Library/Taps/homebrew/homebrew-cask/homebrew-cask" ]; then 56 | rm -rf "$(brew --repository)/Library/Taps/homebrew/homebrew-cask/homebrew-cask" 57 | fi 58 | brew_cache_cleanup 59 | fi 60 | set +x; set +e 61 | 62 | #before_install: 63 | # - brew upgrade pcl 64 | 65 | install: 66 | - git clone https://github.com/seanngpack/feeling-blue-cpp 67 | - cd feeling-blue-cpp 68 | - mkdir build 69 | - cd build 70 | - cmake .. 71 | - sudo make install 72 | 73 | 74 | before_script: 75 | # - brew install vtk 76 | # - brew install librealsense 77 | # - brew install --HEAD pcl 78 | # - brew install opencv 79 | - cd ${TRAVIS_BUILD_DIR} 80 | - mkdir build 81 | 82 | script: 83 | - cd build 84 | - cmake -DBUILD_TESTS=ON .. 85 | - make 86 | - ctest 87 | -------------------------------------------------------------------------------- /src/model/arduino/Arduino.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_ARDUINO_H 2 | #define SWAG_SCANNER_ARDUINO_H 3 | 4 | #include 5 | #include "feeling-blue/feeling-blue.h" 6 | #include 7 | #include 8 | 9 | namespace spdlog { 10 | class logger; 11 | } 12 | 13 | namespace arduino { 14 | class Arduino { 15 | public: 16 | 17 | Arduino(); 18 | 19 | /** 20 | * Rotate the scanner table by input. 21 | * + num for CCW 22 | * - num for CW 23 | * @param deg number of degrees to rotate. 24 | */ 25 | void rotate_by(int deg); 26 | 27 | /** 28 | * Rotate to given target. 29 | * @param target position to rotate to. 30 | */ 31 | void rotate_to(int target); 32 | 33 | 34 | ~Arduino() = default; 35 | 36 | 37 | private: 38 | std::shared_ptr logger; 39 | int current_pos; 40 | std::unique_ptr central_manager; 41 | std::shared_ptr arduino; 42 | std::shared_ptr service; 43 | std::shared_ptr rotate_char; 44 | std::shared_ptr table_pos_char; 45 | std::shared_ptr is_table_rot_char; 46 | std::string DEVICE_NAME = "SwagScanner"; 47 | std::string UART_SERVICE_UUID = "5ffba521-2363-41da-92f5-46adc56b2d37"; 48 | std::string ROTATE_TABLE_CHAR_UUID = "5ffba522-2363-41da-92f5-46adc56b2d37"; 49 | std::string TABLE_POSITION_CHAR_UUID = "5ffba523-2363-41da-92f5-46adc56b2d37"; 50 | std::string IS_TABLE_ROTATING_CHAR_UUID = "5ffba524-2363-41da-92f5-46adc56b2d37"; 51 | 52 | std::mutex mtx; 53 | std::condition_variable cv; 54 | bool ready = false; 55 | 56 | /** 57 | * Unlock main thred when is_rotating notification received and the table is no 58 | * longer rotating. 59 | * 60 | * @param data payload from notification. 61 | */ 62 | void handle_rotation_notification(const std::vector &data); 63 | 64 | /** 65 | * Updates the current position in the settings.json file. 66 | * TODO: this is very slow because I'm reading the file, then writing to it. 67 | */ 68 | void update_current_pos(); 69 | 70 | int get_least(int x, int y); 71 | 72 | int bytes_to_int(const std::vector &bytes); 73 | 74 | }; 75 | } 76 | 77 | #endif //SWAG_SCANNER_ARDUINO_H 78 | -------------------------------------------------------------------------------- /src/controller/scan/ScanControllerGUI.cpp: -------------------------------------------------------------------------------- 1 | #include "ScanControllerGUI.h" 2 | #include "FormsPayload.h" 3 | #include "ScanFileHandler.h" 4 | #include "SR305.h" 5 | #include "ScanModel.h" 6 | #include "Arduino.h" 7 | #include "Logger.h" 8 | #include 9 | 10 | controller::ScanControllerGUI::ScanControllerGUI(std::shared_ptr camera, 11 | std::shared_ptr arduino, 12 | std::shared_ptr model, 13 | std::shared_ptr gui) : 14 | ScanController(std::move(camera), 15 | std::move(arduino), 16 | std::move(model)), 17 | IControllerGUI(std::move(gui)) {} 18 | 19 | void controller::ScanControllerGUI::run() { 20 | model->update_info_json(deg, num_rot); 21 | 22 | const camera::intrinsics intrin = camera->get_intrinsics(); 23 | emit update_console("Started scanning..."); 24 | 25 | if (num_rot == 0) { 26 | camera->scan(); 27 | std::vector depth_frame_raw = camera->get_depth_frame(); 28 | std::shared_ptr> cloud_raw = camera->create_point_cloud(depth_frame_raw, intrin); 29 | model->add_cloud(cloud_raw, "0.pcd"); 30 | model->save_cloud("0.pcd", CloudType::Type::RAW); 31 | } 32 | 33 | logger::info("[STARTED SCANNING]"); 34 | for (int i = 0; i < num_rot; i++) { 35 | std::string name = std::to_string(i * deg) + ".pcd"; 36 | // add delay to avoid ghosting 37 | std::chrono::milliseconds timespan(500); 38 | std::this_thread::sleep_for(timespan); 39 | camera->scan(); 40 | std::vector depth_frame_raw = camera->get_depth_frame(); 41 | std::shared_ptr> cloud_raw = camera->create_point_cloud(depth_frame_raw, intrin); 42 | model->add_cloud(cloud_raw, name); 43 | model->save_cloud(name, CloudType::Type::RAW); 44 | arduino->rotate_by(deg); 45 | // add a delay to avoid ghosting 46 | std::this_thread::sleep_for(timespan); 47 | } 48 | logger::info("[SCANNING COMPLETE]"); 49 | emit update_console("Scan complete!"); 50 | } 51 | 52 | void controller::ScanControllerGUI::update(const IFormsPayload &payload) { 53 | const auto &p = dynamic_cast(payload); 54 | model->set_scan(p.name); 55 | this->deg = p.angle; 56 | this->num_rot = p.rotations; 57 | // careful, don't use set_deg() 58 | } 59 | 60 | 61 | -------------------------------------------------------------------------------- /test/calibrationTests/CalibrationTests.cpp: -------------------------------------------------------------------------------- 1 | #define private public 2 | 3 | #include 4 | #include 5 | #include "CalibrationModel.h" 6 | #include "Visualizer.h" 7 | #include "CalibrationModel.h" 8 | #include "Plane.h" 9 | #include "Normal.h" 10 | #include "Point.h" 11 | 12 | 13 | class CalibrationPhysicalFixture : public ::testing::Test { 14 | 15 | protected: 16 | model::CalibrationModel *mod; 17 | equations::Normal g_n = equations::Normal(-0.0158, -0.8661, -0.4996); 18 | std::vector planes = { 19 | equations::Plane(0.8603, -0.2446, 0.4472, -.201376), 20 | equations::Plane(0.7779, -0.3059, 0.5489, -.242753), 21 | equations::Plane(0.6714, -0.3638, 0.6457, -.28239), 22 | equations::Plane(-0.5484, -0.4098, 0.7289, -.316794), 23 | equations::Plane(-0.4123, -0.4443, 0.7954, -.344772), 24 | equations::Plane(-0.2635, -0.4709, 0.8419, -.364747), 25 | equations::Plane(-0.1081, -0.4863, 0.8670, -.376452), 26 | equations::Plane(-0.0492, -0.4900, 0.8703, -.379304), 27 | equations::Plane(-0.2061, -0.4751, 0.8555, -.374743), 28 | equations::Plane(-0.3655, -0.4537, 0.8128, -.359423) 29 | }; 30 | 31 | virtual void SetUp() { 32 | mod = new model::CalibrationModel(); 33 | } 34 | 35 | virtual void TearDown() { 36 | delete mod; 37 | } 38 | }; 39 | 40 | /** 41 | * Make sure the center point calculation is good. 42 | * Add cloud files in this folder later to verify calculation. 43 | */ 44 | TEST_F(CalibrationPhysicalFixture, calculate_center_pt) { 45 | GTEST_SKIP(); 46 | Eigen::MatrixXd A = mod->build_A_matrix(g_n, planes); 47 | Eigen::MatrixXd b = mod->build_b_matrix(g_n, planes); 48 | pcl::PointXYZ pt = mod->calculate_center_point(); 49 | ASSERT_NEAR(pt.x, -0.000213082, .001); 50 | ASSERT_NEAR(pt.y, 0.0298714, .001); 51 | ASSERT_NEAR(pt.z, 0.42673, .001); 52 | } 53 | 54 | 55 | /** 56 | * Test building the A matrix 57 | */ 58 | TEST_F(CalibrationPhysicalFixture, TestBuildAMatrix) { 59 | 60 | Eigen::MatrixXd A = mod->build_A_matrix(g_n, planes); 61 | ASSERT_NEAR(A(0, 0), 0.08251, .001); 62 | ASSERT_NEAR(A(3, 2), -0.0664, .001); 63 | ASSERT_NEAR(A(8, 2), 0.0427, .001); 64 | } 65 | 66 | /** 67 | * Test building the b matrix 68 | */ 69 | TEST_F(CalibrationPhysicalFixture, TestBuildbMatrix) { 70 | 71 | Eigen::MatrixXd b = mod->build_b_matrix(g_n, planes); 72 | ASSERT_NEAR(b(0, 0), -0.0414, .001); 73 | ASSERT_NEAR(b(5, 0), -0.0117, .001); 74 | ASSERT_NEAR(b(8, 0), 0.0153, .001); 75 | } 76 | -------------------------------------------------------------------------------- /test/cameraTests/CameraTests.cpp: -------------------------------------------------------------------------------- 1 | //#include 2 | //#include 3 | //#include 4 | //#include "SR305.h" 5 | //#include "CameraTypes.h" 6 | //#include 7 | // 8 | ///** 9 | // * TODO: implement these tests for the camera, making sure to test pointcloud creation 10 | // */ 11 | // 12 | //class CameraFixture : public ::testing::Test { 13 | // 14 | //protected: 15 | // std::vector depth_frame; 16 | // std::shared_ptr> cloud_ptr; 17 | // 18 | // 19 | // virtual void SetUp() { 20 | // 21 | // mod = new model::Model; 22 | // 23 | // // Set up depth frame 24 | // for (uint16_t i = 0; i < 100; i++) { 25 | // depth_frame.push_back(i); 26 | // } 27 | // 28 | // // set up point calibration 29 | // auto cloud = std::make_shared>(); 30 | // cloud->height = 10; 31 | // cloud->width = 10; 32 | // cloud->is_dense = true; 33 | // cloud->points.resize(10 * 10); 34 | // 35 | // for (int y = 0; y < 10; y++) { 36 | // for (int x = 0; x < 10; x++) { 37 | // cloud->points[y * 10 + x] = pcl::PointXYZ(x, y, 1); 38 | // } 39 | // } 40 | // cloud_ptr = cloud; 41 | // } 42 | // 43 | // float distortion[5] = {.139, .124, .0043, .00067, -.034}; 44 | // camera::intrinsics *intrinsics_distortion = new camera::intrinsics(10, 10, 45 | // 475.07, 475.07, 46 | // 309.931, 245.011, 47 | // RS2_DISTORTION_INVERSE_BROWN_CONRADY, 48 | // distortion, 49 | // 0.0001); 50 | // 51 | // virtual void TearDown() { 52 | // delete mod; 53 | // } 54 | //}; 55 | // 56 | // 57 | ///** 58 | // * Test creating a point calibration. 59 | // */ 60 | //TEST_F(CameraFixture, TestCreatePointCloud) { 61 | // 62 | // 63 | // std::shared_ptr> test = mod->create_point_cloud(depth_frame, *intrinsics_distortion); 64 | // EXPECT_EQ(test->width, 10); 65 | // EXPECT_EQ(test->height, 10); 66 | //} 67 | // 68 | ///** 69 | // * Test creating the normals calibration. 70 | // */ 71 | //TEST_F(CameraFixture, TestEstimateNormals) { 72 | // auto test = mod->create_point_cloud(depth_frame, *intrinsics_distortion); 73 | // mod->estimate_normal_cloud(test); 74 | // 75 | // ASSERT_EQ(test->width, 10); 76 | // ASSERT_EQ(test->height, 10); 77 | //} 78 | // 79 | // 80 | -------------------------------------------------------------------------------- /test/research/calibration/Calibration.cpp: -------------------------------------------------------------------------------- 1 | #define private public 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "Visualizer.h" 12 | #include "CalibrationModel.h" 13 | #include "Normal.h" 14 | #include "Plane.h" 15 | #include "Point.h" 16 | #include "ScanFileHandler.h" 17 | #include "Algorithms.h" 18 | #include "CalibrationFileHandler.h" 19 | 20 | namespace fs = std::filesystem; 21 | 22 | class CalibrationFixture : public ::testing::Test { 23 | 24 | protected: 25 | model::CalibrationModel *mod; 26 | visual::Visualizer *viewer; 27 | 28 | virtual void SetUp() { 29 | mod = new model::CalibrationModel(); 30 | viewer = new visual::Visualizer(); 31 | } 32 | 33 | virtual void TearDown() { 34 | delete mod; 35 | delete viewer; 36 | } 37 | }; 38 | 39 | /** 40 | * Config settings: 41 | * "decimation_magnitude": 2, 42 | "spatial_filter_magnitude": 5, 43 | "spatial_smooth_alpha": 0.6, 44 | "spatial_smooth_delta": 5 45 | */ 46 | TEST_F(CalibrationFixture, CalibrationTests) { 47 | auto fixture_raw = std::make_shared>(); 48 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/registration/data/0.pcd", 49 | *fixture_raw); 50 | auto *cal_file_handler = new file::CalibrationFileHandler(); 51 | std::vector>> cropped_clouds; 52 | std::vector upright_planes; 53 | std::vector ground_planes; 54 | auto clouds = cal_file_handler->load_clouds(CloudType::Type::CALIBRATION); 55 | for (const auto &cloud : clouds) { 56 | std::vector coeffs = mod->get_calibration_planes_coefs(cloud, true); 57 | ground_planes.emplace_back(coeffs[0]); 58 | upright_planes.emplace_back(coeffs[1]); 59 | } 60 | 61 | equations::Normal axis_dir = mod->calculate_axis_dir(ground_planes); 62 | pcl::PointXYZ center = mod->calculate_center_point(axis_dir, upright_planes); 63 | std::cout << axis_dir.A << " " << axis_dir.B << " " << axis_dir.C << std::endl; 64 | std::cout << "found point" << center.x << " " << center.y << " " << center.z << std::endl; 65 | 66 | pcl::PointXYZ projected_pt; 67 | projected_pt = algos::project_point_to_plane(center, 68 | algos::find_point_in_plane(clouds[0], ground_planes[0], .00001), 69 | axis_dir); 70 | 71 | std::cout << "new projected point" << projected_pt << std::endl; 72 | 73 | viewer->ptVis(clouds[0], center); 74 | viewer->ptVis(clouds[0], projected_pt); 75 | } -------------------------------------------------------------------------------- /test/fileTests/ScanFileHandlerPhysicalTests.cpp: -------------------------------------------------------------------------------- 1 | //// in this test suite we use the actual camera to gather depth data and test the file 2 | //// handling capabilities 3 | // 4 | //#include 5 | //#include "ScanFileHandler.h" 6 | // 7 | //using namespace testing; 8 | // 9 | // 10 | //class ScanFileHandlerPhysicalFixture : public ::testing::Test { 11 | // 12 | // 13 | //protected: 14 | // file::ScanFileHandler *handler; 15 | // std::string folder_path = "/Users/seanngpack/Library/Application Support/SwagScanner/scans/scan_handler_test/raw"; 16 | // virtual void SetUp() { 17 | // handler = new file::ScanFileHandler("scan_handler_test"); 18 | // set_up_test_files(); 19 | // } 20 | // 21 | // virtual void TearDown() { 22 | // delete handler; 23 | // } 24 | // 25 | // /** 26 | // * If the folder doesn't exist then make it and save a pointcloud to it. 27 | // * @param folder_path path to the testing folder setup. 28 | // */ 29 | // void set_up_test_files() { 30 | // if (boost::filesystem::is_empty(folder_path)) { 31 | // std::shared_ptr> calibration = set_up_point_cloud(); 32 | // pcl::io::savePCDFileASCII(folder_path + "/" + "test_cloud.pcd", *calibration); 33 | // } 34 | // } 35 | // 36 | // static std::shared_ptr> set_up_point_cloud() { 37 | // // set up point calibration 38 | // std::shared_ptr> calibration(new pcl::PointCloud); 39 | // calibration->height = 10; 40 | // calibration->width = 10; 41 | // calibration->is_dense = true; 42 | // calibration->points.resize(10 * 10); 43 | // 44 | // for (int y = 0; y < 10; y++) { 45 | // for (int x = 0; x < 10; x++) { 46 | // calibration->points[y * 10 + x] = pcl::PointXYZ(x, y, 1); 47 | // } 48 | // } 49 | // return calibration; 50 | // } 51 | //}; 52 | // 53 | // 54 | //TEST_F(ScanFileHandlerPhysicalFixture, TestLoadCloud) { 55 | // // make an empty calibration to calibration the file into. 56 | // std::shared_ptr> calibration(new pcl::PointCloud); 57 | // // load the file 58 | // pcl::io::loadPCDFile(folder_path + "/" + "test_cloud.pcd", *calibration); 59 | // ASSERT_EQ(calibration->height, 10); 60 | // ASSERT_EQ(calibration->width, 10); 61 | // ASSERT_EQ(calibration->size(), 100); 62 | //} 63 | // 64 | ///** 65 | // * Load clouds from a folder 66 | // */ 67 | //TEST_F(ScanFileHandlerPhysicalFixture, TestLoadClouds) { 68 | // // make an empty calibration to calibration the file into. 69 | // std::vector>, 70 | // Eigen::aligned_allocator>> > data; 71 | // handler->load_clouds(data, CloudType::Type::RAW); 72 | // ASSERT_EQ(data.size(), 4); 73 | // 74 | // 75 | //} 76 | -------------------------------------------------------------------------------- /src/controller/manager/ControllerManager.cpp: -------------------------------------------------------------------------------- 1 | #include "ControllerManager.h" 2 | #include "IController.h" 3 | #include "IControllerGUI.h" 4 | #include "SR305.h" 5 | #include "Arduino.h" 6 | #include "CalibrationModel.h" 7 | #include "ScanModel.h" 8 | #include "ProcessingModel.h" 9 | #include "Visualizer.h" 10 | #include "CalibrationFileHandler.h" 11 | #include "CalibrationController.h" 12 | #include "ProcessingController.h" 13 | #include "ProcessingControllerGUI.h" 14 | #include "ScanController.h" 15 | #include "ScanControllerGUI.h" 16 | #include "MoveController.h" 17 | #include "MoveControllerGUI.h" 18 | #include "CalibrationControllerGUI.h" 19 | #include "EditingControllerGUI.h" 20 | #include "HomeController.h" 21 | #include "ControllerManagerCache.h" 22 | #include "MoveController.h" 23 | #include "SwagGUI.h" 24 | #include "Logger.h" 25 | 26 | 27 | namespace po = boost::program_options; 28 | 29 | controller::ControllerManager::ControllerManager() : cache(std::make_unique(this)) {} 30 | 31 | controller::ControllerManager::~ControllerManager() { 32 | logger::debug("ControllerManger ~destructor called"); 33 | } 34 | 35 | std::shared_ptr controller::ControllerManager::get_controller(const po::variables_map &vm) { 36 | if (vm.count("scan")) { 37 | return cache->get_scan_controller(vm); 38 | } else if (vm.count("calibrate")) { 39 | return cache->get_calibration_controller(vm); 40 | } else if (vm.count("process")) { 41 | return cache->get_process_controller(vm); 42 | } else if (vm.count("move")) { 43 | return cache->get_move_controller(vm); 44 | } else if (vm.count("set_home")) { 45 | return cache->get_home_controller(vm); 46 | } else { 47 | throw std::invalid_argument("Error, must enter a valid base command."); 48 | } 49 | } 50 | 51 | std::shared_ptr controller::ControllerManager::get_controller(const std::string &name) { 52 | if (name == "scan") { 53 | return cache->get_scan_controller(); 54 | } else if (name == "calibrate") { 55 | return cache->get_calibration_controller(); 56 | } else if (name == "process") { 57 | return cache->get_process_controller(); 58 | } else { 59 | throw std::invalid_argument("Error, must enter a valid base command."); 60 | } 61 | } 62 | 63 | std::shared_ptr controller::ControllerManager::get_gui_controller(const std::string &name) { 64 | if (name == "scan") { 65 | return cache->get_scan_controller_gui(); 66 | } else if (name == "calibrate") { 67 | return cache->get_calibration_controller_gui(); 68 | } else if (name == "move") { 69 | return cache->get_move_controller_gui(); 70 | } else if (name == "process") { 71 | return cache->get_process_controller_gui(); 72 | } else if (name == "edit") { 73 | return cache->get_edit_controller_gui(); 74 | } else { 75 | throw std::invalid_argument("Error, must enter a valid controller name."); 76 | } 77 | } 78 | 79 | std::shared_ptr controller::ControllerManager::get_gui() { 80 | return cache->get_gui(); 81 | } 82 | -------------------------------------------------------------------------------- /src/view/Visualizer.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #ifndef SWAG_SCANNER_VISUALIZER_H 6 | #define SWAG_SCANNER_VISUALIZER_H 7 | 8 | namespace pcl { 9 | namespace visualization { 10 | class PointPickingEvent; 11 | } 12 | } 13 | 14 | namespace visual { 15 | class Visualizer { 16 | public: 17 | 18 | static void pointPickingEventOccurred(const pcl::visualization::PointPickingEvent& event, void* viewer_void); 19 | 20 | /** 21 | * Visualize one pointcloud 22 | * @param cloud pointcloud you want to visualize 23 | */ 24 | static void simpleVis(const std::shared_ptr> &cloud); 25 | 26 | 27 | /** 28 | * Visualize pointclouds. first calibration is white, then successive clouds are red and lighter shades of red. 29 | * @param clouds vector of pointcloud you want to visualize 30 | */ 31 | static void simpleVis(const std::vector>> &clouds); 32 | 33 | static void simpleVisColor(const std::vector>> &clouds); 34 | 35 | /** 36 | * Visualize a calibration and a point. 37 | * @param cloud the calibration. 38 | * @param pt the point. 39 | */ 40 | static void ptVis(const std::shared_ptr> &cloud, const pcl::PointXYZ &pt); 41 | 42 | /** 43 | * Visualize two point clouds side by side. 44 | * @param cloud1 45 | * @param cloud2 46 | */ 47 | static void compareVis(const std::shared_ptr> &cloud1, 48 | const std::shared_ptr> &cloud2); 49 | 50 | /** 51 | * Visualize four points clouds in a grid. 52 | * @param cloud1 53 | * @param cloud2 54 | * @param cloud3 55 | * @param cloud4 56 | */ 57 | static void compareVisFour(const std::shared_ptr> &cloud1, 58 | const std::shared_ptr> &cloud2, 59 | const std::shared_ptr> &cloud3, 60 | const std::shared_ptr> &cloud4, 61 | const std::string &cloud1_desc = "calibration 1", 62 | const std::string &cloud2_desc = "calibration 2", 63 | const std::string &cloud3_desc = "calibration 3", 64 | const std::string &cloud4_desc = "calibration 4"); 65 | 66 | /** 67 | * Visualize normal vectors. 68 | * @param cloud base calibration. 69 | * @param normal normal calibration for base calibration. 70 | */ 71 | static void normalsVis(const std::shared_ptr> &cloud, 72 | const std::shared_ptr> &normal); 73 | 74 | }; 75 | } 76 | 77 | #endif //SWAG_SCANNER_VISUALIZER_H 78 | -------------------------------------------------------------------------------- /src/model/arduino/Arduino.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "IFileHandler.h" 3 | #include 4 | #include 5 | 6 | using json = nlohmann::json; 7 | using namespace std::literals::chrono_literals; 8 | 9 | arduino::Arduino::Arduino() : logger(spdlog::get("backend_logger")) { 10 | current_pos = file::IFileHandler::load_swag_scanner_info_json()["current_position"]; 11 | 12 | // connect to peripheral, service, chars... 13 | central_manager = std::make_unique(); 14 | central_manager->start_bluetooth(); 15 | arduino = central_manager->find_peripheral(DEVICE_NAME); 16 | service = arduino->find_service(UART_SERVICE_UUID); 17 | rotate_char = service->find_characteristic(ROTATE_TABLE_CHAR_UUID); 18 | table_pos_char = service->find_characteristic(TABLE_POSITION_CHAR_UUID); 19 | is_table_rot_char = service->find_characteristic(IS_TABLE_ROTATING_CHAR_UUID); 20 | 21 | using namespace std::placeholders; 22 | // subscribe to is_table_rotating characteristic 23 | std::function &)> binded_handler = std::bind( 24 | &Arduino::handle_rotation_notification, this, std::placeholders::_1); 25 | is_table_rot_char->notify(binded_handler); 26 | 27 | logger->info("Finished setting up Arduino bluetooth connections"); 28 | } 29 | 30 | void arduino::Arduino::handle_rotation_notification(const std::vector &data) { 31 | if (bytes_to_int(data) == 0) { 32 | std::unique_lock lock(mtx); 33 | ready = true; 34 | cv.notify_all(); 35 | } 36 | } 37 | 38 | void arduino::Arduino::rotate_by(int deg) { 39 | rotate_char->write_with_response(deg); 40 | std::unique_lock lock(mtx); 41 | while (!ready) { 42 | cv.wait(lock); 43 | } 44 | 45 | if (deg < 0) { 46 | deg += 360; 47 | } 48 | current_pos += deg; 49 | current_pos %= 360; 50 | update_current_pos(); 51 | ready = false; 52 | } 53 | 54 | void arduino::Arduino::rotate_to(int target) { 55 | if (current_pos < target) { 56 | int forward = target - current_pos; 57 | int back = 360 + current_pos - target; 58 | rotate_by(get_least(forward, back)); 59 | } else if (current_pos > target) { 60 | int forward = 360 + target - current_pos; 61 | int back = current_pos - target; 62 | rotate_by(get_least(forward, back)); 63 | } 64 | } 65 | 66 | 67 | void arduino::Arduino::update_current_pos() { 68 | json settings_json = file::IFileHandler::load_swag_scanner_info_json(); 69 | settings_json["current_position"] = current_pos; 70 | file::IFileHandler::write_swag_scanner_info_json(settings_json); 71 | } 72 | 73 | 74 | int arduino::Arduino::get_least(int x, int y) { 75 | if (x <= y) { 76 | return x; 77 | } else { 78 | return -y; 79 | } 80 | } 81 | 82 | int arduino::Arduino::bytes_to_int(const std::vector &bytes) { 83 | if (bytes.size() == 4) { 84 | return int((unsigned char) (bytes[0]) << 24 | 85 | (unsigned char) (bytes[1]) << 16 | 86 | (unsigned char) (bytes[2]) << 8 | 87 | (unsigned char) (bytes[3])); 88 | } 89 | return 0; 90 | } 91 | 92 | 93 | -------------------------------------------------------------------------------- /src/file/CalibrationFileHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CALIBRATIONFILEHANDLER_H 2 | #define SWAG_SCANNER_CALIBRATIONFILEHANDLER_H 3 | 4 | #include "IFileHandler.h" 5 | 6 | namespace equations { 7 | class Normal; 8 | 9 | class Point; 10 | } 11 | 12 | namespace file { 13 | 14 | class CalibrationFileHandler : public IFileHandler { 15 | public: 16 | 17 | /** 18 | * Construstor creates a file handler object. Will point to the latest calibration. 19 | */ 20 | CalibrationFileHandler(); 21 | 22 | /** 23 | * If the create flag is true, then a new calibration scan folder will be created 24 | * by alphanumeric order. 25 | * @param auto_create_flag true if you want to automatically create a new scan folder. 26 | */ 27 | CalibrationFileHandler(bool auto_create_flag); 28 | 29 | /** 30 | * Points to the given calibration scan name. If the calibration does not exist then 31 | * create the folder. 32 | * @param scan_name calibration scan that you want to point to. 33 | */ 34 | CalibrationFileHandler(const char *scan_name); 35 | 36 | /** 37 | * Calibration folder will be created and name will be be assigned based on previous calibrations in 38 | * alphanumeric order. 39 | */ 40 | void auto_create_new_calibration(); 41 | 42 | /** 43 | * Point to the given calibration. If the calibration does not exist then create a new one. 44 | * @param cal_name name of the calibration. 45 | */ 46 | void set_calibration(const std::string &cal_name); 47 | 48 | // TODO: Later refactor this and load calibration... They don't need to take in CloudTypes 49 | void save_cloud(const std::shared_ptr> &cloud, 50 | const std::string &cloud_name, 51 | const CloudType::Type &cloud_type) override; 52 | 53 | std::shared_ptr> load_cloud(const std::string &cloud_name, 54 | const CloudType::Type &cloud_type) override; 55 | 56 | std::vector>> load_clouds( 57 | const CloudType::Type &cloud_type) override; 58 | 59 | /** 60 | * Update the calibration .json file in the current scan folder with the 61 | * axis direction and center point. 62 | * @param dir axis direction. 63 | * @param pt center point. 64 | */ 65 | void update_calibration_json(const equations::Normal &dir, const equations::Point &pt); 66 | 67 | void update_calibration_json(const equations::Normal &dir, const pcl::PointXYZ &pt); 68 | 69 | private: 70 | 71 | /** 72 | * Create a calibration .json file in the current calibration folder with 0 as default parameter. 73 | */ 74 | void create_calibration_json(); 75 | 76 | /** 77 | * Get the calibration file for the current calibration scan. 78 | * @return the calibration .json file. 79 | */ 80 | nlohmann::json get_calibration_json(); 81 | }; 82 | } 83 | 84 | #endif //SWAG_SCANNER_CALIBRATIONFILEHANDLER_H 85 | -------------------------------------------------------------------------------- /src/utils/Logger.h: -------------------------------------------------------------------------------- 1 | // https://github.com/gabime/spdlog/wiki/How-to-use-spdlog-in-DLLs 2 | #ifndef SWAG_SCANNER_LOGGER_H 3 | #define SWAG_SCANNER_LOGGER_H 4 | 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace logger { 16 | 17 | static std::shared_ptr _tp = std::make_shared(51200, 4); 18 | static auto dist_sink = std::make_shared( 19 | std::vector({})); 20 | 21 | 22 | inline std::shared_ptr setup_default_logger() { 23 | auto default_logger = spdlog::stdout_logger_mt("default_logger"); 24 | return default_logger; 25 | } 26 | 27 | 28 | inline std::shared_ptr setup_file_logger() { 29 | auto logger = std::make_shared("backend_logger", 30 | dist_sink); 31 | // [Oct 20 2020] some logging message 32 | logger->flush_on(spdlog::level::info); 33 | // logger->set_pattern("[%b %d %Y] %v"); 34 | return logger; 35 | } 36 | 37 | inline std::shared_ptr get_file_logger() { 38 | return spdlog::get("backend_logger"); 39 | } 40 | 41 | /** 42 | * Change location of output file of the file logger. 43 | * Make sure to put the entire path + file. 44 | * Example: "/user/files/swagscanner/scans/scan1/info/basic_logger.txt" 45 | * Note: may exist a better way to do this, kinda inefficient to keep spawning new sinks. 46 | * 47 | * @param path full path to the new log text file. 48 | */ 49 | inline void set_file_logger_location(const std::string &path) { 50 | auto new_sink = std::make_shared(path); 51 | dist_sink->set_sinks(std::vector({new_sink})); 52 | } 53 | 54 | /** 55 | * Write an info level message. Write to both file and console. If the file sink hasn't been set up, 56 | * then only write to console. 57 | * @param message message you want to write. 58 | */ 59 | inline void info(const std::string &message) { 60 | auto default_logger = spdlog::get("default_logger"); 61 | auto file_logger = spdlog::get("backend_logger"); 62 | if (file_logger != nullptr) { 63 | file_logger->info(message); 64 | } 65 | default_logger->info(message); 66 | } 67 | 68 | inline void debug(const std::string &message) { 69 | auto default_logger = spdlog::get("default_logger"); 70 | auto file_logger = spdlog::get("backend_logger"); 71 | if (file_logger != nullptr) { 72 | file_logger->debug(message); 73 | } 74 | default_logger->debug(message); 75 | } 76 | 77 | inline void error(const std::string &message) { 78 | auto default_logger = spdlog::get("default_logger"); 79 | auto file_logger = spdlog::get("backend_logger"); 80 | if (file_logger != nullptr) { 81 | file_logger->error(message); 82 | } 83 | default_logger->error(message); 84 | } 85 | 86 | } 87 | 88 | #endif //SWAG_SCANNER_LOGGER_H 89 | -------------------------------------------------------------------------------- /src/model/processing/ProcessingModel.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_PROCESSINGMODEL_H 2 | #define SWAG_SCANNER_PROCESSINGMODEL_H 3 | 4 | #include "IModel.h" 5 | #include "ScanFileHandler.h" 6 | 7 | namespace spdlog { 8 | class logger; 9 | } 10 | 11 | namespace model { 12 | class ProcessingModel : public IModel { 13 | public: 14 | ProcessingModel(); 15 | 16 | ~ProcessingModel() = default; 17 | 18 | std::shared_ptr> load_cloud(const std::string &name, 19 | const CloudType::Type type); 20 | 21 | /** 22 | * Set the scan to the input. This triggers the filehandler to set the current working directory 23 | * to the given input. This will also clear any existing clouds in the model. 24 | * 25 | * @param scan_name scan name. 26 | */ 27 | void set_scan(const std::string &scan_name); 28 | 29 | /** 30 | * Save scan cloud. 31 | */ 32 | void save_cloud(const std::string &cloud_name, const CloudType::Type &cloud_type); 33 | 34 | /** 35 | * Save given cloud. 36 | */ 37 | void save_cloud(std::shared_ptr> cloud, 38 | const std::string &cloud_name, 39 | const CloudType::Type &cloud_type); 40 | 41 | 42 | /** 43 | * Use ICP registration between two clouds. 44 | * 45 | * @param cloud_src cloud source. 46 | * @param cloud_target cloud target. 47 | * @param transformed_cloud copy of source -> target. 48 | * @return matrix transformation of source -> target. 49 | */ 50 | Eigen::Matrix4f icp_register_pair_clouds(const std::shared_ptr> &cloud_src, 51 | const std::shared_ptr> &cloud_target, 52 | std::shared_ptr> &transformed_cloud); 53 | 54 | /** 55 | * Do cropping, Run bilateral filter, Remove NaN points, remove outliers. 56 | * 57 | * @param sigma_s filter window for bilateral filter. 58 | * @param sigma_r standard deviation of the gaussian for bilateral filter. 59 | * @param mean_k number of neighbors to analyze. 60 | * @param thresh_mult multipler for standard deviation, members outside st will be removed. 61 | */ 62 | void filter(int sigma_s = 10, 63 | float sigma_r = .01, 64 | int mean_k = 50, 65 | float thresh_mult = 1); 66 | 67 | /** 68 | * Transform clouds to world coordinate. 69 | * Loads the latest calibration for data used in transformation. 70 | * TODO: modify this so processModel can accept any calibration instead of just the latest. 71 | * 72 | * @throws runtime_error if the clouds vector is not loaded yet. 73 | */ 74 | void transform_clouds_to_world(); 75 | 76 | /** 77 | * Perform registration on clouds. Rotates them by scanning angle to original cloud. 78 | */ 79 | void register_clouds(); 80 | 81 | 82 | private: 83 | std::shared_ptr logger; 84 | file::ScanFileHandler file_handler; 85 | 86 | }; 87 | } 88 | 89 | #endif //SWAG_SCANNER_PROCESSINGMODEL_H 90 | -------------------------------------------------------------------------------- /src/view/gui/SwagGUI.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Sean ng pack on 9/22/20. 3 | // 4 | 5 | #ifndef SWAG_SCANNER_SWAGGUI_H 6 | #define SWAG_SCANNER_SWAGGUI_H 7 | 8 | #include "MoveMethod.h" 9 | #include 10 | #include 11 | #include 12 | 13 | class QThreadPool; 14 | 15 | namespace controller { 16 | class ControllerManager; 17 | 18 | class IControllerGUI; 19 | } 20 | 21 | namespace Ui { 22 | class SwagGUI; 23 | } 24 | 25 | namespace pcl 26 | { 27 | class PointXYZ; 28 | template class PointCloud; 29 | } 30 | template 31 | using foo = boost::shared_ptr>; 32 | 33 | class SwagGUI : public QMainWindow { 34 | Q_OBJECT 35 | 36 | public: 37 | explicit SwagGUI(QWidget *parent = 0, controller::ControllerManager *manager = nullptr); 38 | 39 | ~SwagGUI(); 40 | 41 | /** 42 | * Show the cloud in the vtk viewer. Does not support modifications yet. 43 | */ 44 | void display_cloud(const std::shared_ptr> &cloud); 45 | 46 | public Q_SLOTS: 47 | 48 | // calibration slots 49 | void on_calNameEdit_textChanged(const QString &text); 50 | 51 | void on_calDropdownBasic_angleSliderValueChanged(int value); 52 | 53 | void on_calDropdownBasic_rotationSliderValueChanged(int value); 54 | 55 | void on_runCalButton_clicked(); 56 | 57 | // scan slots 58 | 59 | void on_scanNameEdit_textChanged(const QString &text); 60 | 61 | void on_scanDropdownBasic_angleSliderValueChanged(int value); 62 | 63 | void on_scanDropdownBasic_rotationSliderValueChanged(int value); 64 | 65 | void on_runScanButton_clicked(); 66 | 67 | // process slots 68 | 69 | void on_processNameEdit_textChanged(const QString &text); 70 | 71 | void on_runProcessButton_clicked(); 72 | 73 | // table slots 74 | 75 | /** 76 | * Edited does not call signal when set programatically. Need this behavior to link 77 | * the moveTo and moveBy signals to each other. 78 | */ 79 | void on_moveByEdit_textEdited(const QString &text); 80 | 81 | void on_moveToEdit_textEdited(const QString &text); 82 | 83 | void on_moveButton_clicked(); 84 | 85 | void on_setHomeButton_clicked(); 86 | 87 | // edit slots 88 | 89 | void on_openProjectButton_clicked(); 90 | 91 | void on_saveProjectButton_clicked(); 92 | 93 | // menu slots 94 | void on_setupMenuButton_clicked(); 95 | 96 | void on_scanMenuButton_clicked(); 97 | 98 | void on_calibrateMenuButton_clicked(); 99 | 100 | void on_processMenuButton_clicked(); 101 | 102 | void on_editMenuButton_clicked(); 103 | 104 | 105 | 106 | protected: 107 | std::shared_ptr viewer; 108 | 109 | private: 110 | QThreadPool *thread_pool; 111 | 112 | Ui::SwagGUI *ui; 113 | controller::ControllerManager *manager; 114 | 115 | // scan_tab component state variables 116 | QString scan_name; 117 | int scan_angle; 118 | int scan_rotations; 119 | 120 | // cal_tab component state variables 121 | QString cal_name; 122 | int cal_angle; 123 | int cal_rotations; 124 | 125 | // process_tab component state varibles 126 | QString process_name; 127 | 128 | // table_tab copmopnent state variables 129 | MoveMethod move_method; 130 | int move_deg; 131 | 132 | }; 133 | 134 | 135 | #endif //SWAG_SCANNER_SWAGGUI_H 136 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SwagScanner 2 | [![HitCount](http://hits.dwyl.com/{seanngpack}/{swag-scanner}.svg)](http://hits.dwyl.com/{seanngpack}/{swag-scanner}) 3 | [![Issues](https://img.shields.io/github/issues-raw/tterb/PlayMusic.svg?maxAge=25000)](https://github.com/seanngpack/swag-scanner/issues) 4 | [![Build Status](https://travis-ci.com/seanngpack/swag-scanner-cpp.svg?branch=master)](https://travis-ci.com/seanngpack/swag-scanner-cpp) 5 | [![MIT License](https://img.shields.io/apm/l/atomic-design-ui.svg?)](https://github.com/seanngpack/swag-scanner/master/LICENSE) 6 | 7 | ![Test Image 1](doc/img/swaggg.png) 8 | 9 | SwagScanner is a device that can scan your small/medium-sized objects into the virtual world. The system is designed 10 | and built from the ground up. I designed the software pipeline to be extensible, robust, and fast. SwagScanner is 11 | compatible with any depth camera--just verify its minimum scanning distance and acquire the correct mounting hardware 12 | and API and you're good to go. You can find more information about the design 13 | on [my website](https://www.seanngpack.com/swagscanner/). 14 | 15 | ![sponge](doc/img/sponge.png) 16 | 17 | 18 | ### Status 19 | This is the C++ codebase of Swag Scanner. Everything is highly subject to change. 20 | 21 | Hardware: Currently working on physical model complete overhaul. Designing for manufacturing. 22 | 23 | Software: Improving GUI, fine tuning registration algos, hand-writing filtering methods 24 | 25 | 26 | ### Dependencies 27 | 28 |
29 | Click to see dependencies 30 | 31 | * pcl 1.11 32 | ``` 33 | $ brew install pcl 34 | ``` 35 | 36 | Or compile from source 37 | 38 | ``` 39 | $ git clone https://github.com/PointCloudLibrary/pcl 40 | $ cd pcl 41 | $ mkdir build 42 | $ cd build 43 | $ cmake .. 44 | $ sudo make install 45 | ``` 46 | 47 | * opencv4 48 | ``` 49 | brew install opencv 50 | ``` 51 | 52 | * librealsense 53 | ``` 54 | $ brew install librealsense 55 | ``` 56 | 57 | Or compile from source 58 | ``` 59 | $ git clone https://github.com/IntelRealSense/librealsense 60 | $ cd librealsense 61 | $ mkdir build 62 | $ cd build 63 | $ cmake .. 64 | $ sudo make install 65 | ``` 66 | 67 | * Qt 68 | 69 | ``` 70 | $ brew install qt 71 | ``` 72 | 73 | * spdlog 74 | 75 | ``` 76 | $ brew install spdlog 77 | ``` 78 | 79 | * feeling-blue-cpp 80 | ``` 81 | $ git clone https://github.com/seanngpack/feeling-blue-cpp 82 | $ cd feeling-blue 83 | $ mkdir build 84 | $ cd build 85 | $ cmake .. 86 | $ sudo make install 87 | ``` 88 | 89 |
90 | 91 | ### GUI 92 | 93 | SwagScanner has a GUI now! 94 | 95 | ![GUI image](doc/img/GUI.png) 96 | 97 | ### CLI Commands 98 | 99 | SwagScanner can also be accessed through the commandline. 100 | 101 | | --scan | --calibrate | --process | --move | 102 | |-----------------|-----------------|-----------------|--------------| 103 | | --name (string) | --name (string) | --name (string) | --to (int) | 104 | | --deg (int) | --deg (int) | | --by (int) | 105 | | --rot (int) | --rot (int) | | --home (int) | 106 | 107 | Examples: 108 | 109 | Start a scan called "scan1" at 15 degree intervals. 110 | 111 | ```--scan --name scan1 --deg 15``` 112 | 113 | Run a calibration with an autogenerated name (because you didn't pass --name) at 8 degree intervals 12 times 114 | 115 | ```--calibrate --deg 8 --rot 12``` 116 | 117 | 118 | ### Authors 119 | 120 | * **Sean Ng Pack** - [seanngpack.com](https://www.seanngpack.com) 121 | * **Albert Enyedy** - [github](https://github.com/aenyedy56) 122 | 123 | 124 | ### License 125 | 126 | This project is licensed under the MIT License 127 | -------------------------------------------------------------------------------- /src/model/camera/SR305.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_SR305_H 2 | #define SWAG_SCANNER_SR305_H 3 | 4 | #include "ICamera.h" 5 | 6 | namespace spdlog { 7 | class logger; 8 | } 9 | 10 | namespace camera { 11 | 12 | /** 13 | * SR305 camera implementation. Belongs to intel realsense family of depth-sensing cameras. 14 | */ 15 | class SR305 : public ICamera { 16 | 17 | public: 18 | SR305(); 19 | 20 | /** 21 | * Release camera resources at shutdown. 22 | */ 23 | ~SR305(); 24 | 25 | 26 | intrinsics get_intrinsics() override; 27 | 28 | intrinsics get_intrinsics_processed() override; 29 | 30 | void scan() override; 31 | 32 | std::vector get_depth_frame() override; 33 | 34 | /** 35 | * Get the processed depth frame. Subsampling and spatial filtering applied. 36 | * @return processed depth frame. 37 | */ 38 | std::vector get_depth_frame_processed() override; 39 | 40 | /** 41 | * Start the pipeline. 42 | */ 43 | void start_pipe(); 44 | 45 | /** 46 | * Stop the pipeline. 47 | */ 48 | void stop_pipe(); 49 | 50 | std::shared_ptr> 51 | create_point_cloud(const std::vector &depth_frame, 52 | const camera::intrinsics &intrinsics) override; 53 | 54 | /** 55 | * Divide resolution by magnitude. 56 | * @param mag [2 - 8] default = 2 57 | */ 58 | virtual void set_decimation_magnitude(int mag); 59 | 60 | /** 61 | * # of filter iterations; 62 | * @param mag [1 - 5] default = 2; 63 | */ 64 | virtual void set_spatial_filter_magnitude(int mag); 65 | 66 | /** 67 | * Alpha factor in exponential moving average. 68 | * alpha = 1, no filter 69 | * alpha = 0, infinite filter 70 | * lower = more aggressive smoothing, edges become more rounded. 71 | * @param a [.25 - 1], default = .5 72 | */ 73 | virtual void set_spatial_smooth_alpha(float a); 74 | 75 | /** 76 | * Step size boundary, establishes threshold used to preserve "edges". 77 | * If the depth value between neighboring pixels exceed the depth threshold set by this delta parameter, 78 | * then alpha will be temporarily reset to 1 (no filtering). This basically means that if an edge is observed, 79 | * then the smoothing is temporarily turned off. 80 | * @param d [1 - 50], default = 20 81 | */ 82 | virtual void set_spatial_smooth_delta(int d); 83 | 84 | 85 | private: 86 | rs2::device dev; 87 | rs2::pipeline pipe; 88 | rs2::pipeline_profile pipe_profile; 89 | rs2::decimation_filter dec_filter; 90 | rs2::spatial_filter spat_filter; 91 | rs2::hole_filling_filter hole_filter; 92 | rs2::frame current_frame; 93 | float depth_scale; 94 | 95 | // decimation filter parameters 96 | int decimation_magnitude; 97 | 98 | // spatial edge-preservation filter parameters 99 | int spatial_filter_magnitude; 100 | float spatial_smooth_alpha; 101 | int spatial_smooth_delta; 102 | 103 | 104 | int width = 640; 105 | int height = 480; 106 | 107 | /** 108 | * Initialize the pipeline and grab camera parameters for class fields. 109 | */ 110 | void initialize_camera(); 111 | 112 | /** 113 | * Get depth frame. 114 | * @return rs2 frame. 115 | */ 116 | rs2::frame get_rs2_frame(); 117 | 118 | 119 | }; 120 | 121 | 122 | } 123 | 124 | #endif //SWAG_SCANNER_SR305_H 125 | -------------------------------------------------------------------------------- /src/file/ScanFileHandler.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Sean ng pack on 7/8/20. 3 | // 4 | 5 | #ifndef SWAG_SCANNER_SCANFILEHANDLER_H 6 | #define SWAG_SCANNER_SCANFILEHANDLER_H 7 | 8 | #include "IFileHandler.h" 9 | 10 | namespace file { 11 | /** 12 | * Represents a file handler for scanning related processes. 13 | */ 14 | class ScanFileHandler : public IFileHandler { 15 | public: 16 | 17 | /** 18 | * Default constructor makes a ScanFileHandler. Searches for SwagScanner in the /applications path 19 | * and creates a new SwagScanner directory if it doesn't exist. Will also create a folder "1" under 20 | * the /data directory and set it as the current scan folder. * Then it will update the settings/info.json on the 21 | * latest scan. If SwagScanner exists, then it will use the current scan folder according to the 22 | * settings/info.json file. 23 | */ 24 | ScanFileHandler(); 25 | 26 | /** 27 | * Constructor takes in a flag and determines whether to create a new scan folder 28 | * or not. If the flag is set to true then it will create a new scan folder numerically. 29 | * Then it will update the settings/info.json on the latest scan. 30 | * @param auto_create_flag 31 | */ 32 | ScanFileHandler(bool auto_create_flag); 33 | 34 | /** 35 | * Crates a scan folder with the given scan name. If the scan is already there, 36 | * then just set the current scan folder to it and do not touch anything inside the directory. 37 | * 38 | * @param scan_name name of the scan. 39 | */ 40 | ScanFileHandler(const char *scan_name); 41 | 42 | /** 43 | * Scan folder will be created and name will be be assigned based on previous scans in 44 | * alphanumeric order. 45 | */ 46 | void auto_create_new_scan(); 47 | 48 | /** 49 | * Point to the given scan. If the scan does not exist then create a new one. If the input is empty, then return. 50 | * 51 | * @param scan name of the scan. 52 | */ 53 | void set_scan(const std::string &scan_name); 54 | 55 | void save_cloud(const std::shared_ptr> &cloud, 56 | const std::string &cloud_name, 57 | const CloudType::Type &cloud_type) override; 58 | 59 | std::shared_ptr> load_cloud(const std::string &cloud_name, 60 | const CloudType::Type &cloud_type) override; 61 | 62 | std::vector>> load_clouds( 63 | const CloudType::Type &cloud_type) override; 64 | 65 | /** 66 | * Get the latest calibration json file. 67 | * Finds the latest calibration file via info.json. 68 | * @return the calibration json. 69 | */ 70 | nlohmann::json get_calibration_json(); 71 | 72 | /** 73 | * Get the info.json file. 74 | * @return json file. 75 | */ 76 | nlohmann::json get_info_json(); 77 | 78 | /** 79 | * Update the json file in the latest scan with the given info. 80 | * @param date current date and time. 81 | * @param angle angle intervals of the scan. 82 | * @param cal calibration path. 83 | */ 84 | void update_info_json(const std::string &date, 85 | int angle, 86 | int num_rot, 87 | const std::string &cal = "None"); 88 | 89 | private: 90 | 91 | 92 | /** 93 | * Create the sub folders defined in CloudTypes in the scan_folder_path if they 94 | * don't exist. 95 | * Also creates a /calibration folder with a calibration_info.txt file 96 | */ 97 | void create_sub_folders(); 98 | 99 | /** 100 | * Finds the last scan folder using the settings/info.json file. 101 | * TODO: Might wanna move this to IFilehandler. Also might wanna not depend on the json file to accomplish this. 102 | * 103 | * @return path to the latest scan. 104 | */ 105 | std::filesystem::path find_latest_scan(); 106 | 107 | 108 | /** 109 | * Update the settings/info.json file "latest_scan" field. 110 | */ 111 | void set_swag_scanner_info_latest_scan(const std::filesystem::path &folder_path); 112 | 113 | }; 114 | } 115 | 116 | #endif //SWAG_SCANNER_SCANFILEHANDLER_H 117 | -------------------------------------------------------------------------------- /test/utilsTests/AlgosTests.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "Algorithms.h" 3 | #include "Visualizer.h" 4 | #include "CameraTypes.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | class AlgosFixture : public ::testing::Test { 12 | 13 | protected: 14 | camera::intrinsics *intrinsics_no_distoration; 15 | camera::intrinsics *intrinsics_distoration; 16 | std::vector frame; 17 | 18 | virtual void SetUp() { 19 | float no_distortion[5] = {0, 0, 0, 0, 0}; 20 | float distortion[5] = {.139, .124, .0043, .00067, -.034}; 21 | intrinsics_no_distoration = new camera::intrinsics(640, 480, 22 | 475.07, 475.07, 23 | 309.931, 245.011, 24 | RS2_DISTORTION_INVERSE_BROWN_CONRADY, 25 | no_distortion, 26 | 0.0001); 27 | intrinsics_distoration = new camera::intrinsics(640, 480, 28 | 475.07, 475.07, 29 | 309.931, 245.011, 30 | RS2_DISTORTION_INVERSE_BROWN_CONRADY, distortion, 31 | 0.0001); 32 | 33 | 34 | for (float i = 0; i < intrinsics_no_distoration->width * intrinsics_no_distoration->height; i++) { 35 | frame.push_back(1); 36 | } 37 | } 38 | 39 | virtual void TearDown() { 40 | delete intrinsics_no_distoration; 41 | delete intrinsics_distoration; 42 | } 43 | 44 | 45 | }; 46 | 47 | /** 48 | * Tests deprojection method to see if a point is being made correctly and see if the math 49 | * is good. 50 | */ 51 | TEST_F(AlgosFixture, TestDeprojectNoDistortion) { 52 | pcl::PointXYZ actual = algos::deproject_pixel_to_point(10, 10, 100, *intrinsics_no_distoration); 53 | 54 | pcl::PointXYZ expected; 55 | expected.x = -.0063134059; 56 | expected.y = -.0049468707; 57 | expected.z = .01; 58 | 59 | ASSERT_FLOAT_EQ(expected.x, actual.x); 60 | ASSERT_FLOAT_EQ(expected.y, actual.y); 61 | ASSERT_FLOAT_EQ(expected.z, actual.z); 62 | } 63 | 64 | /** 65 | * Tests deprojection method with distortion coefficients. 66 | */ 67 | TEST_F(AlgosFixture, TestDeprojectDistortion) { 68 | pcl::PointXYZ actual = algos::deproject_pixel_to_point(10, 10, 100, *intrinsics_distoration); 69 | 70 | pcl::PointXYZ expected; 71 | expected.x = -.0063134059; 72 | expected.y = -.0049468707; 73 | expected.z = .01; 74 | 75 | ASSERT_FLOAT_EQ(expected.x, actual.x); 76 | ASSERT_FLOAT_EQ(expected.y, actual.y); 77 | ASSERT_FLOAT_EQ(expected.z, actual.z); 78 | } 79 | 80 | /** 81 | * Given center of bed point, axis of rotation, transform the calibration into the world 82 | * coordinate frame!!! 83 | * Then visualize it. 84 | */ 85 | TEST_F(AlgosFixture, TestTransformCoordinate) { 86 | GTEST_SKIP(); 87 | std::string folder_path = "/Users/seanngpack/Library/Application Support/SwagScanner/calibration/test5/12.pcd"; 88 | auto cloud_in = std::make_shared>(); 89 | auto result = std::make_shared>(); 90 | auto result_cropped = std::make_shared>(); 91 | pcl::PointXYZ pt(-0.006283042311759926, 92 | 0.014217784268003741, 93 | 0.4304016110847342); 94 | 95 | pcl::io::loadPCDFile(folder_path, *cloud_in); 96 | 97 | //point -> origin so I flipped the signs 98 | // NOTE I FLIPPED THE SIGNS OF ORIGINAL 99 | Eigen::Vector3f trans_vec(0.006283042311759926, 100 | -0.014217784268003741, 101 | -0.4304016110847342); 102 | Eigen::Translation translation(trans_vec); 103 | 104 | float a_dot_b = Eigen::Vector3f(-0.0020733693898364438, 105 | -0.8143288642168045, 106 | -0.5803786158561707).dot( 107 | Eigen::Vector3f(0, 0, 1)); 108 | float angle = acos(a_dot_b); 109 | std::cout << (180 / 3.1415) * angle << std::endl; 110 | // rotate about the x axis to align the z axis together 111 | Eigen::AngleAxis rot(-angle, Eigen::Vector3f(1, 112 | 0, 113 | 0)); 114 | 115 | // apply A onto B 116 | Eigen::Transform combined = 117 | rot * translation; 118 | std::cout << combined.matrix() << std::endl; 119 | pcl::transformPointCloud(*cloud_in, *result, combined.matrix()); 120 | 121 | 122 | } 123 | 124 | 125 | -------------------------------------------------------------------------------- /src/model/processing/ProcessingModel.cpp: -------------------------------------------------------------------------------- 1 | #include "ProcessingModel.h" 2 | #include "Normal.h" 3 | #include "Algorithms.h" 4 | #include "Constants.h" 5 | #include "Logger.h" 6 | #include 7 | #include 8 | 9 | using json = nlohmann::json; 10 | 11 | model::ProcessingModel::ProcessingModel() : file_handler() {} 12 | 13 | std::shared_ptr> model::ProcessingModel::load_cloud(const std::string &name, 14 | const CloudType::Type type) { 15 | return file_handler.load_cloud(name, type); 16 | } 17 | 18 | void model::ProcessingModel::set_scan(const std::string &scan_name) { 19 | file_handler.set_scan(scan_name); 20 | // TODO: probably decouple loading clouds from this method 21 | clouds = file_handler.load_clouds(CloudType::Type::RAW); 22 | // TODO: dont forget to assign cloud names to the map 23 | } 24 | 25 | void model::ProcessingModel::save_cloud(const std::string &cloud_name, const CloudType::Type &cloud_type) { 26 | auto cloud = clouds[clouds_map[cloud_name]]; 27 | file_handler.save_cloud(cloud, cloud_name, cloud_type); 28 | } 29 | 30 | void model::ProcessingModel::save_cloud(std::shared_ptr> cloud, 31 | const std::string &cloud_name, 32 | const CloudType::Type &cloud_type) { 33 | file_handler.save_cloud(cloud, cloud_name, cloud_type); 34 | } 35 | 36 | void model::ProcessingModel::filter(int sigma_s, 37 | float sigma_r, 38 | int mean_k, 39 | float thresh_mult) { 40 | using namespace constants; 41 | int clouds_vector_size = clouds.size(); 42 | for (int i = 0; i < clouds_vector_size; i++) { 43 | crop_cloud(clouds[i], 44 | scan_min_x, scan_max_x, 45 | scan_min_y, scan_max_y, 46 | scan_min_z, scan_max_z); 47 | bilateral_filter(clouds[i], sigma_s, sigma_r); 48 | remove_nan(clouds[i]); 49 | remove_outliers(clouds[i]); 50 | // do cloud saving here, try to get the name from the map 51 | add_cloud(clouds[i], std::to_string(i) + ".pcd"); 52 | save_cloud(clouds[i], std::to_string(i) + ".pcd", CloudType::Type::FILTERED); 53 | } 54 | } 55 | 56 | void model::ProcessingModel::transform_clouds_to_world() { 57 | if (clouds.empty()) { 58 | throw std::runtime_error("Cannot perform transformation, must load clouds first."); 59 | } 60 | json calibration_json = file_handler.get_calibration_json(); 61 | std::vector temp0 = calibration_json["axis_direction"].get>(); 62 | equations::Normal rot_axis(temp0); 63 | auto temp = calibration_json["origin_point"].get>(); 64 | pcl::PointXYZ center_pt(temp[0], temp[1], temp[2]); 65 | 66 | for (auto &cloud: clouds) { 67 | Eigen::Matrix4f transform = algos::calc_transform_to_world_matrix(center_pt, rot_axis); 68 | pcl::transformPointCloud(*cloud, *cloud, transform); 69 | } 70 | } 71 | 72 | void model::ProcessingModel::register_clouds() { 73 | json info_json = file_handler.get_info_json(); 74 | int angle = info_json["angle"]; 75 | 76 | auto global_cloud = std::make_shared>(); 77 | *global_cloud = *clouds[0]; 78 | pcl::PointCloud rotated; 79 | for (int i = 1; i < clouds.size(); i++) { 80 | rotated = algos::rotate_cloud_about_z_axis(clouds[i], angle * i); 81 | *global_cloud += rotated; 82 | } 83 | remove_outliers(global_cloud, 50, 1); 84 | add_cloud(global_cloud, "REGISTERED.pcd"); 85 | save_cloud(global_cloud, "REGISTERED.pcd", CloudType::Type::REGISTERED); 86 | } 87 | 88 | Eigen::Matrix4f 89 | model::ProcessingModel::icp_register_pair_clouds(const std::shared_ptr> &cloud_src, 90 | const std::shared_ptr> &cloud_target, 91 | std::shared_ptr> &transformed_cloud) { 92 | pcl::IterativeClosestPoint icp; 93 | icp.setInputSource(cloud_src); 94 | icp.setInputTarget(cloud_target); 95 | icp.setMaximumIterations(100); 96 | icp.setTransformationEpsilon(1e-10); 97 | icp.setMaxCorrespondenceDistance(.05); // not really sure how this affects results 98 | icp.setEuclideanFitnessEpsilon(.0001); // big effect 99 | icp.setRANSACOutlierRejectionThreshold(.0001); // doesn't seem to affect results much 100 | logger::info("ICP registering clouds..."); 101 | icp.align(*transformed_cloud); 102 | if (icp.hasConverged()) { 103 | logger::info("ICP has converged, score is: " + std::to_string(icp.getFitnessScore())); 104 | auto trans = icp.getFinalTransformation().cast(); 105 | std::stringstream ss; 106 | ss << trans; 107 | logger::info(ss.str()); 108 | } else { 109 | PCL_ERROR ("\nICP has not converged.\n"); 110 | } 111 | return icp.getFinalTransformation(); 112 | } 113 | 114 | -------------------------------------------------------------------------------- /src/controller/manager/ControllerManagerCache.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CONTROLLERMANAGERCACHE_H 2 | #define SWAG_SCANNER_CONTROLLERMANAGERCACHE_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camera { 8 | class SR305; 9 | } 10 | 11 | namespace arduino { 12 | class Arduino; 13 | } 14 | 15 | namespace model { 16 | class CalibrationModel; 17 | 18 | class ScanModel; 19 | 20 | class ProcessingModel; 21 | } 22 | 23 | namespace visual { 24 | class Visualizer; 25 | } 26 | 27 | namespace file { 28 | class CalibrationFileHandler; 29 | 30 | class ScanFileHandler; 31 | } 32 | 33 | class SwagGUI; 34 | 35 | namespace controller { 36 | class ScanController; 37 | 38 | class ScanControllerGUI; 39 | 40 | class CalibrationController; 41 | 42 | class CalibrationControllerGUI; 43 | 44 | class ProcessingController; 45 | 46 | class ProcessingControllerGUI; 47 | 48 | class FilterTestingController; 49 | 50 | class EditingControllerGUI; 51 | 52 | class HomeController; 53 | 54 | class MoveController; 55 | 56 | class MoveControllerGUI; 57 | 58 | class ControllerManager; 59 | } 60 | 61 | 62 | namespace controller { 63 | /** 64 | * This manages objects needed to create controllers. 65 | */ 66 | class ControllerManagerCache { 67 | public: 68 | /** 69 | * Default constructor preallocates expensive objects that are probably gonna be used. 70 | * Preallocated objects: Model, ScanFileHandler, CalibrationFileHandler 71 | */ 72 | ControllerManagerCache(ControllerManager *factory); 73 | 74 | ~ControllerManagerCache(); 75 | 76 | // -------------------------------------------------------------------------------- 77 | // Get objects 78 | // -------------------------------------------------------------------------------- 79 | 80 | std::shared_ptr get_camera(); 81 | 82 | std::shared_ptr get_arduino(); 83 | 84 | std::shared_ptr get_calibration_model(); 85 | 86 | std::shared_ptr get_scan_model(); 87 | 88 | std::shared_ptr get_processing_model(); 89 | 90 | std::shared_ptr get_gui(); 91 | 92 | // -------------------------------------------------------------------------------- 93 | // Get controllers 94 | // -------------------------------------------------------------------------------- 95 | 96 | std::shared_ptr 97 | get_scan_controller(const boost::program_options::variables_map &vm); 98 | 99 | std::shared_ptr get_scan_controller(); 100 | 101 | std::shared_ptr get_scan_controller_gui(); 102 | 103 | 104 | std::shared_ptr 105 | get_calibration_controller(const boost::program_options::variables_map &vm); 106 | 107 | std::shared_ptr get_calibration_controller(); 108 | 109 | std::shared_ptr get_calibration_controller_gui(); 110 | 111 | std::shared_ptr 112 | get_process_controller(const boost::program_options::variables_map &vm); 113 | 114 | std::shared_ptr get_process_controller(); 115 | 116 | std::shared_ptr get_process_controller_gui(); 117 | 118 | std::shared_ptr get_edit_controller_gui(); 119 | 120 | std::shared_ptr 121 | get_move_controller(const boost::program_options::variables_map &vm); 122 | 123 | std::shared_ptr get_move_controller_gui(); 124 | 125 | std::shared_ptr 126 | get_home_controller(const boost::program_options::variables_map &vm); 127 | 128 | private: 129 | ControllerManager *factory; 130 | 131 | std::shared_ptr camera; 132 | std::shared_ptr arduino; 133 | std::shared_ptr calibration_model; 134 | std::shared_ptr scan_model; 135 | std::shared_ptr processing_model; 136 | 137 | // controllers 138 | std::shared_ptr scan_controller; 139 | std::shared_ptr calibration_controller; 140 | std::shared_ptr process_controller; 141 | std::shared_ptr move_controller; 142 | 143 | //gui 144 | std::shared_ptr gui; 145 | 146 | // gui controllers 147 | std::shared_ptr scan_controller_gui; 148 | std::shared_ptr calibration_controller_gui; 149 | std::shared_ptr move_controller_gui; 150 | std::shared_ptr process_controller_gui; 151 | std::shared_ptr edit_controller_gui; 152 | 153 | 154 | }; 155 | } 156 | 157 | #endif //SWAG_SCANNER_CONTROLLERMANAGERCACHE_H 158 | -------------------------------------------------------------------------------- /src/model/calibration/CalibrationModel.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_CALIBRATIONMODEL_H 2 | #define SWAG_SCANNER_CALIBRATIONMODEL_H 3 | 4 | #include "IModel.h" 5 | #include "CalibrationFileHandler.h" 6 | #include "Normal.h" 7 | #include 8 | #include 9 | 10 | namespace file { 11 | class CalibrationFileHandler; 12 | } 13 | 14 | namespace equations { 15 | class Normal; 16 | 17 | class Point; 18 | 19 | class Plane; 20 | } 21 | 22 | namespace model { 23 | /** 24 | * Represents a calibration model to do calibration processing on the clouds. 25 | */ 26 | class CalibrationModel : public IModel { 27 | 28 | public: 29 | 30 | /** 31 | * Initialize file handler in here. 32 | */ 33 | CalibrationModel(); 34 | 35 | /** 36 | * Set the calibration name. This triggers the filehandler to set the current working directory 37 | * to the given input. This will also clear any existing clouds in the model. 38 | * 39 | * @param cal_name calibration name. 40 | */ 41 | void set_calibration(const std::string &cal_name); 42 | 43 | /** 44 | * Save calibration to the current calibration folder. 45 | */ 46 | void save_cloud(const std::string &cloud_name); 47 | 48 | 49 | /** 50 | * Calculate the center point of the turntable. 51 | * Using vector of clouds, find the ground and upright planes, get the axis of rotation, 52 | * build A and b matrices, and finally solve for the center point x using in Ax = b using SVD. 53 | * 54 | * @param axis_dir axis of rotation direction. 55 | * @param upright_planes use the upright planes in calculation. 56 | * @return center point of turntable from the camera origin in meters. 57 | */ 58 | pcl::PointXYZ calculate_center_point(); 59 | 60 | /** 61 | * Overloaded method also accepts axis direction planes instead of using model's. 62 | * 63 | * @param axis_dir direction of rotation axis. 64 | * @param upright_planes vector of upright plane equations. 65 | * @return 66 | */ 67 | pcl::PointXYZ calculate_center_point(const equations::Normal &axis_dir, 68 | const std::vector &upright_planes); 69 | 70 | 71 | /** 72 | * Project center point to ground plane. 73 | * 74 | * @param cloud the calibration that the plane you are projecting to belongs to. 75 | * @param pt point you want to project. 76 | * @param delta the threshold to search for point on plane. 77 | * @return projected point on the plane. 78 | * @throws if the center point has not been calculated yet. 79 | */ 80 | pcl::PointXYZ refine_center_point(double delta = .00001); 81 | 82 | /** 83 | * Update calibration json with axis of rotation and center point info. 84 | */ 85 | void update_calibration_json(); 86 | 87 | 88 | private: 89 | file::CalibrationFileHandler file_handler; 90 | std::vector ground_planes; 91 | std::vector upright_planes; 92 | pcl::PointXYZ center_point; 93 | equations::Normal axis_of_rotation; 94 | 95 | 96 | /** 97 | * Get the upright and ground plane equations. 98 | * Utilizes a hardcoded axis and RANSAC normals to find the ground plane. It finds a plane that is within 99 | * the epsilon angle deviation to robustly find the ground plane. Then it will calculate the normals 100 | * and find the perpendicular plane which should be the upright pane. 101 | * 102 | * @param cloud calibration calibration. 103 | * @param visual_flag flag whether to visualize segmentation or not. 104 | * @return vector of upright and ground plane equations. 105 | */ 106 | std::vector 107 | get_calibration_planes_coefs(const std::shared_ptr> &cloud, 108 | bool visual_flag = false); 109 | 110 | /** 111 | * Calculate the A matrix in Ax = b 112 | * @param g_n normal vector to the ground plane. 113 | * @param upright_planes vector of plane equations for the upright planes. 114 | * @return the A matrix (N-1, 3) 115 | */ 116 | Eigen::MatrixXd build_A_matrix(const equations::Normal &g_n, 117 | const std::vector &upright_planes); 118 | 119 | /** 120 | * Calculate the b matrix in Ax = b 121 | * @param g_n normal vector to the ground plane. 122 | * @param upright_planes vector of plane equations for the upright planes. 123 | * @return the b matrix (1, N-1) 124 | */ 125 | Eigen::MatrixXd build_b_matrix(const equations::Normal &g_n, 126 | const std::vector &upright_planes); 127 | 128 | /** 129 | * Given a vector of ground planes, get the average of their normals to get the rotation axis direction. 130 | * 131 | * @param ground_planes planes to calculate wiht. 132 | * @return rotation axis direction. 133 | */ 134 | equations::Normal calculate_axis_dir(const std::vector &ground_planes); 135 | 136 | 137 | }; 138 | 139 | 140 | } 141 | 142 | #endif //SWAG_SCANNER_CALIBRATIONMODEL_H 143 | -------------------------------------------------------------------------------- /src/file/CalibrationFileHandler.cpp: -------------------------------------------------------------------------------- 1 | #include "CalibrationFileHandler.h" 2 | #include "Normal.h" 3 | #include "Point.h" 4 | #include "Logger.h" 5 | #include 6 | #include 7 | 8 | namespace fs = std::filesystem; 9 | using json = nlohmann::json; 10 | 11 | file::CalibrationFileHandler::CalibrationFileHandler() { 12 | scan_folder_path = find_latest_calibration(); 13 | scan_name = scan_folder_path.stem().string(); 14 | logger = logger::get_file_logger(); 15 | logger::set_file_logger_location(get_scan_path() + "/log.txt"); 16 | } 17 | 18 | file::CalibrationFileHandler::CalibrationFileHandler(bool auto_create_flag) { 19 | if (auto_create_flag) { 20 | auto_create_new_calibration(); 21 | } else { 22 | scan_folder_path = find_latest_calibration(); 23 | scan_name = scan_folder_path.stem().string(); 24 | } 25 | logger = logger::get_file_logger(); 26 | logger::set_file_logger_location(get_scan_path() + "/log.txt"); 27 | } 28 | 29 | file::CalibrationFileHandler::CalibrationFileHandler(const char *scan_name) { 30 | set_calibration((std::string) scan_name); 31 | logger = logger::get_file_logger(); 32 | logger::set_file_logger_location(get_scan_path() + "/log.txt"); 33 | } 34 | 35 | void file::CalibrationFileHandler::auto_create_new_calibration() { 36 | scan_folder_path = find_next_scan_folder_numeric(CloudType::Type::CALIBRATION); 37 | scan_name = scan_folder_path.stem().string(); 38 | create_directory(scan_folder_path); 39 | } 40 | 41 | void file::CalibrationFileHandler::set_calibration(const std::string &cal_name) { 42 | if (cal_name.empty()) { 43 | return; 44 | } 45 | scan_name = cal_name; 46 | scan_folder_path = swag_scanner_path / "calibration" / cal_name; 47 | if (!is_directory(scan_folder_path)) { 48 | create_directory(scan_folder_path); 49 | create_calibration_json(); 50 | } 51 | logger::set_file_logger_location(get_scan_path() + "/log.txt"); 52 | } 53 | 54 | void file::CalibrationFileHandler::save_cloud(const std::shared_ptr> &cloud, 55 | const std::string &cloud_name, 56 | const CloudType::Type &cloud_type) { 57 | fs::path out_path = scan_folder_path / cloud_name; 58 | pcl::io::savePCDFileASCII(out_path.string(), *cloud); 59 | logger::info("saved cloud: " + cloud_name + " of type: " + CloudType::String(cloud_type)); 60 | } 61 | 62 | std::shared_ptr> file::CalibrationFileHandler::load_cloud(const std::string &cloud_name, 63 | const CloudType::Type &cloud_type) { 64 | std::shared_ptr> cloud; 65 | fs::path open_path = scan_folder_path / cloud_name; 66 | if (pcl::io::loadPCDFile(open_path.string(), *cloud) == -1) { 67 | PCL_ERROR ("Couldn't read file \n"); 68 | } 69 | return cloud; 70 | } 71 | 72 | std::vector>> file::CalibrationFileHandler::load_clouds( 73 | const CloudType::Type &cloud_type) { 74 | std::vector>> cloud_vector; 75 | std::vector cloud_paths; 76 | fs::path load_path = scan_folder_path; 77 | 78 | // load paths into cloud_paths vector 79 | for (const auto &p : fs::directory_iterator(load_path)) { 80 | // extension must be .pcd and must have number in the filename 81 | if (p.path().extension() == ".pcd" && p.path().string().find_first_of("0123456789") != std::string::npos) { 82 | cloud_paths.push_back(p.path()); 83 | } 84 | } 85 | 86 | // sort the paths numerically 87 | std::sort(cloud_paths.begin(), cloud_paths.end(), path_sort); 88 | 89 | // finally we load the clouds into the cloud_vector 90 | logger::info("loading clouds from: " + load_path.string()); 91 | for (auto &p : cloud_paths) { 92 | auto cloud = std::make_shared>(); 93 | if (pcl::io::loadPCDFile(p.string(), *cloud) == -1) { 94 | PCL_ERROR ("Couldn't read file \n"); 95 | } 96 | cloud_vector.push_back(cloud); 97 | } 98 | return cloud_vector; 99 | } 100 | 101 | 102 | void file::CalibrationFileHandler::update_calibration_json(const equations::Normal &dir, const equations::Point &pt) { 103 | json calibration_json = get_calibration_json(); 104 | calibration_json["axis_direction"] = {dir.A, dir.B, dir.C}; 105 | calibration_json["origin_point"] = {pt.x, pt.y, pt.z}; 106 | 107 | std::ofstream updated_file(scan_folder_path / fs::path(scan_name + ".json")); 108 | updated_file << std::setw(4) << calibration_json << std::endl; // write to file 109 | } 110 | 111 | void file::CalibrationFileHandler::update_calibration_json(const equations::Normal &dir, const pcl::PointXYZ &pt) { 112 | update_calibration_json(dir, equations::Point(pt.x, pt.y, pt.z)); 113 | } 114 | 115 | void file::CalibrationFileHandler::create_calibration_json() { 116 | std::ofstream calibration(scan_folder_path / fs::path(scan_name + ".json")); // create json file 117 | json calibration_json = { 118 | {"origin_point", {0.0, 0.0, 0.0}}, 119 | {"axis_direction", {0.0, 0.0, 0.0}} 120 | }; 121 | calibration << std::setw(4) << calibration_json << std::endl; // write to file 122 | } 123 | 124 | json file::CalibrationFileHandler::get_calibration_json() { 125 | std::ifstream calibration(scan_folder_path / fs::path(scan_name + ".json")); 126 | json calibration_json; 127 | calibration >> calibration_json; 128 | return calibration_json; 129 | } 130 | 131 | 132 | -------------------------------------------------------------------------------- /src/utils/Algorithms.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_ALGORITHMS_H 2 | #define SWAG_SCANNER_ALGORITHMS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace camera { 11 | class intrinsics; 12 | } 13 | 14 | namespace equations { 15 | class Normal; 16 | 17 | class Plane; 18 | 19 | class Point; 20 | } 21 | 22 | /** 23 | * Assortment of hand-made algos for drop&play. 24 | * List may not be big because I may implement them directly by the 25 | * function definitions. 26 | */ 27 | namespace algos { 28 | 29 | /** 30 | * Given three points, deproject their pixel coordinates to space coordinates and 31 | * then save to a PointXYZ format. 32 | * @param x pixel x. 33 | * @param y pixel y. 34 | * @param z depth (unconverted). 35 | * @return a PointXYZ object with the deprojected point in real space. 36 | */ 37 | pcl::PointXYZ deproject_pixel_to_point(float x_pixel, 38 | float y_pixel, 39 | float z, 40 | const camera::intrinsics &intrinsics); 41 | 42 | /** 43 | * Project point to plane 44 | * 45 | * @param pt point you want to project. 46 | * @param plane_pt a point on the plane. 47 | * @param plane the plane you want to project onto. 48 | * @param normal normal of the plane. 49 | * @return projected point on the plane. 50 | */ 51 | pcl::PointXYZ project_point_to_plane(const pcl::PointXYZ &pt, 52 | const pcl::PointXYZ &plane_pt, 53 | const equations::Normal &normal); 54 | 55 | /** 56 | * Find a point lying on the given plane in the calibration. 57 | * 58 | * @param cloud calibration. 59 | * @param plane plane. 60 | * @param delta error threshold for finding the point. 61 | * @return point in the plane or point of 0,0,0. 62 | */ 63 | pcl::PointXYZ find_point_in_plane(const std::shared_ptr> &cloud, 64 | const equations::Plane &plane, 65 | double delta = .00001); 66 | 67 | /** 68 | * Check if a point is inside a plane with given threshold. 69 | * 70 | * @param pt point you want to check. 71 | * @param plane plane the pt belongs to. 72 | * @param delta error threshold. 73 | * @return true if the point is in the plane, false otherwise. 74 | */ 75 | bool check_point_in_plane(const pcl::PointXYZ &pt, 76 | const equations::Plane &plane, 77 | double delta); 78 | 79 | 80 | /** 81 | * Rotate a point calibration about a line. 82 | * 83 | * @param cloud the calibration you want to rotate. Must be an unorganized calibration. 84 | * @param pt a point on the axis you want to rotate about. 85 | * @param line_direction direction vector for the line (normalized) 86 | * @param theta angle in radians you want to rotate. 87 | * @return the rotated calibration. 88 | */ 89 | pcl::PointCloud 90 | rotate_cloud_about_line(const std::shared_ptr> &cloud, 91 | const std::vector &pt, 92 | const std::vector &line_direction, 93 | float theta); 94 | 95 | /** 96 | * Given a copy of a point from the pointcloud, a point that a line passes through, 97 | * and a direction vector, rotate the pointcloud point about that line and return 98 | * a copy of the new point. 99 | * Equation derived by Glenn Murray. 100 | * 101 | * @param point the point you want to rotate. 102 | * @param line_point point on the line. 103 | * @param line_direction direction vector (normalized) of the axis. 104 | * @return a new rotated point. 105 | */ 106 | pcl::PointXYZ rotate_point_about_line(const pcl::PointXYZ &point, 107 | const std::vector &line_point, 108 | const std::vector &line_direction, 109 | float theta); 110 | 111 | 112 | /** 113 | * Rotate calibration about the z-axis. 114 | * 115 | * @param cloud calibration to rotate. 116 | * @param theta rotation degree. 117 | * @return 118 | */ 119 | pcl::PointCloud 120 | rotate_cloud_about_z_axis(const std::shared_ptr> &cloud, 121 | float theta); 122 | 123 | /** 124 | * Calculate the transformation matrix from center of turntable to origin (0,0,0). 125 | * 126 | * @param center center of turntable. 127 | * @param ground_normal normal vector of ground plane. 128 | * @return matrix of the transformation. 129 | */ 130 | Eigen::Matrix4f calc_transform_to_world_matrix(const pcl::PointXYZ ¢er, 131 | const equations::Normal &ground_normal); 132 | 133 | /** 134 | * Transform cloud to world coordinate and return a copy. 135 | */ 136 | void transform_cloud_to_world(const std::shared_ptr> &cloud, 137 | const pcl::PointXYZ ¢er, 138 | const equations::Normal &ground_normal); 139 | 140 | 141 | /** 142 | * Given a vector of planes, average them. 143 | * 144 | * @param planes planes. 145 | * @return average of the planes. 146 | */ 147 | equations::Plane average_planes(const std::vector &planes); 148 | } 149 | 150 | #endif //SWAG_SCANNER_ALGORITHMS_H 151 | -------------------------------------------------------------------------------- /test/research/segmentation/Segmentation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "Visualizer.h" 16 | #include "ProcessingModel.h" 17 | #include "Normal.h" 18 | #include "Plane.h" 19 | #include "Point.h" 20 | #include "Constants.h" 21 | #include "ScanFileHandler.h" 22 | 23 | namespace fs = std::filesystem; 24 | 25 | class SegmentationFixture : public ::testing::Test { 26 | 27 | protected: 28 | model::ProcessingModel *mod; 29 | visual::Visualizer *viewer; 30 | 31 | virtual void SetUp() { 32 | mod = new model::ProcessingModel(); 33 | viewer = new visual::Visualizer(); 34 | } 35 | 36 | virtual void TearDown() { 37 | delete mod; 38 | delete viewer; 39 | } 40 | }; 41 | 42 | /** 43 | * Config settings: 44 | * "decimation_magnitude": 2, 45 | "spatial_filter_magnitude": 5, 46 | "spatial_smooth_alpha": 0.6, 47 | "spatial_smooth_delta": 5 48 | 49 | * This test is to test calibration plane segmentation. 50 | */ 51 | TEST_F(SegmentationFixture, TestCalibrationPlaneSegmentation) { 52 | auto cloud = std::make_shared>(); 53 | auto cloud_plane = std::make_shared>(); 54 | auto cloud_normals = std::make_shared>(); 55 | 56 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/segmentation/data/0.pcd", *cloud);; 57 | cloud = (mod->crop_cloud_cpy(cloud, -.10, .10, -100, .11, -100, .49)); 58 | 59 | // calculate the normals 60 | pcl::NormalEstimation ne; 61 | auto tree = std::make_shared>(); 62 | ne.setSearchMethod(tree); 63 | ne.setInputCloud(cloud); 64 | ne.setKSearch(50); 65 | ne.compute(*cloud_normals); 66 | 67 | auto ground_coeff = std::make_shared(); 68 | auto inliers = std::make_shared(); 69 | pcl::SACSegmentationFromNormals seg; 70 | 71 | seg.setOptimizeCoefficients(true); 72 | seg.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE); 73 | seg.setNormalDistanceWeight(0.02); 74 | seg.setMethodType(pcl::SAC_RANSAC); 75 | seg.setMaxIterations(10000); 76 | seg.setDistanceThreshold(0.005); 77 | seg.setInputCloud(cloud); 78 | seg.setInputNormals(cloud_normals); 79 | // set hardcoded ground normal axis value with wide epsilon value 80 | seg.setAxis(Eigen::Vector3f(.00295, -.7803, -.3831)); 81 | seg.setEpsAngle(0.523599); 82 | seg.segment(*inliers, *ground_coeff); 83 | 84 | if (inliers->indices.empty()) { 85 | PCL_ERROR ("Could not estimate a planar model for the given dataset."); 86 | } 87 | 88 | std::cerr << "Model coefficients: " << ground_coeff->values[0] << " " 89 | << ground_coeff->values[1] << " " 90 | << ground_coeff->values[2] << " " 91 | << ground_coeff->values[3] << std::endl; 92 | 93 | pcl::ExtractIndices extract; 94 | pcl::ExtractIndices extract_normals; 95 | extract.setInputCloud(cloud); 96 | extract.setIndices(inliers); 97 | extract.setNegative(false); 98 | 99 | // Get the points associated with the planar surface 100 | extract.filter(*cloud_plane); 101 | 102 | visual::Visualizer visualizer; 103 | std::vector>> clouds{cloud, cloud_plane}; 104 | visualizer.simpleVis(clouds); 105 | 106 | // Remove the planar inliers, extract the rest 107 | extract.setNegative(true); 108 | extract.filter(*cloud); 109 | 110 | // remove normal inliers 111 | extract_normals.setNegative(true); 112 | extract_normals.setInputCloud(cloud_normals); 113 | extract_normals.setIndices(inliers); 114 | extract_normals.filter(*cloud_normals); 115 | 116 | viewer->normalsVis(cloud, cloud_normals); 117 | 118 | // LETS GET THE UPRIGHT PLANE!! 119 | 120 | auto up_coeff = std::make_shared(); 121 | pcl::SACSegmentationFromNormals seg2; 122 | 123 | seg2.setOptimizeCoefficients(true); 124 | seg2.setModelType(pcl::SACMODEL_NORMAL_PLANE); 125 | seg2.setNormalDistanceWeight(0.02); 126 | seg2.setMethodType(pcl::SAC_RANSAC); 127 | seg2.setMaxIterations(10000); 128 | seg2.setDistanceThreshold(0.003); 129 | seg2.setInputCloud(cloud); 130 | seg2.setInputNormals(cloud_normals); 131 | seg2.segment(*inliers, *up_coeff); 132 | 133 | std::cerr << "Up Model coefficients: " << up_coeff->values[0] << " " 134 | << up_coeff->values[1] << " " 135 | << up_coeff->values[2] << " " 136 | << up_coeff->values[3] << std::endl; 137 | 138 | extract.setInputCloud(cloud); 139 | extract.setIndices(inliers); 140 | extract.setNegative(false); 141 | 142 | // Get the points associated with the planar surface 143 | extract.filter(*cloud_plane); 144 | 145 | 146 | clouds = {cloud, cloud_plane}; 147 | visualizer.simpleVis(clouds); 148 | 149 | auto ground_vect = Eigen::Vector3f(ground_coeff->values[0], ground_coeff->values[1], ground_coeff->values[2]); 150 | auto up_vect = Eigen::Vector3f(up_coeff->values[0], up_coeff->values[1], up_coeff->values[2]); 151 | 152 | double angle = std::atan2(ground_vect.cross(up_vect).norm(), ground_vect.dot(up_vect)); 153 | double angle_deg = angle * (180.0 / 3.141592653589793238463); 154 | logger::info("the angle between two planes is " + std::to_string(angle_deg) + ", error: " + 155 | std::to_string(abs((angle_deg - 90) / 90.0) * 100.0) + "%"); 156 | } -------------------------------------------------------------------------------- /src/file/IFileHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef SWAG_SCANNER_IFILEHANDLER_H 2 | #define SWAG_SCANNER_IFILEHANDLER_H 3 | 4 | #include "CloudType.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace spdlog { 13 | class logger; 14 | } 15 | 16 | namespace file { 17 | /** 18 | * Abstract base class for File Handling objects. 19 | * This class is specialized into CalibrationFileHandler and ScanFileHandler, so it serves more as 20 | * an interface. There is currently no need to modify this class to be a base type for CalibrationFileHandler 21 | * and ScanFileHandler. 22 | */ 23 | class IFileHandler { 24 | public: 25 | static std::filesystem::path swag_scanner_path; 26 | 27 | /** 28 | * Checks to see if a /SwagScanner folder exists in Library/Application Support. 29 | * If the folder does not exist, then create one and load in default configduration. 30 | * Otherwise, continue. 31 | * 32 | * @returns true if the program folder is already there. False if it isn't. 33 | */ 34 | static bool check_program_folder(); 35 | 36 | /** 37 | * Static method get the settings.json file from root of project. 38 | * 39 | * @return json file. 40 | */ 41 | static nlohmann::json load_swag_scanner_info_json(); 42 | 43 | /** 44 | * Static method write to settings.json. 45 | * 46 | * @param j json file that follows format of settings.json 47 | */ 48 | static void write_swag_scanner_info_json(const nlohmann::json &j); 49 | 50 | 51 | /** 52 | * Get ./settings/config.json 53 | * 54 | * @return config.json file 55 | */ 56 | static nlohmann::json get_swag_scanner_config_json(); 57 | 58 | /** 59 | * Get the current scan name. 60 | * 61 | * @return current scan name. 62 | */ 63 | inline std::string get_scan_name() { 64 | return this->scan_name; 65 | } 66 | 67 | /** 68 | * Get the path of the current scan. 69 | * @return path of current scan. 70 | */ 71 | inline std::string get_scan_path() { 72 | return this->scan_folder_path; 73 | } 74 | 75 | /** 76 | * Go to the SwagScanner/calibration directory and find the latest calibration by date. 77 | * 78 | * @return path to the latest calibration. 79 | * 80 | * ex: find_latest_calibration() -> .../SwagScanner/calibration/testCal1/testCal1.json 81 | */ 82 | std::filesystem::path find_latest_calibration(); 83 | 84 | /** 85 | * Save the given calibration to the current output_path. 86 | * @param cloud the calibration you want to save. 87 | * @para cloud_type enum for the type of calibration you are saving. Affects the subfolder path. 88 | */ 89 | virtual void save_cloud(const std::shared_ptr> &cloud, 90 | const std::string &cloud_name, 91 | const CloudType::Type &cloud_type) = 0; 92 | 93 | /* 94 | * Load a pointcloud from the scan folder given the name and type. 95 | * 96 | * @param cloud_name name of the calibration. 97 | * @param cloud_type type of the calibration. 98 | * @return calibration. 99 | * 100 | * Example: load_cloud("12", CloudType::RAW) 101 | */ 102 | virtual std::shared_ptr> load_cloud(const std::string &cloud_name, 103 | const CloudType::Type &cloud_type) = 0; 104 | 105 | /** 106 | * Get all the scans. 107 | * 108 | * @return vector of all the scan names. 109 | */ 110 | static std::vector get_all_scans(); 111 | 112 | /** 113 | * Get all the calibrations. 114 | * 115 | * @return vector of all the calibration names. 116 | */ 117 | static std::vector get_all_calibrations(); 118 | 119 | 120 | /** 121 | * Loads all clouds in the current scan folder into a vector given the calibration type. 122 | * Only works with files that have numbers in the file name. 123 | * 124 | * @Edge case when passed CALIBRATION as the type, it will search through the 125 | * /calibration folder in the root directory and load the latest calibration. 126 | * 127 | * @param cloud_type determines which folder to search for. 128 | * @return vector of loaded calibration pointers. 129 | * 130 | */ 131 | virtual std::vector>> 132 | load_clouds(const CloudType::Type &cloud_type) = 0; 133 | 134 | 135 | virtual ~IFileHandler() {} 136 | 137 | protected: 138 | std::shared_ptr logger; 139 | std::filesystem::path scan_folder_path; 140 | std::string scan_name; 141 | 142 | /** 143 | * Sorting function that sorts files and directories numerically in order from lowest to greatest. 144 | * @param path1 first path. 145 | * @param path2 second path. 146 | * @return true if the first file is smaller than the second, false otherwise. 147 | */ 148 | static bool path_sort(const std::filesystem::path &path1, const std::filesystem::path &path2); 149 | 150 | /** 151 | * Find the next scan folder by sorting the existing scans numerically. 152 | * E.g. if there are scans 1->10 in the all data folder, that means the next 153 | * scan must be 11. 154 | * 155 | */ 156 | virtual std::filesystem::path 157 | find_next_scan_folder_numeric(const CloudType::Type &type); 158 | 159 | std::filesystem::path find_next_scan_folder_numeric(); 160 | }; 161 | } 162 | #endif //SWAG_SCANNER_IFILEHANDLER_H 163 | -------------------------------------------------------------------------------- /test/research/depthFiltering/CompareDepthFiltering.cpp: -------------------------------------------------------------------------------- 1 | #define private public 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "Visualizer.h" 12 | #include "Plane.h" 13 | #include "CalibrationModel.h" 14 | #include "Constants.h" 15 | 16 | namespace fs = std::filesystem; 17 | 18 | class CompareDepthFilteringFixture : public ::testing::Test { 19 | 20 | protected: 21 | model::CalibrationModel *mod; 22 | visual::Visualizer *viewer; 23 | 24 | virtual void SetUp() { 25 | mod = new model::CalibrationModel(); 26 | viewer = new visual::Visualizer(); 27 | } 28 | 29 | virtual void TearDown() { 30 | delete mod; 31 | delete viewer; 32 | } 33 | }; 34 | 35 | /** 36 | * This test does a four comparison of a scans of the calibration fixture. 37 | * fixture_1.pcd: 38 | * "decimation_magnitude": 2, 39 | "spatial_filter_magnitude": 1, 40 | "spatial_smooth_alpha": 0.45, 41 | "spatial_smooth_delta": 5 42 | 43 | * fixture_2.pcd: 44 | "spatial_filter_magnitude": 5, 45 | 46 | * fixture_3.pcd: 47 | * "spatial_filter_magnitude": 5, 48 | * "spatial_smooth_alpha": 0.6, 49 | * This one performs the best tbh 50 | */ 51 | 52 | TEST_F(CompareDepthFilteringFixture, CompareCalFixture) { 53 | using namespace constants; 54 | std::cout << "Current path is " << fs::current_path() << '\n'; 55 | auto fixture_raw = std::make_shared>(); 56 | auto fixture_1 = std::make_shared>(); 57 | auto fixture_2 = std::make_shared>(); 58 | auto fixture_3 = std::make_shared>(); 59 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/fixture_raw.pcd", 60 | *fixture_raw); 61 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/fixture_1.pcd", 62 | *fixture_1); 63 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/fixture_2.pcd", 64 | *fixture_2); 65 | pcl::io::loadPLYFile("/Users/seanngpack/Desktop/test.ply", *fixture_3); 66 | // pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/fixture_3.pcd", *fixture_3); 67 | 68 | fixture_1 = mod->crop_cloud_cpy(fixture_1, cal_min_x, cal_max_x, cal_min_y, cal_max_y, cal_min_z, cal_max_z); 69 | 70 | viewer->compareVisFour(fixture_raw, fixture_1, fixture_2, fixture_3); 71 | } 72 | 73 | 74 | /** 75 | * This test compares realsense spatial filtering to pcl bilateral filtering 76 | * Results so far: Seems like realsense filtering vastly outperforms pcl bilateral filtering. 77 | * fixture_raw is raw cloud 78 | * 79 | * 80 | * findings: small sigmar_r keeps the edge -> increase r increase filter 81 | * small sigma_s = less filtering, more edge -> decrease s increase filter 82 | * im getting less angle error using this pipeline -> crop -> voxel -> bilateral 83 | * calibration does better with less filtering 84 | * 85 | * Doesn't seem like bilateral filtering does a very good job, 86 | * 87 | * fixture_1 is realsense filtered cloud 88 | * fixture_2 is pcl filtered cloud 89 | * fixture_3 is pcl filtered cloud 90 | */ 91 | TEST_F(CompareDepthFilteringFixture, CompareBilateralFilters) { 92 | using namespace constants; 93 | std::cout << "Current path is " << fs::current_path() << '\n'; 94 | auto fixture_raw = std::make_shared>(); 95 | auto fixture_1 = std::make_shared>(); 96 | auto fixture_2 = std::make_shared>(); 97 | auto fixture_3 = std::make_shared>(); 98 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/raw_cloud.pcd", 99 | *fixture_raw); 100 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/calibration_cloud.pcd", 101 | *fixture_1); 102 | 103 | std::cout << fixture_1->size() << std::endl; 104 | 105 | mod->get_calibration_planes_coefs(fixture_1, false); 106 | 107 | 108 | *fixture_2 = *fixture_raw; 109 | mod->crop_cloud(fixture_2, cal_min_x, cal_max_x, cal_min_y, cal_max_y, cal_min_z, cal_max_z); 110 | mod->bilateral_filter(fixture_2, 20, .01); 111 | mod->voxel_grid_filter(fixture_2, .001); 112 | mod->get_calibration_planes_coefs(fixture_2, false); 113 | std::cout << fixture_2->size() << std::endl; 114 | *fixture_3 = *fixture_raw; 115 | mod->crop_cloud(fixture_3, cal_min_x, cal_max_x, cal_min_y, cal_max_y, cal_min_z, cal_max_z); 116 | mod->bilateral_filter(fixture_3, 10, .001); 117 | mod->voxel_grid_filter(fixture_3, .001); 118 | mod->get_calibration_planes_coefs(fixture_3, false); 119 | 120 | fixture_1 = mod->crop_cloud_cpy(fixture_1, cal_min_x, cal_max_x, cal_min_y, cal_max_y, cal_min_z, cal_max_z); 121 | 122 | viewer->compareVisFour(fixture_raw, fixture_1, fixture_2, fixture_3); 123 | } 124 | 125 | /** 126 | * Compare bilateral filtering against full-resolution clouds to find optimal settings. 127 | */ 128 | TEST_F(CompareDepthFilteringFixture, TestBilateralFilterFullResolution) { 129 | using namespace constants; 130 | std::cout << "Current path is " << fs::current_path() << '\n'; 131 | auto fixture_raw = std::make_shared>(); 132 | auto fixture_1 = std::make_shared>(); 133 | auto fixture_2 = std::make_shared>(); 134 | auto fixture_3 = std::make_shared>(); 135 | pcl::io::loadPCDFile(fs::current_path().string() + "/research/depthFiltering/data/sponge.pcd", 136 | *fixture_raw); 137 | mod->crop_cloud(fixture_raw, cal_min_x, cal_max_x, cal_min_y, cal_max_y, cal_min_z, cal_max_z); 138 | std::cout << fixture_raw->size() << std::endl; 139 | 140 | *fixture_1 = *fixture_raw; 141 | mod->bilateral_filter(fixture_1, 10, .01); 142 | *fixture_2 = *fixture_raw; 143 | mod->bilateral_filter(fixture_2, 10, .005); 144 | *fixture_3 = *fixture_raw; 145 | mod->bilateral_filter(fixture_3, 10, .001); 146 | 147 | viewer->compareVisFour(fixture_raw, fixture_1, fixture_2, fixture_3); 148 | } -------------------------------------------------------------------------------- /src/utils/Algorithms.cpp: -------------------------------------------------------------------------------- 1 | #include "Algorithms.h" 2 | #include "Normal.h" 3 | #include "Plane.h" 4 | #include "CameraTypes.h" 5 | #include "Logger.h" 6 | #include 7 | 8 | pcl::PointXYZ algos::deproject_pixel_to_point(float x_pixel, 9 | float y_pixel, 10 | float z, 11 | const camera::intrinsics &intrinsics) { 12 | float depth = z * intrinsics.depth_scale; 13 | float x = (x_pixel - intrinsics.ppx) / intrinsics.fx; 14 | float y = (y_pixel - intrinsics.ppy) / intrinsics.fy; 15 | float ux = x * depth; 16 | float uy = y * depth; 17 | 18 | pcl::PointXYZ point = pcl::PointXYZ(ux, uy, depth); 19 | return point; 20 | } 21 | 22 | pcl::PointXYZ algos::project_point_to_plane(const pcl::PointXYZ &pt, 23 | const pcl::PointXYZ &plane_pt, 24 | const equations::Normal &normal) { 25 | double a = normal.A; 26 | double b = normal.B; 27 | double c = normal.C; 28 | double d = plane_pt.x; 29 | double e = plane_pt.y; 30 | double f = plane_pt.z; 31 | 32 | double x = pt.x; 33 | double y = pt.y; 34 | double z = pt.z; 35 | 36 | double t = (a * d - a * x + b * e - b * y + c * f - c * z) / (a * a + b * b + c * c); 37 | 38 | pcl::PointXYZ point = pcl::PointXYZ(x + t * a, 39 | y + t * b, 40 | z + t * c); 41 | return point; 42 | } 43 | 44 | bool algos::check_point_in_plane(const pcl::PointXYZ &pt, 45 | const equations::Plane &plane, 46 | double delta) { 47 | 48 | 49 | double lhs = pt.x * plane.A + pt.y * plane.B + pt.z * plane.C + plane.D; 50 | if (lhs > -delta && lhs < delta) { 51 | logger::info("found point for projection with error: " + std::to_string(lhs)); 52 | return true; 53 | } 54 | return false; 55 | } 56 | 57 | pcl::PointXYZ algos::find_point_in_plane(const std::shared_ptr> &cloud, 58 | const equations::Plane &plane, 59 | double delta) { 60 | for (const auto &pt: cloud->points) { 61 | if (check_point_in_plane(pt, plane, delta)) { 62 | return pt; 63 | } 64 | } 65 | logger::error("cannot find point in threshold, try loosening it"); 66 | return pcl::PointXYZ(0, 0, 0); 67 | } 68 | 69 | 70 | pcl::PointCloud 71 | algos::rotate_cloud_about_line(const std::shared_ptr> &cloud, 72 | const std::vector &pt, 73 | const std::vector &line_direction, 74 | float theta) { 75 | pcl::PointCloud transformed; 76 | 77 | transformed.resize(cloud->size()); 78 | 79 | for (int i = 0; i < cloud->size(); i++) { 80 | transformed.points[i] = rotate_point_about_line(cloud->points[i], 81 | pt, 82 | line_direction, 83 | theta); 84 | } 85 | return transformed; 86 | } 87 | 88 | pcl::PointXYZ algos::rotate_point_about_line(const pcl::PointXYZ &point, 89 | const std::vector &line_point, 90 | const std::vector &line_direction, 91 | float theta) { 92 | float x = point.x; 93 | float y = point.y; 94 | float z = point.z; 95 | float a = line_point[0]; 96 | float b = line_point[1]; 97 | float c = line_point[2]; 98 | float u = line_direction[0]; 99 | float v = line_direction[1]; 100 | float w = line_direction[2]; 101 | 102 | pcl::PointXYZ p; 103 | p.x = (a * (v * v + w * w) - u * (b * v + c * w - u * x - v * y - w * z)) * (1 - cos(theta)) + x * cos(theta) + 104 | (-c * v + b * w - w * y + v * z) * sin(theta); 105 | p.y = (b * (u * u + w * w) - v * (a * u + c * w - u * x - v * y - w * z)) * (1 - cos(theta)) + y * cos(theta) + 106 | (c * u - a * w + w * x - u * z) * sin(theta); 107 | p.z = (c * (u * u + v * v) - w * (a * u + b * v - u * x - v * y - w * z)) * (1 - cos(theta)) + z * cos(theta) + 108 | (-b * u + a * v - v * x + u * y) * sin(theta); 109 | 110 | return p; 111 | } 112 | 113 | pcl::PointCloud 114 | algos::rotate_cloud_about_z_axis(const std::shared_ptr> &cloud, float theta) { 115 | pcl::PointCloud rotated; 116 | Eigen::Affine3f transform(Eigen::Affine3f::Identity()); 117 | // note, rotating in negative direction 118 | transform.rotate(Eigen::AngleAxisf(-(theta * M_PI) / 180, Eigen::Vector3f::UnitZ())); 119 | pcl::transformPointCloud(*cloud, rotated, transform); 120 | return rotated; 121 | } 122 | 123 | Eigen::Matrix4f algos::calc_transform_to_world_matrix(const pcl::PointXYZ ¢er, 124 | const equations::Normal &ground_normal) { 125 | Eigen::Vector3f translation_vect(center.getVector3fMap()); 126 | translation_vect = -translation_vect; 127 | Eigen::Translation translation(translation_vect); 128 | 129 | float a_dot_b = Eigen::Vector3f(ground_normal.A, 130 | ground_normal.B, 131 | ground_normal.C).dot( 132 | Eigen::Vector3f(0, 0, 1)); 133 | float angle = -acos(a_dot_b); 134 | Eigen::AngleAxis rotation(angle, Eigen::Vector3f(1, 0, 0)); 135 | Eigen::Transform transform = rotation * translation; 136 | return transform.matrix(); 137 | } 138 | 139 | void algos::transform_cloud_to_world(const std::shared_ptr> &cloud, 140 | const pcl::PointXYZ ¢er, 141 | const equations::Normal &ground_normal) { 142 | Eigen::Matrix4f transform = calc_transform_to_world_matrix(center, ground_normal); 143 | pcl::transformPointCloud(*cloud, *cloud, transform); 144 | } 145 | 146 | equations::Plane algos::average_planes(const std::vector &planes) { 147 | equations::Plane avg; 148 | for (auto &g: planes) { 149 | avg.A += g.A; 150 | avg.B += g.B; 151 | avg.C += g.C; 152 | avg.D += g.D; 153 | } 154 | avg.A /= planes.size(); 155 | avg.B /= planes.size(); 156 | avg.C /= planes.size(); 157 | avg.D /= planes.size(); 158 | return avg; 159 | } --------------------------------------------------------------------------------