├── .github └── workflows │ └── wheels.yml ├── .gitignore ├── CMakeLists.txt ├── CSFDemo ├── CMakeLists.txt ├── CSF1.png ├── CSF2.png ├── CSFDemo.cpp ├── Cfg.h └── params.cfg ├── LICENSE.txt ├── MANIFEST.in ├── README.md ├── matlab ├── csf_filtering.cpp ├── demo_mex.m ├── demo_with_toolbox.m ├── demo_without_toolbox.m ├── readme.txt ├── sample.ply └── sample.txt ├── pyproject.toml ├── python ├── CSF │ ├── CSF.i │ ├── CSF.py │ └── CSF_wrap.cxx ├── Makefile ├── notes.txt └── tests │ └── csf_test.py ├── setup.py └── src ├── CMakeLists.txt ├── CSF.cpp ├── CSF.h ├── Cloth.cpp ├── Cloth.h ├── Constraint.cpp ├── Constraint.h ├── Particle.cpp ├── Particle.h ├── Rasterization.cpp ├── Rasterization.h ├── Vec3.h ├── XYZReader.cpp ├── XYZReader.h ├── c2cdist.cpp ├── c2cdist.h ├── point_cloud.cpp └── point_cloud.h /.github/workflows/wheels.yml: -------------------------------------------------------------------------------- 1 | name: build wheels 2 | 3 | #trigger on PR and only if something is tagged on branches 4 | on: 5 | push: 6 | branches: ["*"] 7 | tags: ["*"] 8 | pull_request: 9 | branches: ["*"] 10 | 11 | jobs: 12 | build_wheels: 13 | name: Build wheels on ${{ matrix.os }} 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-22.04, windows-2022, macOS-12] 18 | 19 | steps: 20 | - uses: actions/checkout@v4 21 | 22 | # Used to host cibuildwheel 23 | - uses: actions/setup-python@v5 24 | 25 | - name: Install cibuildwheel 26 | run: python -m pip install cibuildwheel==2.16.5 27 | 28 | - name: Build wheels 29 | run: python -m cibuildwheel --output-dir wheelhouse 30 | 31 | - uses: actions/upload-artifact@v3 32 | with: 33 | name: wheels 34 | path: ./wheelhouse/*.whl 35 | 36 | publish: 37 | name: Publish to PyPI 38 | needs: build_wheels 39 | runs-on: ubuntu-latest 40 | #Only publish to PyPI when a commit is tagged 41 | if: github.event_name == 'push' && startsWith(github.event.ref, 'refs/tags') 42 | steps: 43 | - uses: actions/checkout@v4 44 | 45 | - uses: actions/setup-python@v5 46 | 47 | - uses: actions/download-artifact@v3 48 | with: 49 | name: wheels 50 | path: wheelhouse 51 | 52 | - name: Publish to PyPI 53 | env: 54 | TWINE_USERNAME: __token__ 55 | TWINE_PASSWORD: ${{ secrets.PYPI_TOKEN }} 56 | # construct the source package and upload src and wheels to PiPy 57 | run: | 58 | python -m pip install --upgrade pip 59 | python -m pip install twine build 60 | python -m build --sdist 61 | twine upload dist/* 62 | twine upload wheelhouse/*.whl 63 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | .vscode -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(CSF LANGUAGES CXX) 3 | 4 | find_package(OpenMP) 5 | if(OpenMP_CXX_FOUND) 6 | add_compile_definitions("CSF_USE_OPENMP") 7 | endif() 8 | 9 | add_subdirectory(src) 10 | 11 | option(BUILD_DEMO "Build csfdemo executable" OFF) 12 | set(BUILD_SHARED_LIBS "Build as shared library" OFF) 13 | 14 | if(BUILD_DEMO) 15 | add_subdirectory(CSFDemo) 16 | endif() 17 | -------------------------------------------------------------------------------- /CSFDemo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(csfdemo CSFDemo.cpp) 2 | 3 | if(OpenMP_CXX_FOUND) 4 | target_link_libraries(csfdemo PUBLIC CSF OpenMP::OpenMP_CXX) 5 | endif() 6 | 7 | install(TARGETS csfdemo DESTINATION bin) 8 | -------------------------------------------------------------------------------- /CSFDemo/CSF1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jianboqi/CSF/5527b5953626d2322fd1a5e118b964b993f0cc7a/CSFDemo/CSF1.png -------------------------------------------------------------------------------- /CSFDemo/CSF2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jianboqi/CSF/5527b5953626d2322fd1a5e118b964b993f0cc7a/CSFDemo/CSF2.png -------------------------------------------------------------------------------- /CSFDemo/CSFDemo.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | 19 | #include 20 | #include "Cfg.h" 21 | #include "../src/CSF.h" 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | int main(int argc,char* argv[]) 28 | { 29 | Cfg cfg; 30 | std::string slop_smooth; 31 | cfg.readConfigFile("params.cfg", "slop_smooth", slop_smooth); 32 | bool ss = false; 33 | if (slop_smooth == "true" || slop_smooth == "True") 34 | { 35 | ss = true; 36 | } 37 | else if (slop_smooth == "false" || slop_smooth == "False") 38 | { 39 | ss = false; 40 | } 41 | else{ 42 | if (atoi(slop_smooth.c_str()) == 0){ 43 | ss = false; 44 | } 45 | else 46 | { 47 | ss = true; 48 | } 49 | } 50 | 51 | std::string class_threshold; 52 | cfg.readConfigFile("params.cfg", "class_threshold", class_threshold); 53 | std::string cloth_resolution; 54 | cfg.readConfigFile("params.cfg", "cloth_resolution", cloth_resolution); 55 | std::string iterations; 56 | cfg.readConfigFile("params.cfg", "iterations", iterations); 57 | std::string rigidness; 58 | cfg.readConfigFile("params.cfg", "rigidness", rigidness); 59 | std::string time_step; 60 | cfg.readConfigFile("params.cfg", "time_step", time_step); 61 | std::string terr_pointClouds_filepath; 62 | cfg.readConfigFile("params.cfg", "terr_pointClouds_filepath", terr_pointClouds_filepath); 63 | 64 | CSF csf; 65 | //step 1 Input the point cloud 66 | csf.readPointsFromFile(terr_pointClouds_filepath); 67 | 68 | clock_t start, end; 69 | start = clock(); 70 | 71 | //In real application, the point cloud may be provided by main program 72 | //csf.setPointCloud(pc);//pc is PointCloud class 73 | 74 | //step 2 parameter setting 75 | csf.params.bSloopSmooth = ss; 76 | csf.params.class_threshold = atof(class_threshold.c_str()); 77 | csf.params.cloth_resolution = atof(cloth_resolution.c_str()); 78 | csf.params.interations = atoi(iterations.c_str()); 79 | csf.params.rigidness = atoi(rigidness.c_str()); 80 | csf.params.time_step = atof(time_step.c_str()); 81 | 82 | //step3 do filtering 83 | std::vector groundIndexes, offGroundIndexes; 84 | if (argc == 2 && strcmp(argv[1], "-c")==0) 85 | { 86 | std::cout << "Export cloth enabled." << std::endl; 87 | csf.do_filtering(groundIndexes, offGroundIndexes, true); 88 | } 89 | else 90 | { 91 | csf.do_filtering(groundIndexes, offGroundIndexes, false); 92 | } 93 | 94 | 95 | end = clock(); 96 | double dur = (double)(end - start); 97 | printf("Use Time:%f\n", (dur / CLOCKS_PER_SEC)); 98 | 99 | csf.savePoints(groundIndexes,"ground.txt"); 100 | csf.savePoints(offGroundIndexes, "non-ground.txt"); 101 | 102 | return 0; 103 | } -------------------------------------------------------------------------------- /CSFDemo/Cfg.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef _CFG_H_ 19 | #define _CFG_H_ 20 | #include 21 | #include 22 | #include 23 | 24 | class Cfg 25 | { 26 | public: 27 | bool readConfigFile(const char * cfgfilepath, const std::string & key, std::string & value) 28 | { 29 | std::fstream cfgFile; 30 | cfgFile.open(cfgfilepath); 31 | if (!cfgFile.is_open()) 32 | { 33 | std::cout << "can not open cfg file!" << std::endl; 34 | return false; 35 | } 36 | char tmp[1000]; 37 | while (!cfgFile.eof()) 38 | { 39 | cfgFile.getline(tmp, 1000); 40 | std::string line(tmp); 41 | std::size_t pos = line.find('='); 42 | if (pos == std::string::npos) return false; 43 | std::string tmpKey = line.substr(0, pos); 44 | if (key == tmpKey) 45 | { 46 | value = line.substr(pos + 1); 47 | return true; 48 | } 49 | } 50 | return false; 51 | } 52 | }; 53 | 54 | 55 | #endif -------------------------------------------------------------------------------- /CSFDemo/params.cfg: -------------------------------------------------------------------------------- 1 | terr_pointClouds_filepath=samp311.txt 2 | slop_smooth=true 3 | cloth_resolution=0.5 4 | rigidness=3 5 | iterations=500 6 | class_threshold=0.5 7 | time_step=0.65 -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include src/*.h -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![csf1](https://github.com/jianboqi/CSF/blob/master/CSFDemo/CSF1.png) ![csf2](https://github.com/jianboqi/CSF/blob/master/CSFDemo/CSF2.png) 2 | # CSF 3 | Airborne LiDAR filtering method based on Cloth Simulation. 4 | This is the code for the article: 5 | 6 | W. Zhang, J. Qi*, P. Wan, H. Wang, D. Xie, X. Wang, and G. Yan, “An Easy-to-Use Airborne LiDAR Data Filtering Method Based on Cloth Simulation,” Remote Sens., vol. 8, no. 6, p. 501, 2016. 7 | (http://www.mdpi.com/2072-4292/8/6/501/htm) 8 | 9 | 10 | **New feature has been implemented:** 11 | 12 | Now, We has wrapped a Python interface for CSF with swig. It is simpler to use now. This new feature can make CSF easier to be embeded into a large project. For example, it can work with Laspy (https://github.com/laspy/laspy). What you do is just read a point cloud into a python 2D list, and pass it to CSF. 13 | The following example shows how to use it with laspy. 14 | ```python 15 | # coding: utf-8 16 | import laspy 17 | import CSF 18 | import numpy as np 19 | 20 | inFile = laspy.read(r"in.las") # read a las file 21 | points = inFile.points 22 | xyz = np.vstack((inFile.x, inFile.y, inFile.z)).transpose() # extract x, y, z and put into a list 23 | 24 | csf = CSF.CSF() 25 | 26 | # prameter settings 27 | csf.params.bSloopSmooth = False 28 | csf.params.cloth_resolution = 0.5 29 | # more details about parameter: http://ramm.bnu.edu.cn/projects/CSF/download/ 30 | 31 | csf.setPointCloud(xyz) 32 | ground = CSF.VecInt() # a list to indicate the index of ground points after calculation 33 | non_ground = CSF.VecInt() # a list to indicate the index of non-ground points after calculation 34 | csf.do_filtering(ground, non_ground) # do actual filtering. 35 | 36 | outFile = laspy.LasData(inFile.header) 37 | outFile.points = points[np.array(ground)] # extract ground points, and save it to a las file. 38 | out_file.write(r"out.las") 39 | ``` 40 | 41 | **Reading data from txt file:** 42 | 43 | If the lidar data is stored in txt file (x y z for each line), it can also be imported directly. 44 | 45 | ```python 46 | import CSF 47 | 48 | csf = CSF.CSF() 49 | csf.readPointsFromFile('samp52.txt') 50 | 51 | csf.params.bSloopSmooth = False 52 | csf.params.cloth_resolution = 0.5 53 | 54 | ground = CSF.VecInt() # a list to indicate the index of ground points after calculation 55 | non_ground = CSF.VecInt() # a list to indicate the index of non-ground points after calculation 56 | csf.do_filtering(ground, non_ground) # do actual filtering. 57 | csf.savePoints(ground,"ground.txt") 58 | ``` 59 | 60 | ### How to use CSF in Python 61 | Thanks to [@rjanvier](https://github.com/rjanvier)'s contribution. Now we can install CSF from pip as: 62 | ```python 63 | pip install cloth-simulation-filter 64 | ``` 65 | 66 | ### How to use CSF in Matlab 67 | see more details from file `demo_mex.m` under matlab folder. 68 | 69 | ### How to use CSF in R 70 | 71 | Thanks to the nice work of @Jean-Romain, through the collaboration, the CSF has been made as a R package, the details can be found in the [RCSF repository](https://github.com/Jean-Romain/RCSF). This package can be used easily with the [lidR package](https://github.com/Jean-Romain/lidR): 72 | 73 | ```r 74 | library(lidR) 75 | las <- readLAS("file.las") 76 | las <- lasground(las, csf()) 77 | ``` 78 | 79 | ### How to use CSF in C++ 80 | Now, CSF is built by CMake, it produces a static library, which can be used by other c++ programs. 81 | #### linux 82 | To build the library, run: 83 | ```bash 84 | mkdir build #or other name 85 | cd build 86 | cmake .. 87 | make 88 | sudo make install 89 | ``` 90 | or if you want to build the library and the demo executable `csfdemo` 91 | 92 | ```bash 93 | mkdir build #or other name 94 | cd build 95 | cmake -DBUILD_DEMO=ON .. 96 | make 97 | sudo make install 98 | 99 | ``` 100 | 101 | #### Windows 102 | You can use CMake GUI to generate visual studio solution file. 103 | 104 | ### Binary Version 105 | For binary release version, it can be downloaded at: http://ramm.bnu.edu.cn/projects/CSF/download/ 106 | 107 | Note: This code has been changed a lot since the publication of the corresponding paper. A lot of optimizations have been made. We are still working on it, and wish it could be better. 108 | 109 | ### Cloudcompare Pulgin 110 | At last, if you are interested in Cloudcompare, there is a good news. our method has been implemented as a Cloudcompare plugin, you can refer to : https://github.com/cloudcompare/trunk 111 | 112 | ### Related project 113 | A tool named `CSFTools` has been recently released, it is based on CSF, and provides dem/chm generation, normalization. Please refer to: https://github.com/jianboqi/CSFTools 114 | 115 | ### License 116 | CSF is maintained and developed by Jianbo QI. It is now released under Apache 2.0. 117 | 118 | -------------------------------------------------------------------------------- /matlab/csf_filtering.cpp: -------------------------------------------------------------------------------- 1 | 2 | // SimpleDLLTest.cpp : ¶¨Òå¿ØÖÆ̨ӦÓóÌÐòµÄÈë¿Úµã¡£ 3 | #include 4 | #include 5 | #include 6 | #include "../src/CSF.h" 7 | using namespace std; 8 | // #pragma comment(lib, "CSF.lib") 9 | 10 | //input: pointcloud riginess isSlopSmooth cloth_resolution 11 | void csf_filtering(double* points 12 | ,int rigidness 13 | , bool isSmooth 14 | , double cloth_resolution 15 | , double class_threshold 16 | , int interations 17 | , double time_step 18 | ,int rows 19 | ,std::vector& groundIndex 20 | ,std::vector& nongroundIndex 21 | ,int& groundRows 22 | ,int& nongroundRows 23 | ) 24 | { 25 | #define A(i,j) points[i+j*rows] 26 | 27 | CSF csf; 28 | //step 1 read point cloud from N*3 array 29 | 30 | csf.setPointCloud(points,rows); 31 | 32 | //±¸×¢£ºÔÚʵ¼ÊʹÓùý³ÌÖУ¬µãÔÆÊý¾ÝÓÉÖ÷³ÌÐòÌṩ£¬µ÷Óú¯ÊýΪ 33 | //csf.setPointCloud(pc);//pcΪPointCloudÀà 34 | 35 | //step 2 ÉèÖòÎÊý 36 | csf.params.bSloopSmooth = isSmooth; 37 | csf.params.class_threshold = class_threshold; 38 | csf.params.cloth_resolution = cloth_resolution; 39 | csf.params.interations = interations; 40 | csf.params.rigidness = rigidness ; 41 | csf.params.time_step = time_step; 42 | 43 | //step3 Ö´ÐÐÂ˲¨,resultÖд¢´æµÄÊǵØÃæµãµÄË÷Òý 44 | csf.do_filtering(groundIndex,nongroundIndex); 45 | groundRows = groundIndex.size(); 46 | nongroundRows = nongroundIndex.size(); 47 | } 48 | 49 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) 50 | { 51 | double *points = mxGetPr(prhs[0]); 52 | double resolution = mxGetScalar(prhs[3]); 53 | bool isSmooth = mxIsLogicalScalarTrue(prhs[2]); 54 | int rigidness = (int)mxGetScalar(prhs[1]); 55 | double class_threshold = mxGetScalar(prhs[4]); 56 | int interations = (int) mxGetScalar(prhs[5]); 57 | double time_step = mxGetScalar(prhs[6]); 58 | 59 | int rows = mxGetM(prhs[0]); 60 | std::vector groundIndex,nongroundIndex; 61 | int groundRows,nongroundRows; 62 | csf_filtering(points,rigidness,isSmooth,resolution,class_threshold,interations,time_step, 63 | rows,groundIndex, nongroundIndex,groundRows,nongroundRows); 64 | plhs[0] = mxCreateNumericMatrix(groundRows,1, mxINT32_CLASS, mxREAL); 65 | plhs[1] = mxCreateNumericMatrix(nongroundRows,1, mxINT32_CLASS, mxREAL); 66 | 67 | int* outputgroundMatrix = (int *)mxGetData(plhs[0]); 68 | int* outputnongroundMatrix = (int *)mxGetData(plhs[1]); 69 | for(int i=0;i; 19 | %template(VecFloat) vector; 20 | %template(VecVecFloat) vector< vector >; 21 | %template(VecDouble) vector; 22 | } 23 | 24 | %apply (double* IN_ARRAY2, int DIM1, int DIM2) {(double *points, int rows, int cols)}; 25 | %include "../src/CSF.h" 26 | -------------------------------------------------------------------------------- /python/CSF/CSF.py: -------------------------------------------------------------------------------- 1 | # This file was automatically generated by SWIG (http://www.swig.org). 2 | # Version 4.0.2 3 | # 4 | # Do not make changes to this file unless you know what you are doing--modify 5 | # the SWIG interface file instead. 6 | 7 | from sys import version_info as _swig_python_version_info 8 | if _swig_python_version_info < (2, 7, 0): 9 | raise RuntimeError("Python 2.7 or later required") 10 | 11 | # Import the low-level C/C++ module 12 | if __package__ or "." in __name__: 13 | from . import _CSF 14 | else: 15 | import _CSF 16 | 17 | try: 18 | import builtins as __builtin__ 19 | except ImportError: 20 | import __builtin__ 21 | 22 | def _swig_repr(self): 23 | try: 24 | strthis = "proxy of " + self.this.__repr__() 25 | except __builtin__.Exception: 26 | strthis = "" 27 | return "<%s.%s; %s >" % (self.__class__.__module__, self.__class__.__name__, strthis,) 28 | 29 | 30 | def _swig_setattr_nondynamic_instance_variable(set): 31 | def set_instance_attr(self, name, value): 32 | if name == "thisown": 33 | self.this.own(value) 34 | elif name == "this": 35 | set(self, name, value) 36 | elif hasattr(self, name) and isinstance(getattr(type(self), name), property): 37 | set(self, name, value) 38 | else: 39 | raise AttributeError("You cannot add instance attributes to %s" % self) 40 | return set_instance_attr 41 | 42 | 43 | def _swig_setattr_nondynamic_class_variable(set): 44 | def set_class_attr(cls, name, value): 45 | if hasattr(cls, name) and not isinstance(getattr(cls, name), property): 46 | set(cls, name, value) 47 | else: 48 | raise AttributeError("You cannot add class attributes to %s" % cls) 49 | return set_class_attr 50 | 51 | 52 | def _swig_add_metaclass(metaclass): 53 | """Class decorator for adding a metaclass to a SWIG wrapped class - a slimmed down version of six.add_metaclass""" 54 | def wrapper(cls): 55 | return metaclass(cls.__name__, cls.__bases__, cls.__dict__.copy()) 56 | return wrapper 57 | 58 | 59 | class _SwigNonDynamicMeta(type): 60 | """Meta class to enforce nondynamic attributes (no new attributes) for a class""" 61 | __setattr__ = _swig_setattr_nondynamic_class_variable(type.__setattr__) 62 | 63 | 64 | class SwigPyIterator(object): 65 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 66 | 67 | def __init__(self, *args, **kwargs): 68 | raise AttributeError("No constructor defined - class is abstract") 69 | __repr__ = _swig_repr 70 | __swig_destroy__ = _CSF.delete_SwigPyIterator 71 | 72 | def value(self): 73 | return _CSF.SwigPyIterator_value(self) 74 | 75 | def incr(self, n=1): 76 | return _CSF.SwigPyIterator_incr(self, n) 77 | 78 | def decr(self, n=1): 79 | return _CSF.SwigPyIterator_decr(self, n) 80 | 81 | def distance(self, x): 82 | return _CSF.SwigPyIterator_distance(self, x) 83 | 84 | def equal(self, x): 85 | return _CSF.SwigPyIterator_equal(self, x) 86 | 87 | def copy(self): 88 | return _CSF.SwigPyIterator_copy(self) 89 | 90 | def next(self): 91 | return _CSF.SwigPyIterator_next(self) 92 | 93 | def __next__(self): 94 | return _CSF.SwigPyIterator___next__(self) 95 | 96 | def previous(self): 97 | return _CSF.SwigPyIterator_previous(self) 98 | 99 | def advance(self, n): 100 | return _CSF.SwigPyIterator_advance(self, n) 101 | 102 | def __eq__(self, x): 103 | return _CSF.SwigPyIterator___eq__(self, x) 104 | 105 | def __ne__(self, x): 106 | return _CSF.SwigPyIterator___ne__(self, x) 107 | 108 | def __iadd__(self, n): 109 | return _CSF.SwigPyIterator___iadd__(self, n) 110 | 111 | def __isub__(self, n): 112 | return _CSF.SwigPyIterator___isub__(self, n) 113 | 114 | def __add__(self, n): 115 | return _CSF.SwigPyIterator___add__(self, n) 116 | 117 | def __sub__(self, *args): 118 | return _CSF.SwigPyIterator___sub__(self, *args) 119 | def __iter__(self): 120 | return self 121 | 122 | # Register SwigPyIterator in _CSF: 123 | _CSF.SwigPyIterator_swigregister(SwigPyIterator) 124 | 125 | class VecInt(object): 126 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 127 | __repr__ = _swig_repr 128 | 129 | def iterator(self): 130 | return _CSF.VecInt_iterator(self) 131 | def __iter__(self): 132 | return self.iterator() 133 | 134 | def __nonzero__(self): 135 | return _CSF.VecInt___nonzero__(self) 136 | 137 | def __bool__(self): 138 | return _CSF.VecInt___bool__(self) 139 | 140 | def __len__(self): 141 | return _CSF.VecInt___len__(self) 142 | 143 | def __getslice__(self, i, j): 144 | return _CSF.VecInt___getslice__(self, i, j) 145 | 146 | def __setslice__(self, *args): 147 | return _CSF.VecInt___setslice__(self, *args) 148 | 149 | def __delslice__(self, i, j): 150 | return _CSF.VecInt___delslice__(self, i, j) 151 | 152 | def __delitem__(self, *args): 153 | return _CSF.VecInt___delitem__(self, *args) 154 | 155 | def __getitem__(self, *args): 156 | return _CSF.VecInt___getitem__(self, *args) 157 | 158 | def __setitem__(self, *args): 159 | return _CSF.VecInt___setitem__(self, *args) 160 | 161 | def pop(self): 162 | return _CSF.VecInt_pop(self) 163 | 164 | def append(self, x): 165 | return _CSF.VecInt_append(self, x) 166 | 167 | def empty(self): 168 | return _CSF.VecInt_empty(self) 169 | 170 | def size(self): 171 | return _CSF.VecInt_size(self) 172 | 173 | def swap(self, v): 174 | return _CSF.VecInt_swap(self, v) 175 | 176 | def begin(self): 177 | return _CSF.VecInt_begin(self) 178 | 179 | def end(self): 180 | return _CSF.VecInt_end(self) 181 | 182 | def rbegin(self): 183 | return _CSF.VecInt_rbegin(self) 184 | 185 | def rend(self): 186 | return _CSF.VecInt_rend(self) 187 | 188 | def clear(self): 189 | return _CSF.VecInt_clear(self) 190 | 191 | def get_allocator(self): 192 | return _CSF.VecInt_get_allocator(self) 193 | 194 | def pop_back(self): 195 | return _CSF.VecInt_pop_back(self) 196 | 197 | def erase(self, *args): 198 | return _CSF.VecInt_erase(self, *args) 199 | 200 | def __init__(self, *args): 201 | _CSF.VecInt_swiginit(self, _CSF.new_VecInt(*args)) 202 | 203 | def push_back(self, x): 204 | return _CSF.VecInt_push_back(self, x) 205 | 206 | def front(self): 207 | return _CSF.VecInt_front(self) 208 | 209 | def back(self): 210 | return _CSF.VecInt_back(self) 211 | 212 | def assign(self, n, x): 213 | return _CSF.VecInt_assign(self, n, x) 214 | 215 | def resize(self, *args): 216 | return _CSF.VecInt_resize(self, *args) 217 | 218 | def insert(self, *args): 219 | return _CSF.VecInt_insert(self, *args) 220 | 221 | def reserve(self, n): 222 | return _CSF.VecInt_reserve(self, n) 223 | 224 | def capacity(self): 225 | return _CSF.VecInt_capacity(self) 226 | __swig_destroy__ = _CSF.delete_VecInt 227 | 228 | # Register VecInt in _CSF: 229 | _CSF.VecInt_swigregister(VecInt) 230 | 231 | class VecFloat(object): 232 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 233 | __repr__ = _swig_repr 234 | 235 | def iterator(self): 236 | return _CSF.VecFloat_iterator(self) 237 | def __iter__(self): 238 | return self.iterator() 239 | 240 | def __nonzero__(self): 241 | return _CSF.VecFloat___nonzero__(self) 242 | 243 | def __bool__(self): 244 | return _CSF.VecFloat___bool__(self) 245 | 246 | def __len__(self): 247 | return _CSF.VecFloat___len__(self) 248 | 249 | def __getslice__(self, i, j): 250 | return _CSF.VecFloat___getslice__(self, i, j) 251 | 252 | def __setslice__(self, *args): 253 | return _CSF.VecFloat___setslice__(self, *args) 254 | 255 | def __delslice__(self, i, j): 256 | return _CSF.VecFloat___delslice__(self, i, j) 257 | 258 | def __delitem__(self, *args): 259 | return _CSF.VecFloat___delitem__(self, *args) 260 | 261 | def __getitem__(self, *args): 262 | return _CSF.VecFloat___getitem__(self, *args) 263 | 264 | def __setitem__(self, *args): 265 | return _CSF.VecFloat___setitem__(self, *args) 266 | 267 | def pop(self): 268 | return _CSF.VecFloat_pop(self) 269 | 270 | def append(self, x): 271 | return _CSF.VecFloat_append(self, x) 272 | 273 | def empty(self): 274 | return _CSF.VecFloat_empty(self) 275 | 276 | def size(self): 277 | return _CSF.VecFloat_size(self) 278 | 279 | def swap(self, v): 280 | return _CSF.VecFloat_swap(self, v) 281 | 282 | def begin(self): 283 | return _CSF.VecFloat_begin(self) 284 | 285 | def end(self): 286 | return _CSF.VecFloat_end(self) 287 | 288 | def rbegin(self): 289 | return _CSF.VecFloat_rbegin(self) 290 | 291 | def rend(self): 292 | return _CSF.VecFloat_rend(self) 293 | 294 | def clear(self): 295 | return _CSF.VecFloat_clear(self) 296 | 297 | def get_allocator(self): 298 | return _CSF.VecFloat_get_allocator(self) 299 | 300 | def pop_back(self): 301 | return _CSF.VecFloat_pop_back(self) 302 | 303 | def erase(self, *args): 304 | return _CSF.VecFloat_erase(self, *args) 305 | 306 | def __init__(self, *args): 307 | _CSF.VecFloat_swiginit(self, _CSF.new_VecFloat(*args)) 308 | 309 | def push_back(self, x): 310 | return _CSF.VecFloat_push_back(self, x) 311 | 312 | def front(self): 313 | return _CSF.VecFloat_front(self) 314 | 315 | def back(self): 316 | return _CSF.VecFloat_back(self) 317 | 318 | def assign(self, n, x): 319 | return _CSF.VecFloat_assign(self, n, x) 320 | 321 | def resize(self, *args): 322 | return _CSF.VecFloat_resize(self, *args) 323 | 324 | def insert(self, *args): 325 | return _CSF.VecFloat_insert(self, *args) 326 | 327 | def reserve(self, n): 328 | return _CSF.VecFloat_reserve(self, n) 329 | 330 | def capacity(self): 331 | return _CSF.VecFloat_capacity(self) 332 | __swig_destroy__ = _CSF.delete_VecFloat 333 | 334 | # Register VecFloat in _CSF: 335 | _CSF.VecFloat_swigregister(VecFloat) 336 | 337 | class VecVecFloat(object): 338 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 339 | __repr__ = _swig_repr 340 | 341 | def iterator(self): 342 | return _CSF.VecVecFloat_iterator(self) 343 | def __iter__(self): 344 | return self.iterator() 345 | 346 | def __nonzero__(self): 347 | return _CSF.VecVecFloat___nonzero__(self) 348 | 349 | def __bool__(self): 350 | return _CSF.VecVecFloat___bool__(self) 351 | 352 | def __len__(self): 353 | return _CSF.VecVecFloat___len__(self) 354 | 355 | def __getslice__(self, i, j): 356 | return _CSF.VecVecFloat___getslice__(self, i, j) 357 | 358 | def __setslice__(self, *args): 359 | return _CSF.VecVecFloat___setslice__(self, *args) 360 | 361 | def __delslice__(self, i, j): 362 | return _CSF.VecVecFloat___delslice__(self, i, j) 363 | 364 | def __delitem__(self, *args): 365 | return _CSF.VecVecFloat___delitem__(self, *args) 366 | 367 | def __getitem__(self, *args): 368 | return _CSF.VecVecFloat___getitem__(self, *args) 369 | 370 | def __setitem__(self, *args): 371 | return _CSF.VecVecFloat___setitem__(self, *args) 372 | 373 | def pop(self): 374 | return _CSF.VecVecFloat_pop(self) 375 | 376 | def append(self, x): 377 | return _CSF.VecVecFloat_append(self, x) 378 | 379 | def empty(self): 380 | return _CSF.VecVecFloat_empty(self) 381 | 382 | def size(self): 383 | return _CSF.VecVecFloat_size(self) 384 | 385 | def swap(self, v): 386 | return _CSF.VecVecFloat_swap(self, v) 387 | 388 | def begin(self): 389 | return _CSF.VecVecFloat_begin(self) 390 | 391 | def end(self): 392 | return _CSF.VecVecFloat_end(self) 393 | 394 | def rbegin(self): 395 | return _CSF.VecVecFloat_rbegin(self) 396 | 397 | def rend(self): 398 | return _CSF.VecVecFloat_rend(self) 399 | 400 | def clear(self): 401 | return _CSF.VecVecFloat_clear(self) 402 | 403 | def get_allocator(self): 404 | return _CSF.VecVecFloat_get_allocator(self) 405 | 406 | def pop_back(self): 407 | return _CSF.VecVecFloat_pop_back(self) 408 | 409 | def erase(self, *args): 410 | return _CSF.VecVecFloat_erase(self, *args) 411 | 412 | def __init__(self, *args): 413 | _CSF.VecVecFloat_swiginit(self, _CSF.new_VecVecFloat(*args)) 414 | 415 | def push_back(self, x): 416 | return _CSF.VecVecFloat_push_back(self, x) 417 | 418 | def front(self): 419 | return _CSF.VecVecFloat_front(self) 420 | 421 | def back(self): 422 | return _CSF.VecVecFloat_back(self) 423 | 424 | def assign(self, n, x): 425 | return _CSF.VecVecFloat_assign(self, n, x) 426 | 427 | def resize(self, *args): 428 | return _CSF.VecVecFloat_resize(self, *args) 429 | 430 | def insert(self, *args): 431 | return _CSF.VecVecFloat_insert(self, *args) 432 | 433 | def reserve(self, n): 434 | return _CSF.VecVecFloat_reserve(self, n) 435 | 436 | def capacity(self): 437 | return _CSF.VecVecFloat_capacity(self) 438 | __swig_destroy__ = _CSF.delete_VecVecFloat 439 | 440 | # Register VecVecFloat in _CSF: 441 | _CSF.VecVecFloat_swigregister(VecVecFloat) 442 | 443 | class VecDouble(object): 444 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 445 | __repr__ = _swig_repr 446 | 447 | def iterator(self): 448 | return _CSF.VecDouble_iterator(self) 449 | def __iter__(self): 450 | return self.iterator() 451 | 452 | def __nonzero__(self): 453 | return _CSF.VecDouble___nonzero__(self) 454 | 455 | def __bool__(self): 456 | return _CSF.VecDouble___bool__(self) 457 | 458 | def __len__(self): 459 | return _CSF.VecDouble___len__(self) 460 | 461 | def __getslice__(self, i, j): 462 | return _CSF.VecDouble___getslice__(self, i, j) 463 | 464 | def __setslice__(self, *args): 465 | return _CSF.VecDouble___setslice__(self, *args) 466 | 467 | def __delslice__(self, i, j): 468 | return _CSF.VecDouble___delslice__(self, i, j) 469 | 470 | def __delitem__(self, *args): 471 | return _CSF.VecDouble___delitem__(self, *args) 472 | 473 | def __getitem__(self, *args): 474 | return _CSF.VecDouble___getitem__(self, *args) 475 | 476 | def __setitem__(self, *args): 477 | return _CSF.VecDouble___setitem__(self, *args) 478 | 479 | def pop(self): 480 | return _CSF.VecDouble_pop(self) 481 | 482 | def append(self, x): 483 | return _CSF.VecDouble_append(self, x) 484 | 485 | def empty(self): 486 | return _CSF.VecDouble_empty(self) 487 | 488 | def size(self): 489 | return _CSF.VecDouble_size(self) 490 | 491 | def swap(self, v): 492 | return _CSF.VecDouble_swap(self, v) 493 | 494 | def begin(self): 495 | return _CSF.VecDouble_begin(self) 496 | 497 | def end(self): 498 | return _CSF.VecDouble_end(self) 499 | 500 | def rbegin(self): 501 | return _CSF.VecDouble_rbegin(self) 502 | 503 | def rend(self): 504 | return _CSF.VecDouble_rend(self) 505 | 506 | def clear(self): 507 | return _CSF.VecDouble_clear(self) 508 | 509 | def get_allocator(self): 510 | return _CSF.VecDouble_get_allocator(self) 511 | 512 | def pop_back(self): 513 | return _CSF.VecDouble_pop_back(self) 514 | 515 | def erase(self, *args): 516 | return _CSF.VecDouble_erase(self, *args) 517 | 518 | def __init__(self, *args): 519 | _CSF.VecDouble_swiginit(self, _CSF.new_VecDouble(*args)) 520 | 521 | def push_back(self, x): 522 | return _CSF.VecDouble_push_back(self, x) 523 | 524 | def front(self): 525 | return _CSF.VecDouble_front(self) 526 | 527 | def back(self): 528 | return _CSF.VecDouble_back(self) 529 | 530 | def assign(self, n, x): 531 | return _CSF.VecDouble_assign(self, n, x) 532 | 533 | def resize(self, *args): 534 | return _CSF.VecDouble_resize(self, *args) 535 | 536 | def insert(self, *args): 537 | return _CSF.VecDouble_insert(self, *args) 538 | 539 | def reserve(self, n): 540 | return _CSF.VecDouble_reserve(self, n) 541 | 542 | def capacity(self): 543 | return _CSF.VecDouble_capacity(self) 544 | __swig_destroy__ = _CSF.delete_VecDouble 545 | 546 | # Register VecDouble in _CSF: 547 | _CSF.VecDouble_swigregister(VecDouble) 548 | 549 | class Params(object): 550 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 551 | __repr__ = _swig_repr 552 | bSloopSmooth = property(_CSF.Params_bSloopSmooth_get, _CSF.Params_bSloopSmooth_set) 553 | time_step = property(_CSF.Params_time_step_get, _CSF.Params_time_step_set) 554 | class_threshold = property(_CSF.Params_class_threshold_get, _CSF.Params_class_threshold_set) 555 | cloth_resolution = property(_CSF.Params_cloth_resolution_get, _CSF.Params_cloth_resolution_set) 556 | rigidness = property(_CSF.Params_rigidness_get, _CSF.Params_rigidness_set) 557 | interations = property(_CSF.Params_interations_get, _CSF.Params_interations_set) 558 | 559 | def __init__(self): 560 | _CSF.Params_swiginit(self, _CSF.new_Params()) 561 | __swig_destroy__ = _CSF.delete_Params 562 | 563 | # Register Params in _CSF: 564 | _CSF.Params_swigregister(Params) 565 | 566 | class CSF(object): 567 | thisown = property(lambda x: x.this.own(), lambda x, v: x.this.own(v), doc="The membership flag") 568 | __repr__ = _swig_repr 569 | 570 | def __init__(self, *args): 571 | _CSF.CSF_swiginit(self, _CSF.new_CSF(*args)) 572 | __swig_destroy__ = _CSF.delete_CSF 573 | 574 | def readPointsFromFile(self, filename): 575 | return _CSF.CSF_readPointsFromFile(self, filename) 576 | 577 | def getPointCloud(self, *args): 578 | return _CSF.CSF_getPointCloud(self, *args) 579 | 580 | def savePoints(self, grp, path): 581 | return _CSF.CSF_savePoints(self, grp, path) 582 | 583 | def size(self): 584 | return _CSF.CSF_size(self) 585 | 586 | def setPointCloud(self, *args): 587 | return _CSF.CSF_setPointCloud(self, *args) 588 | 589 | def do_filtering(self, groundIndexes, offGroundIndexes, exportCloth=True): 590 | return _CSF.CSF_do_filtering(self, groundIndexes, offGroundIndexes, exportCloth) 591 | 592 | def do_cloth_export(self): 593 | return _CSF.CSF_do_cloth_export(self) 594 | params = property(_CSF.CSF_params_get, _CSF.CSF_params_set) 595 | index = property(_CSF.CSF_index_get, _CSF.CSF_index_set) 596 | 597 | # Register CSF in _CSF: 598 | _CSF.CSF_swigregister(CSF) 599 | 600 | 601 | 602 | -------------------------------------------------------------------------------- /python/Makefile: -------------------------------------------------------------------------------- 1 | dummy: 2 | @echo 'Use make [wrapper|build|install]' 3 | 4 | clean: 5 | @rm -rf build 6 | 7 | wrapper: 8 | @swig3.0 -python -c++ -o CSF_wrap.cxx CSF.i 9 | 10 | build: wrapper 11 | @python setup.py build 12 | 13 | install: build 14 | @python setup.py install 15 | -------------------------------------------------------------------------------- /python/notes.txt: -------------------------------------------------------------------------------- 1 | dependencies: 2 | sudo aptitude install swig3.0 3 | 4 | If you modify the c++ code in CSFDLL, you should regenerate 5 | CSF_wrap.cxx using the Makefile provided here. 6 | 7 | Use make wrapper to generate CSF_wrap.cxx 8 | 9 | The Makefile also provides two utility targets to aid development. 10 | make build 11 | make install 12 | -------------------------------------------------------------------------------- /python/tests/csf_test.py: -------------------------------------------------------------------------------- 1 | # basic CSF python tests 2 | 3 | import CSF 4 | import numpy as np 5 | 6 | 7 | def test_csf_from_numpy(): 8 | """Test CSF+numpy with a flat ground plane.""" 9 | x = np.random.uniform(-100, 100, size=10_000) 10 | y = np.random.uniform(-100, 100, size=10_000) 11 | z = np.random.uniform(-0.1, -0.1, size=10_000) 12 | 13 | csf = CSF.CSF() 14 | csf.setPointCloud(np.c_[x, y, z]) 15 | 16 | ground, non_ground = CSF.VecInt(), CSF.VecInt() 17 | 18 | csf.do_filtering(ground, non_ground) 19 | assert len(ground) > 0 20 | assert len(non_ground) == 0 21 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import platform 2 | from setuptools import setup, Extension 3 | import numpy 4 | 5 | if platform.system() == "Windows": 6 | openmp_args = ["/openmp", "/std:c++11"] 7 | openmp_linking_args = [] 8 | openmp_macro = [("CSF_USE_OPENMP", None)] 9 | elif platform.system() == "Linux": 10 | openmp_args = ["-fopenmp", "-std=c++11"] 11 | openmp_linking_args = ["-fopenmp"] 12 | openmp_macro = [("CSF_USE_OPENMP", None)] 13 | else: # macOS, macOS clang won't come with openmp 14 | openmp_args = ["-std=c++11"] 15 | openmp_linking_args = [] 16 | openmp_macro = [] 17 | 18 | with open("README.md", encoding="utf8") as readme: 19 | readme_content = readme.read() 20 | 21 | sources = [ 22 | "python/CSF/CSF_wrap.cxx", 23 | "src/c2cdist.cpp", 24 | "src/Cloth.cpp", 25 | "src/CSF.cpp", 26 | "src/Particle.cpp", 27 | "src/point_cloud.cpp", 28 | "src/Rasterization.cpp", 29 | "src/XYZReader.cpp", 30 | ] 31 | 32 | include_dirs = ["src/", numpy.get_include()] 33 | 34 | csf_module = Extension( 35 | name="_CSF", 36 | sources=sources, 37 | include_dirs=include_dirs, 38 | extra_compile_args=openmp_args, 39 | extra_link_args=openmp_linking_args, 40 | define_macros=openmp_macro 41 | ) 42 | 43 | setup( 44 | name="cloth_simulation_filter", 45 | version="1.1.5", 46 | author="Jianbo Qi", 47 | url="http://ramm.bnu.edu.cn/projects/CSF/", 48 | long_description=readme_content, 49 | long_description_content_type='text/markdown', 50 | maintainer="Jianbo Qi", 51 | maintainer_email="jianboqi@126.com", 52 | license="Apache-2.0", 53 | keywords="LiDAR DTM DSM Classification", 54 | description="CSF: Ground Filtering based on Cloth Simulation", 55 | package_dir={"": "python/CSF"}, 56 | ext_modules=[csf_module], 57 | py_modules=["CSF"], 58 | ) 59 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(CSF_SOURCES 2 | c2cdist.cpp 3 | Cloth.cpp 4 | Constraint.cpp 5 | CSF.cpp 6 | Particle.cpp 7 | point_cloud.cpp 8 | Rasterization.cpp 9 | XYZReader.cpp 10 | ) 11 | 12 | set(CSF_HEADERS 13 | c2cdist.h 14 | Cloth.h 15 | Constraint.h 16 | CSF.h 17 | Particle.h 18 | point_cloud.h 19 | Rasterization.h 20 | Vec3.h 21 | XYZReader.h 22 | ) 23 | 24 | add_library(CSF ${CSF_SOURCES} ${CSF_HEADERS}) 25 | 26 | if (MSVC) 27 | # warning level 4 and all warnings as errors 28 | add_compile_options(/W4 /WX) 29 | else() 30 | # lots of warnings and all warnings as errors 31 | add_compile_options(-Wall -Wextra -pedantic -Werror) 32 | endif() 33 | 34 | if(OpenMP_CXX_FOUND) 35 | target_link_libraries(CSF PUBLIC OpenMP::OpenMP_CXX) 36 | endif() 37 | 38 | install(TARGETS CSF LIBRARY DESTINATION lib) 39 | install(TARGETS CSF ARCHIVE DESTINATION lib) 40 | install(FILES ${CSF_HEADERS} DESTINATION include) 41 | -------------------------------------------------------------------------------- /src/CSF.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #define DLL_IMPLEMENT 19 | 20 | #include "CSF.h" 21 | #include "XYZReader.h" 22 | #include "Vec3.h" 23 | #include "Rasterization.h" 24 | #include "c2cdist.h" 25 | #include 26 | 27 | 28 | CSF::CSF(int index) { 29 | params.bSloopSmooth = true; 30 | params.time_step = 0.65; 31 | params.class_threshold = 0.5; 32 | params.cloth_resolution = 1; 33 | params.rigidness = 3; 34 | params.interations = 500; 35 | 36 | this->index = index; 37 | } 38 | 39 | CSF::CSF() { 40 | params.bSloopSmooth = true; 41 | params.time_step = 0.65; 42 | params.class_threshold = 0.5; 43 | params.cloth_resolution = 1; 44 | params.rigidness = 3; 45 | params.interations = 500; 46 | this->index = 0; 47 | } 48 | 49 | CSF::~CSF() 50 | {} 51 | 52 | void CSF::setPointCloud(std::vector points) { 53 | point_cloud.resize(points.size()); 54 | 55 | int pointCount = static_cast(points.size()); 56 | #ifdef CSF_USE_OPENMP 57 | #pragma omp parallel for 58 | #endif 59 | for (int i = 0; i < pointCount; i++) { 60 | csf::Point las; 61 | las.x = points[i].x; 62 | las.y = -points[i].z; 63 | las.z = points[i].y; 64 | point_cloud[i] = las; 65 | } 66 | } 67 | 68 | void CSF::setPointCloud(double *points, int rows, int cols) { 69 | point_cloud.resize(rows); 70 | #define A(i, j) points[i * cols + j] 71 | for (int i = 0; i < rows; i++) { 72 | point_cloud[i] = {A(i, 0), -A(i, 2) , A(i, 1)}; 73 | } 74 | } 75 | 76 | 77 | 78 | void CSF::setPointCloud(double *points, int rows) { 79 | #define Mat(i, j) points[i + j * rows] 80 | point_cloud.resize(rows); 81 | for (int i = 0; i < rows; i++) { 82 | point_cloud[i] = {Mat(i, 0), -Mat(i, 2) , Mat(i, 1)}; 83 | } 84 | } 85 | 86 | void CSF::setPointCloud(csf::PointCloud& pc) { 87 | point_cloud.resize(pc.size()); 88 | int pointCount = static_cast(pc.size()); 89 | #ifdef CSF_USE_OPENMP 90 | #pragma omp parallel for 91 | #endif 92 | for (int i = 0; i < pointCount; i++) { 93 | csf::Point las; 94 | las.x = pc[i].x; 95 | las.y = -pc[i].z; 96 | las.z = pc[i].y; 97 | point_cloud[i] = las; 98 | } 99 | } 100 | 101 | 102 | void CSF::readPointsFromFile(std::string filename) { 103 | this->point_cloud.resize(0); 104 | read_xyz(filename, this->point_cloud); 105 | } 106 | 107 | 108 | Cloth CSF::do_cloth() { 109 | // Terrain 110 | std::cout << "[" << this->index << "] Configuring terrain..." << std::endl; 111 | csf::Point bbMin, bbMax; 112 | point_cloud.computeBoundingBox(bbMin, bbMax); 113 | std::cout << "[" << this->index << "] - bbMin: " << bbMin.x << " " << bbMin.y << " " << bbMin.z << std::endl; 114 | std::cout << "[" << this->index << "] - bbMax: " << bbMax.x << " " << bbMax.y << " " << bbMax.z << std::endl; 115 | 116 | double cloth_y_height = 0.05; 117 | 118 | int clothbuffer_d = 2; 119 | Vec3 origin_pos( 120 | bbMin.x - clothbuffer_d *params.cloth_resolution, 121 | bbMax.y + cloth_y_height, 122 | bbMin.z - clothbuffer_d *params.cloth_resolution 123 | ); 124 | 125 | int width_num = static_cast( 126 | std::floor((bbMax.x - bbMin.x) / params.cloth_resolution) 127 | ) + 2 * clothbuffer_d; 128 | 129 | int height_num = static_cast( 130 | std::floor((bbMax.z - bbMin.z) / params.cloth_resolution) 131 | ) + 2 * clothbuffer_d; 132 | 133 | std::cout << "[" << this->index << "] Configuring cloth..." << std::endl; 134 | std::cout << "[" << this->index << "] - width: " << width_num << " " 135 | << "height: " << height_num << std::endl; 136 | 137 | Cloth cloth( 138 | origin_pos, 139 | width_num, 140 | height_num, 141 | params.cloth_resolution, 142 | params.cloth_resolution, 143 | 0.3, 144 | 9999, 145 | params.rigidness, 146 | params.time_step 147 | ); 148 | 149 | std::cout << "[" << this->index << "] Rasterizing..." << std::endl; 150 | Rasterization::RasterTerrian(cloth, point_cloud, cloth.getHeightvals()); 151 | 152 | double time_step2 = params.time_step * params.time_step; 153 | double gravity = 0.2; 154 | 155 | std::cout << "[" << this->index << "] Simulating..." << std::endl; 156 | cloth.addForce(Vec3(0, -gravity, 0) * time_step2); 157 | 158 | // boost::progress_display pd(params.interations); 159 | for (int i = 0; i < params.interations; i++) { 160 | double maxDiff = cloth.timeStep(); 161 | cloth.terrCollision(); 162 | //params.class_threshold / 100 163 | if ((maxDiff != 0) && (maxDiff < 0.005)) { 164 | // early stop 165 | break; 166 | } 167 | // pd++; 168 | } 169 | 170 | if (params.bSloopSmooth) { 171 | std::cout << "[" << this->index << "] - post handle..." << std::endl; 172 | cloth.movableFilter(); 173 | } 174 | 175 | return cloth; 176 | } 177 | 178 | std::vector CSF::do_cloth_export() { 179 | auto cloth = do_cloth(); 180 | return cloth.toVector(); 181 | } 182 | 183 | void CSF::do_filtering(std::vector& groundIndexes, 184 | std::vector& offGroundIndexes, 185 | bool exportCloth) { 186 | auto cloth = do_cloth(); 187 | if (exportCloth) 188 | cloth.saveToFile(); 189 | c2cdist c2c(params.class_threshold); 190 | c2c.calCloud2CloudDist(cloth, point_cloud, groundIndexes, offGroundIndexes); 191 | } 192 | 193 | 194 | void CSF::savePoints(std::vector grp, std::string path) { 195 | if (path == "") { 196 | return; 197 | } 198 | 199 | std::ofstream f1(path.c_str(), std::ios::out); 200 | 201 | if (!f1) 202 | return; 203 | 204 | for (std::size_t i = 0; i < grp.size(); i++) { 205 | f1 << std::fixed << std::setprecision(8) 206 | << point_cloud[grp[i]].x << " " 207 | << point_cloud[grp[i]].z << " " 208 | << -point_cloud[grp[i]].y << std::endl; 209 | } 210 | 211 | f1.close(); 212 | } 213 | -------------------------------------------------------------------------------- /src/CSF.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | // ####################################################################################### 19 | // # # 20 | // # CSF: Airborne LiDAR filtering based on Cloth Simulation # 21 | // # # 22 | // # Please cite the following paper, If you use this software in your work. # 23 | // # # 24 | // # Zhang W, Qi J, Wan P, Wang H, Xie D, Wang X, Yan G. An Easy-to-Use Airborne LiDAR # 25 | // # Data Filtering Method Based on Cloth Simulation. Remote Sensing. 2016; 8(6):501. # 26 | // # (http://ramm.bnu.edu.cn/) # 27 | // # # 28 | // # Wuming Zhang; Jianbo Qi; Peng Wan; Hongtao Wang # 29 | // # # 30 | // # contact us: 2009zwm@gmail.com; wpqjbzwm@126.com # 31 | // # # 32 | // ####################################################################################### 33 | 34 | 35 | // cloth simulation filter for airborne lidar filtering 36 | #ifndef _CSF_H_ 37 | #define _CSF_H_ 38 | #include 39 | #include 40 | #include "point_cloud.h" 41 | #include "Cloth.h" 42 | 43 | 44 | struct Params { 45 | // refer to the website:http://ramm.bnu.edu.cn/projects/CSF/ for the setting of these paramters 46 | bool bSloopSmooth; 47 | double time_step; 48 | double class_threshold; 49 | double cloth_resolution; 50 | int rigidness; 51 | int interations; 52 | }; 53 | 54 | #ifdef _CSF_DLL_EXPORT_ 55 | # ifdef DLL_IMPLEMENT 56 | # define DLL_API __declspec(dllexport) 57 | # else // ifdef DLL_IMPLEMENT 58 | # define DLL_API __declspec(dllimport) 59 | # endif // ifdef DLL_IMPLEMENT 60 | #endif // ifdef _CSF_DLL_EXPORT_ 61 | 62 | #ifdef _CSF_DLL_EXPORT_ 63 | class DLL_API CSF 64 | #else // ifdef _CSF_DLL_EXPORT_ 65 | class CSF 66 | #endif // ifdef _CSF_DLL_EXPORT_ 67 | { 68 | public: 69 | 70 | CSF(int index); 71 | CSF(); 72 | ~CSF(); 73 | 74 | // set pointcloud from vector 75 | void setPointCloud(std::vector points); 76 | 77 | // set point cloud from a two-dimentional array. it defines a N*3 point cloud by the given rows. 78 | // it is the method used to set point cloud from python (numpy array) 79 | void setPointCloud(double *points, int rows, int cols); 80 | 81 | // set point cloud from a one-dimentional array. it defines a N*3 point cloud by the given rows. 82 | // it is the method used to set point cloud from matlab 83 | void setPointCloud(double *points, int rows); 84 | 85 | // read pointcloud from txt file: (X Y Z) for each line 86 | void readPointsFromFile(std::string filename); 87 | 88 | inline csf::PointCloud& getPointCloud() { 89 | return point_cloud; 90 | } 91 | 92 | inline const csf::PointCloud& getPointCloud() const { 93 | return point_cloud; 94 | } 95 | 96 | // save points to file 97 | void savePoints(std::vector grp, std::string path); 98 | 99 | // get size of pointcloud 100 | std::size_t size() { 101 | return point_cloud.size(); 102 | } 103 | 104 | // PointCloud set pointcloud 105 | void setPointCloud(csf::PointCloud& pc); 106 | 107 | // The results are index of ground points in the original 108 | // pointcloud and write the cloth particles coordinates 109 | void do_filtering(std::vector& groundIndexes, 110 | std::vector& offGroundIndexes, 111 | bool exportCloth=true); 112 | 113 | 114 | std::vector do_cloth_export(); 115 | 116 | private: 117 | 118 | // Do the filtering and return the Cloth object 119 | Cloth do_cloth(); 120 | 121 | #ifdef _CSF_DLL_EXPORT_ 122 | class __declspec (dllexport)csf::PointCloud point_cloud; 123 | #else // ifdef _CSF_DLL_EXPORT_ 124 | csf::PointCloud point_cloud; 125 | #endif // ifdef _CSF_DLL_EXPORT_ 126 | 127 | public: 128 | 129 | Params params; 130 | int index; 131 | }; 132 | 133 | #endif // ifndef _CSF_H_ 134 | -------------------------------------------------------------------------------- /src/Cloth.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "Cloth.h" 19 | #include 20 | 21 | 22 | Cloth::Cloth(const Vec3& _origin_pos, 23 | int _num_particles_width, 24 | int _num_particles_height, 25 | double _step_x, 26 | double _step_y, 27 | double _smoothThreshold, 28 | double _heightThreshold, 29 | int rigidness, 30 | double time_step) 31 | : constraint_iterations(rigidness), 32 | time_step(time_step), 33 | smoothThreshold(_smoothThreshold), 34 | heightThreshold(_heightThreshold), 35 | origin_pos(_origin_pos), 36 | step_x(_step_x), 37 | step_y(_step_y), 38 | num_particles_width(_num_particles_width), 39 | num_particles_height(_num_particles_height) { 40 | // I am essentially using this vector as an array with room for 41 | // num_particles_width*num_particles_height particles 42 | particles.resize(num_particles_width * num_particles_height); 43 | 44 | double time_step2 = time_step * time_step; 45 | 46 | // creating particles in a grid of particles from (0,0,0) to 47 | // (width,-height,0) creating particles in a grid 48 | for (int i = 0; i < num_particles_width; i++) { 49 | for (int j = 0; j < num_particles_height; j++) { 50 | Vec3 pos(origin_pos.f[0] + i *step_x, 51 | origin_pos.f[1], 52 | origin_pos.f[2] + j *step_y); 53 | 54 | // insert particle in column i at j'th row 55 | particles[j * num_particles_width + i] = Particle(pos, time_step2); 56 | particles[j * num_particles_width + i].pos_x = i; 57 | particles[j * num_particles_width + i].pos_y = j; 58 | } 59 | } 60 | 61 | // Connecting immediate neighbor particles with constraints 62 | // (distance 1 and sqrt(2) in the grid) 63 | for (int x = 0; x < num_particles_width; x++) { 64 | for (int y = 0; y < num_particles_height; y++) { 65 | if (x < num_particles_width - 1) 66 | makeConstraint(getParticle(x, y), getParticle(x + 1, y)); 67 | 68 | if (y < num_particles_height - 1) 69 | makeConstraint(getParticle(x, y), getParticle(x, y + 1)); 70 | 71 | if ((x < num_particles_width - 1) && (y < num_particles_height - 1)) 72 | makeConstraint(getParticle(x, y), getParticle(x + 1, y + 1)); 73 | 74 | if ((x < num_particles_width - 1) && (y < num_particles_height - 1)) 75 | makeConstraint(getParticle(x + 1, y), getParticle(x, y + 1)); 76 | } 77 | } 78 | 79 | // Connecting secondary neighbors with constraints (distance 2 and sqrt(4) in the grid) 80 | for (int x = 0; x < num_particles_width; x++) { 81 | for (int y = 0; y < num_particles_height; y++) { 82 | if (x < num_particles_width - 2) 83 | makeConstraint(getParticle(x, y), getParticle(x + 2, y)); 84 | 85 | if (y < num_particles_height - 2) 86 | makeConstraint(getParticle(x, y), getParticle(x, y + 2)); 87 | 88 | if ((x < num_particles_width - 2) && (y < num_particles_height - 2)) 89 | makeConstraint(getParticle(x, y), getParticle(x + 2, y + 2)); 90 | 91 | if ((x < num_particles_width - 2) && (y < num_particles_height - 2)) 92 | makeConstraint(getParticle(x + 2, y), getParticle(x, y + 2)); 93 | } 94 | } 95 | } 96 | 97 | double Cloth::timeStep() { 98 | int particleCount = static_cast(particles.size()); 99 | #ifdef CSF_USE_OPENMP 100 | #pragma omp parallel for 101 | #endif 102 | for (int i = 0; i < particleCount; i++) { 103 | particles[i].timeStep(); 104 | } 105 | 106 | #ifdef CSF_USE_OPENMP 107 | #pragma omp parallel for 108 | #endif 109 | for (int j = 0; j < particleCount; j++) { 110 | particles[j].satisfyConstraintSelf(constraint_iterations); 111 | } 112 | 113 | double maxDiff = 0; 114 | 115 | for (int i = 0; i < particleCount; i++) { 116 | if (particles[i].isMovable()) { 117 | double diff = fabs(particles[i].old_pos.f[1] - particles[i].pos.f[1]); 118 | 119 | if (diff > maxDiff) 120 | maxDiff = diff; 121 | } 122 | } 123 | 124 | return maxDiff; 125 | } 126 | 127 | void Cloth::addForce(const Vec3 direction) { 128 | for (std::size_t i = 0; i < particles.size(); i++) { 129 | particles[i].addForce(direction); 130 | } 131 | } 132 | 133 | void Cloth::terrCollision() { 134 | int particleCount = static_cast(particles.size()); 135 | 136 | #ifdef CSF_USE_OPENMP 137 | #pragma omp parallel for 138 | #endif 139 | for (int i = 0; i < particleCount; i++) { 140 | Vec3 v = particles[i].getPos(); 141 | 142 | if (v.f[1] < heightvals[i]) { 143 | particles[i].offsetPos(Vec3(0, heightvals[i] - v.f[1], 0)); 144 | particles[i].makeUnmovable(); 145 | } 146 | } 147 | } 148 | 149 | void Cloth::movableFilter() { 150 | std::vector tmpParticles; 151 | 152 | for (int x = 0; x < num_particles_width; x++) { 153 | for (int y = 0; y < num_particles_height; y++) { 154 | Particle *ptc = getParticle(x, y); 155 | 156 | if (ptc->isMovable() && !ptc->isVisited) { 157 | std::queue que; 158 | std::vector connected; // store the connected component 159 | std::vector > neibors; 160 | int sum = 1; 161 | int index = y * num_particles_width + x; 162 | 163 | // visit the init node 164 | connected.push_back(XY(x, y)); 165 | particles[index].isVisited = true; 166 | 167 | // enqueue the init node 168 | que.push(index); 169 | 170 | while (!que.empty()) { 171 | Particle *ptc_f = &particles[que.front()]; 172 | que.pop(); 173 | int cur_x = ptc_f->pos_x; 174 | int cur_y = ptc_f->pos_y; 175 | std::vector neibor; 176 | 177 | if (cur_x > 0) { 178 | Particle *ptc_left = getParticle(cur_x - 1, cur_y); 179 | 180 | if (ptc_left->isMovable()) { 181 | if (!ptc_left->isVisited) { 182 | sum++; 183 | ptc_left->isVisited = true; 184 | connected.push_back(XY(cur_x - 1, cur_y)); 185 | que.push(num_particles_width * cur_y + cur_x - 1); 186 | neibor.push_back(sum - 1); 187 | ptc_left->c_pos = sum - 1; 188 | } else { 189 | neibor.push_back(ptc_left->c_pos); 190 | } 191 | } 192 | } 193 | 194 | if (cur_x < num_particles_width - 1) { 195 | Particle *ptc_right = getParticle(cur_x + 1, cur_y); 196 | 197 | if (ptc_right->isMovable()) { 198 | if (!ptc_right->isVisited) { 199 | sum++; 200 | ptc_right->isVisited = true; 201 | connected.push_back(XY(cur_x + 1, cur_y)); 202 | que.push(num_particles_width * cur_y + cur_x + 1); 203 | neibor.push_back(sum - 1); 204 | ptc_right->c_pos = sum - 1; 205 | } else { 206 | neibor.push_back(ptc_right->c_pos); 207 | } 208 | } 209 | } 210 | 211 | if (cur_y > 0) { 212 | Particle *ptc_bottom = getParticle(cur_x, cur_y - 1); 213 | 214 | if (ptc_bottom->isMovable()) { 215 | if (!ptc_bottom->isVisited) { 216 | sum++; 217 | ptc_bottom->isVisited = true; 218 | connected.push_back(XY(cur_x, cur_y - 1)); 219 | que.push(num_particles_width * (cur_y - 1) + cur_x); 220 | neibor.push_back(sum - 1); 221 | ptc_bottom->c_pos = sum - 1; 222 | } else { 223 | neibor.push_back(ptc_bottom->c_pos); 224 | } 225 | } 226 | } 227 | 228 | if (cur_y < num_particles_height - 1) { 229 | Particle *ptc_top = getParticle(cur_x, cur_y + 1); 230 | 231 | if (ptc_top->isMovable()) { 232 | if (!ptc_top->isVisited) { 233 | sum++; 234 | ptc_top->isVisited = true; 235 | connected.push_back(XY(cur_x, cur_y + 1)); 236 | que.push(num_particles_width * (cur_y + 1) + cur_x); 237 | neibor.push_back(sum - 1); 238 | ptc_top->c_pos = sum - 1; 239 | } else { 240 | neibor.push_back(ptc_top->c_pos); 241 | } 242 | } 243 | } 244 | neibors.push_back(neibor); 245 | } 246 | 247 | if (sum > MAX_PARTICLE_FOR_POSTPROCESSIN) { 248 | std::vector edgePoints = findUnmovablePoint(connected); 249 | handle_slop_connected(edgePoints, connected, neibors); 250 | } 251 | } 252 | } 253 | } 254 | } 255 | 256 | std::vector Cloth::findUnmovablePoint(std::vector connected) { 257 | std::vector edgePoints; 258 | 259 | for (std::size_t i = 0; i < connected.size(); i++) { 260 | int x = connected[i].x; 261 | int y = connected[i].y; 262 | int index = y * num_particles_width + x; 263 | Particle *ptc = getParticle(x, y); 264 | 265 | if (x > 0) { 266 | Particle *ptc_x = getParticle(x - 1, y); 267 | 268 | if (!ptc_x->isMovable()) { 269 | int index_ref = y * num_particles_width + x - 1; 270 | 271 | if ((fabs(heightvals[index] - heightvals[index_ref]) < smoothThreshold) && 272 | (ptc->getPos().f[1] - heightvals[index] < heightThreshold)) { 273 | Vec3 offsetVec = Vec3(0, heightvals[index] - ptc->getPos().f[1], 0); 274 | particles[index].offsetPos(offsetVec); 275 | ptc->makeUnmovable(); 276 | edgePoints.push_back(i); 277 | continue; 278 | } 279 | } 280 | } 281 | 282 | if (x < num_particles_width - 1) { 283 | Particle *ptc_x = getParticle(x + 1, y); 284 | 285 | if (!ptc_x->isMovable()) { 286 | int index_ref = y * num_particles_width + x + 1; 287 | 288 | if ((fabs(heightvals[index] - heightvals[index_ref]) < smoothThreshold) && 289 | (ptc->getPos().f[1] - heightvals[index] < heightThreshold)) { 290 | Vec3 offsetVec = Vec3(0, heightvals[index] - ptc->getPos().f[1], 0); 291 | particles[index].offsetPos(offsetVec); 292 | ptc->makeUnmovable(); 293 | edgePoints.push_back(i); 294 | continue; 295 | } 296 | } 297 | } 298 | 299 | if (y > 0) { 300 | Particle *ptc_y = getParticle(x, y - 1); 301 | 302 | if (!ptc_y->isMovable()) { 303 | int index_ref = (y - 1) * num_particles_width + x; 304 | 305 | if ((fabs(heightvals[index] - heightvals[index_ref]) < smoothThreshold) && 306 | (ptc->getPos().f[1] - heightvals[index] < heightThreshold)) { 307 | Vec3 offsetVec = Vec3(0, heightvals[index] - ptc->getPos().f[1], 0); 308 | particles[index].offsetPos(offsetVec); 309 | ptc->makeUnmovable(); 310 | edgePoints.push_back(i); 311 | continue; 312 | } 313 | } 314 | } 315 | 316 | if (y < num_particles_height - 1) { 317 | Particle *ptc_y = getParticle(x, y + 1); 318 | 319 | if (!ptc_y->isMovable()) { 320 | int index_ref = (y + 1) * num_particles_width + x; 321 | 322 | if ((fabs(heightvals[index] - heightvals[index_ref]) < smoothThreshold) && 323 | (ptc->getPos().f[1] - heightvals[index] < heightThreshold)) { 324 | Vec3 offsetVec = Vec3(0, heightvals[index] - ptc->getPos().f[1], 0); 325 | particles[index].offsetPos(offsetVec); 326 | ptc->makeUnmovable(); 327 | edgePoints.push_back(i); 328 | continue; 329 | } 330 | } 331 | } 332 | } 333 | 334 | return edgePoints; 335 | } 336 | 337 | void Cloth::handle_slop_connected(std::vector edgePoints, std::vector connected, std::vector > neibors) { 338 | std::vector visited; 339 | 340 | for (std::size_t i = 0; i < connected.size(); i++) visited.push_back(false); 341 | 342 | std::queue que; 343 | 344 | for (std::size_t i = 0; i < edgePoints.size(); i++) { 345 | que.push(edgePoints[i]); 346 | visited[edgePoints[i]] = true; 347 | } 348 | 349 | while (!que.empty()) { 350 | int index = que.front(); 351 | que.pop(); 352 | 353 | int index_center = connected[index].y * num_particles_width + connected[index].x; 354 | 355 | for (std::size_t i = 0; i < neibors[index].size(); i++) { 356 | int index_neibor = connected[neibors[index][i]].y * num_particles_width + connected[neibors[index][i]].x; 357 | 358 | if ((fabs(heightvals[index_center] - heightvals[index_neibor]) < smoothThreshold) && 359 | (fabs(particles[index_neibor].getPos().f[1] - heightvals[index_neibor]) < heightThreshold)) { 360 | Vec3 offsetVec = Vec3(0, heightvals[index_neibor] - particles[index_neibor].getPos().f[1], 0); 361 | particles[index_neibor].offsetPos(offsetVec); 362 | particles[index_neibor].makeUnmovable(); 363 | 364 | if (visited[neibors[index][i]] == false) { 365 | que.push(neibors[index][i]); 366 | visited[neibors[index][i]] = true; 367 | } 368 | } 369 | } 370 | } 371 | } 372 | 373 | std::vector Cloth::toVector() { 374 | std::vector clothCoordinates; 375 | clothCoordinates.reserve(particles.size()*3); 376 | for(auto& particle : particles) { 377 | clothCoordinates.push_back(particle.getPos().f[0]); 378 | clothCoordinates.push_back(particle.getPos().f[2]); 379 | clothCoordinates.push_back(-particle.getPos().f[1]); 380 | } 381 | return clothCoordinates; 382 | } 383 | 384 | 385 | void Cloth::saveToFile(std::string path) { 386 | std::string filepath = "cloth_nodes.txt"; 387 | 388 | if (path == "") { 389 | filepath = "cloth_nodes.txt"; 390 | } else { 391 | filepath = path; 392 | } 393 | 394 | std::ofstream f1(filepath.c_str()); 395 | 396 | if (!f1) 397 | return; 398 | 399 | for (std::size_t i = 0; i < particles.size(); i++) { 400 | f1 << std::fixed << std::setprecision(8) << particles[i].getPos().f[0] << " "<< particles[i].getPos().f[2] << " "<< -particles[i].getPos().f[1] << std::endl; 401 | } 402 | 403 | f1.close(); 404 | } 405 | 406 | void Cloth::saveMovableToFile(std::string path) { 407 | std::string filepath = "cloth_movable.txt"; 408 | 409 | if (path == "") { 410 | filepath = "cloth_movable.txt"; 411 | } else { 412 | filepath = path; 413 | } 414 | 415 | std::ofstream f1(filepath.c_str()); 416 | 417 | if (!f1) 418 | return; 419 | 420 | for (std::size_t i = 0; i < particles.size(); i++) { 421 | if (particles[i].isMovable()) { 422 | f1 << std::fixed << std::setprecision(8) << particles[i].getPos().f[0] << " " 423 | << particles[i].getPos().f[2] << " "<< -particles[i].getPos().f[1] << std::endl; 424 | } 425 | } 426 | 427 | f1.close(); 428 | } 429 | -------------------------------------------------------------------------------- /src/Cloth.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | /* 19 | * This source code is about a ground filtering algorithm for airborn LiDAR data 20 | * based on physical process simulations, specifically cloth simulation. 21 | * 22 | * this code is based on a Cloth Simulation Tutorial at the cg.alexandra.dk blog. 23 | * Thanks to Jesper Mosegaard (clothTutorial@jespermosegaard.dk) 24 | * 25 | * 26 | * 27 | * When applying the cloth simulation to LIDAR point filtering. A lot of features 28 | * have been added to the original source code, including 29 | * configuration file management 30 | * point cloud data read/write 31 | * point-to-point collsion detection 32 | * nearest point search structure from CGAL 33 | * addding a terrain class 34 | * 35 | * 36 | */ 37 | // using discrete steps (drop and pull) to approximate the physical process 38 | 39 | #ifndef _CLOTH_H_ 40 | #define _CLOTH_H_ 41 | 42 | 43 | #ifdef _WIN32 44 | # include 45 | #endif // ifdef _WIN32 46 | #include 47 | #include 48 | #include 49 | #ifdef CSF_USE_OPENMP 50 | #include 51 | #endif 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | #include "Vec3.h" 59 | #include "Particle.h" 60 | // #include 61 | // post processing is only for connected component which is large than 50 62 | #define MAX_PARTICLE_FOR_POSTPROCESSIN 50 63 | 64 | struct XY { 65 | XY(int x1, int y1) { 66 | x = x1; y = y1; 67 | } 68 | 69 | int x; 70 | int y; 71 | }; 72 | 73 | class Cloth { 74 | private: 75 | 76 | // total number of particles is num_particles_width * num_particles_height 77 | int constraint_iterations; 78 | 79 | int rigidness; 80 | double time_step; 81 | 82 | std::vector particles; // all particles that are part of this cloth 83 | 84 | double smoothThreshold; 85 | double heightThreshold; 86 | 87 | public: 88 | 89 | Vec3 origin_pos; 90 | double step_x, step_y; 91 | std::vector heightvals; // height values 92 | int num_particles_width; // number of particles in width direction 93 | int num_particles_height; // number of particles in height direction 94 | 95 | Particle* getParticle(int x, int y) { 96 | return &particles[y * num_particles_width + x]; 97 | } 98 | 99 | void makeConstraint(Particle *p1, Particle *p2) { 100 | p1->neighborsList.push_back(p2); 101 | p2->neighborsList.push_back(p1); 102 | } 103 | 104 | public: 105 | 106 | int getSize() { 107 | return num_particles_width * num_particles_height; 108 | } 109 | 110 | std::size_t get1DIndex(int x, int y) { 111 | return y * num_particles_width + x; 112 | } 113 | 114 | inline std::vector& getHeightvals() { 115 | return heightvals; 116 | } 117 | 118 | Particle* getParticle1d(int index) { 119 | return &particles[index]; 120 | } 121 | 122 | public: 123 | 124 | /* This is a important constructor for the entire system of 125 | * particles and constraints */ 126 | Cloth(const Vec3& origin_pos, 127 | int num_particles_width, 128 | int num_particles_height, 129 | double step_x, 130 | double step_y, 131 | double smoothThreshold, 132 | double heightThreshold, 133 | int rigidness, 134 | double time_step); 135 | 136 | /* this is an important methods where the time is progressed one 137 | * time step for the entire cloth. This includes calling 138 | * satisfyConstraint() for every constraint, and calling 139 | * timeStep() for all particles 140 | */ 141 | double timeStep(); 142 | 143 | /* used to add gravity (or any other arbitrary vector) to all 144 | * particles */ 145 | void addForce(const Vec3 direction); 146 | 147 | void terrCollision(); 148 | 149 | void movableFilter(); 150 | 151 | std::vector findUnmovablePoint(std::vector connected); 152 | 153 | void handle_slop_connected(std::vector edgePoints, 154 | std::vector connected, 155 | std::vector > neibors); 156 | 157 | void saveToFile(std::string path = ""); 158 | 159 | std::vector toVector(); 160 | 161 | void saveMovableToFile(std::string path = ""); 162 | }; 163 | 164 | 165 | #endif // ifndef _CLOTH_H_ 166 | -------------------------------------------------------------------------------- /src/Constraint.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "Constraint.h" 19 | 20 | 21 | void Constraint::satisfyConstraint(int constraintTimes) { 22 | Vec3 correctionVector(0, p2->pos.f[1] - p1->pos.f[1], 0); 23 | 24 | if (p1->isMovable() && p2->isMovable()) { 25 | // Lets make it half that length, so that we can move BOTH p1 and p2. 26 | Vec3 correctionVectorHalf = correctionVector * ( 27 | constraintTimes > 14 ? 0.5 : doubleMove[constraintTimes - 1] 28 | ); 29 | p1->offsetPos(correctionVectorHalf); 30 | p2->offsetPos(-correctionVectorHalf); 31 | } else if (p1->isMovable() && !p2->isMovable()) { 32 | Vec3 correctionVectorHalf = correctionVector * ( 33 | constraintTimes > 14 ? 1 : singleMove[constraintTimes - 1] 34 | ); 35 | p1->offsetPos(correctionVectorHalf); 36 | } else if (!p1->isMovable() && p2->isMovable()) { 37 | Vec3 correctionVectorHalf = correctionVector * ( 38 | constraintTimes > 14 ? 1 : singleMove[constraintTimes - 1] 39 | ); 40 | p2->offsetPos(-correctionVectorHalf); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/Constraint.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef _CONSTRAINT_H_ 19 | #define _CONSTRAINT_H_ 20 | 21 | 22 | #include "Vec3.h" 23 | #include "Particle.h" 24 | 25 | const double singleMove[14] = { 0.4, 0.64, 0.784, 0.8704, 0.92224, 0.95334, 0.97201, 0.9832, 0.98992, 0.99395, 0.99637, 0.99782, 0.99869, 0.99922 }; 26 | 27 | const double doubleMove[14] = { 0.4, 0.48, 0.496, 0.4992, 0.49984, 0.49997, 0.49999, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5 }; 28 | 29 | class Constraint { 30 | private: 31 | 32 | double rest_distance; // the length between particle p1 and p2 in rest configuration 33 | 34 | public: 35 | 36 | Particle *p1, *p2; // the two particles that are connected through this constraint 37 | 38 | Constraint(Particle *p1, Particle *p2) : p1(p1), p2(p2) {} 39 | 40 | /* This is one of the important methods, where a single constraint 41 | * between two particles p1 and p2 is solved the method is called 42 | * by Cloth.time_step() many times per frame*/ 43 | void satisfyConstraint(int constraintTimes); 44 | }; 45 | 46 | 47 | #endif // ifndef _CONSTRAINT_H_ 48 | -------------------------------------------------------------------------------- /src/Particle.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "Particle.h" 19 | 20 | /* This is one of the important methods, where the time is progressed 21 | * a single step size (TIME_STEPSIZE) The method is called by 22 | * Cloth.time_step() Given the equation "force = mass * acceleration" 23 | * the next position is found through verlet integration*/ 24 | void Particle::timeStep() { 25 | if (movable) { 26 | Vec3 temp = pos; 27 | pos = pos + (pos - old_pos) * (1.0 - DAMPING) + acceleration * time_step2; 28 | old_pos = temp; 29 | } 30 | } 31 | 32 | void Particle::satisfyConstraintSelf(int constraintTimes) { 33 | Particle *p1 = this; 34 | 35 | for (std::size_t i = 0; i < neighborsList.size(); i++) { 36 | Particle *p2 = neighborsList[i]; 37 | Vec3 correctionVector(0, p2->pos.f[1] - p1->pos.f[1], 0); 38 | 39 | if (p1->isMovable() && p2->isMovable()) { 40 | // Lets make it half that length, so that we can move BOTH p1 and p2. 41 | Vec3 correctionVectorHalf = correctionVector * ( 42 | constraintTimes > 14 ? 0.5 : doubleMove1[constraintTimes] 43 | ); 44 | p1->offsetPos(correctionVectorHalf); 45 | p2->offsetPos(-correctionVectorHalf); 46 | } else if (p1->isMovable() && !p2->isMovable()) { 47 | Vec3 correctionVectorHalf = correctionVector * ( 48 | constraintTimes > 14 ? 1 : singleMove1[constraintTimes] 49 | ); 50 | p1->offsetPos(correctionVectorHalf); 51 | } else if (!p1->isMovable() && p2->isMovable()) { 52 | Vec3 correctionVectorHalf = correctionVector * ( 53 | constraintTimes > 14 ? 1 : singleMove1[constraintTimes] 54 | ); 55 | p2->offsetPos(-correctionVectorHalf); 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /src/Particle.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef _PARTICLE_H_ 19 | #define _PARTICLE_H_ 20 | 21 | #include "Vec3.h" 22 | #include 23 | /* Some physics constants */ 24 | #define DAMPING 0.01 // how much to damp the cloth simulation each frame 25 | #define MAX_INF 9999999999 26 | #define MIN_INF -9999999999 27 | 28 | /* 29 | Initially, we have to make modifications of particle positions for each constraint(constraintTimes = rigidness), However, to save computation time, we 30 | precomputed the total displacement of a particle for all constraintTimes. 31 | For singleMove1, which means one of the two particles is unmovable, then we move the other one only: 32 | if constraintTimes = 0: singleMove1 = 0 33 | if constraintTimes = 1: singleMove1 = 0.3, i.e., each time we move 0.3 (scale factor of the total distance) for a particle towards the other one 34 | if constraintTimes = 2: singleMove1 = (1-0.3)*0.3+0.3 = 0.51 35 | if constraintTimes = 3: singleMove1 = (1-0.51)*0.3+0.51 = 0.657 36 | ... 37 | 38 | For doubleMove1, we move both of the two particles towards each other. 39 | if constraintTimes = 0: singleMove2 = 0 40 | if constraintTimes = 1: singleMove2 = 0.3, i.e., each time we move 0.3 (scale factor of the total distance) for the two particles towards each other 41 | if constraintTimes = 2: singleMove2 = (1-0.3*2)*0.3+0.3 = 0.42 42 | if constraintTimes = 3: singleMove2 = (1-0.42*2)*0.3+0.42 = 0.468 43 | ... 44 | 45 | */ 46 | const double singleMove1[15] = { 0, 0.3, 0.51, 0.657, 0.7599, 0.83193, 0.88235, 0.91765, 0.94235, 0.95965, 0.97175, 0.98023, 0.98616, 0.99031, 0.99322 }; 47 | const double doubleMove1[15] = { 0, 0.3, 0.42, 0.468, 0.4872, 0.4949, 0.498, 0.4992, 0.4997, 0.4999, 0.4999, 0.5, 0.5, 0.5, 0.5 }; 48 | 49 | /* The particle class represents a particle of mass that can move 50 | * around in 3D space*/ 51 | class Particle { 52 | private: 53 | 54 | bool movable; // can the particle move or not ? used to pin parts of the cloth 55 | double mass; // the mass of the particle (is always 1 in this example) 56 | Vec3 acceleration; // a vector representing the current acceleration of the particle 57 | Vec3 accumulated_normal; // an accumulated normal (i.e. non normalized), used for OpenGL soft shading 58 | double time_step2; 59 | 60 | public: 61 | 62 | // These two memebers are used in the process of edge smoothing after 63 | // the cloth simulation step. 64 | Vec3 pos; // the current position of the particle in 3D space 65 | // the position of the particle in the previous time step, used as 66 | // part of the verlet numerical integration scheme 67 | Vec3 old_pos; 68 | bool isVisited; 69 | int neibor_count; 70 | int pos_x; // position in the cloth grid 71 | int pos_y; 72 | int c_pos; 73 | 74 | std::vector neighborsList; 75 | 76 | std::vector correspondingLidarPointList; 77 | std::size_t nearestPointIndex; 78 | double nearestPointHeight; 79 | double tmpDist; 80 | void satisfyConstraintSelf(int constraintTimes); 81 | 82 | public: 83 | 84 | Particle(Vec3 pos, double time_step) : 85 | movable(true), 86 | mass(1), 87 | acceleration(Vec3(0, 0, 0)), 88 | accumulated_normal(Vec3(0, 0, 0)), 89 | time_step2(time_step), 90 | pos(pos), 91 | old_pos(pos) { 92 | isVisited = false; 93 | neibor_count = 0; 94 | pos_x = 0; 95 | pos_y = 0; 96 | c_pos = 0; 97 | nearestPointHeight = MIN_INF; 98 | tmpDist = MAX_INF; 99 | } 100 | 101 | Particle() : 102 | movable(true), 103 | mass(1), 104 | acceleration(Vec3(0, 0, 0)), 105 | accumulated_normal(Vec3(0, 0, 0)), 106 | pos(Vec3(0, 0, 0)), 107 | old_pos(Vec3(0, 0, 0)) { 108 | isVisited = false; 109 | neibor_count = 0; 110 | pos_x = 0; 111 | pos_y = 0; 112 | c_pos = 0; 113 | nearestPointHeight = MIN_INF; 114 | tmpDist = MAX_INF; 115 | } 116 | 117 | 118 | bool isMovable() { 119 | return movable; 120 | } 121 | 122 | void addForce(Vec3 f) { 123 | acceleration += f / mass; 124 | } 125 | 126 | /* This is one of the important methods, where the time is 127 | * progressed a single step size (TIME_STEPSIZE) The method is 128 | * called by Cloth.time_step()*/ 129 | void timeStep(); 130 | 131 | Vec3& getPos() { 132 | return pos; 133 | } 134 | 135 | Vec3 getPosCopy() { 136 | return pos; 137 | } 138 | 139 | void resetAcceleration() { 140 | acceleration = Vec3(0, 0, 0); 141 | } 142 | 143 | void offsetPos(const Vec3 v) { 144 | if (movable) pos += v; 145 | } 146 | 147 | void makeUnmovable() { 148 | movable = false; 149 | } 150 | 151 | void addToNormal(Vec3 normal) { 152 | accumulated_normal += normal.normalized(); 153 | } 154 | 155 | Vec3& getNormal() { 156 | // note: The normal is not unit length 157 | return accumulated_normal; 158 | } 159 | 160 | void resetNormal() { 161 | accumulated_normal = Vec3(0, 0, 0); 162 | } 163 | 164 | void printself(std::string s = "") { 165 | std::cout << s << ": " << this->getPos().f[0] << " movable: " << this->movable << std::endl; 166 | } 167 | }; 168 | 169 | 170 | #endif // ifndef _PARTICLE_H_ 171 | -------------------------------------------------------------------------------- /src/Rasterization.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "Rasterization.h" 19 | #include 20 | 21 | 22 | double Rasterization::findHeightValByScanline(Particle *p, Cloth& cloth) { 23 | int xpos = p->pos_x; 24 | int ypos = p->pos_y; 25 | 26 | for (int i = xpos + 1; i < cloth.num_particles_width; i++) { 27 | double crresHeight = cloth.getParticle(i, ypos)->nearestPointHeight; 28 | 29 | if (crresHeight > MIN_INF) 30 | return crresHeight; 31 | } 32 | 33 | for (int i = xpos - 1; i >= 0; i--) { 34 | double crresHeight = cloth.getParticle(i, ypos)->nearestPointHeight; 35 | 36 | if (crresHeight > MIN_INF) 37 | return crresHeight; 38 | } 39 | 40 | for (int j = ypos - 1; j >= 0; j--) { 41 | double crresHeight = cloth.getParticle(xpos, j)->nearestPointHeight; 42 | 43 | if (crresHeight > MIN_INF) 44 | return crresHeight; 45 | } 46 | 47 | for (int j = ypos + 1; j < cloth.num_particles_height; j++) { 48 | double crresHeight = cloth.getParticle(xpos, j)->nearestPointHeight; 49 | 50 | if (crresHeight > MIN_INF) 51 | return crresHeight; 52 | } 53 | 54 | return findHeightValByNeighbor(p); 55 | } 56 | 57 | 58 | double Rasterization::findHeightValByNeighbor(Particle *p) { 59 | std::queue nqueue; 60 | std::vector pbacklist; 61 | int neiborsize = p->neighborsList.size(); 62 | 63 | for (int i = 0; i < neiborsize; i++) { 64 | p->isVisited = true; 65 | nqueue.push(p->neighborsList[i]); 66 | } 67 | 68 | // iterate over the nqueue 69 | while (!nqueue.empty()) { 70 | Particle *pneighbor = nqueue.front(); 71 | nqueue.pop(); 72 | pbacklist.push_back(pneighbor); 73 | 74 | if (pneighbor->nearestPointHeight > MIN_INF) { 75 | for (std::size_t i = 0; i < pbacklist.size(); i++) 76 | pbacklist[i]->isVisited = false; 77 | 78 | while (!nqueue.empty()) { 79 | Particle *pp = nqueue.front(); 80 | pp->isVisited = false; 81 | nqueue.pop(); 82 | } 83 | 84 | return pneighbor->nearestPointHeight; 85 | } else { 86 | int nsize = pneighbor->neighborsList.size(); 87 | 88 | for (int i = 0; i < nsize; i++) { 89 | Particle *ptmp = pneighbor->neighborsList[i]; 90 | 91 | if (!ptmp->isVisited) { 92 | ptmp->isVisited = true; 93 | nqueue.push(ptmp); 94 | } 95 | } 96 | } 97 | } 98 | 99 | return MIN_INF; 100 | } 101 | 102 | void Rasterization::RasterTerrian(Cloth & cloth, 103 | csf::PointCloud& pc, 104 | std::vector & heightVal) { 105 | 106 | for (std::size_t i = 0; i < pc.size(); i++) { 107 | double pc_x = pc[i].x; 108 | double pc_z = pc[i].z; 109 | 110 | double deltaX = pc_x - cloth.origin_pos.f[0]; 111 | double deltaZ = pc_z - cloth.origin_pos.f[2]; 112 | int col = int(deltaX / cloth.step_x + 0.5); 113 | int row = int(deltaZ / cloth.step_y + 0.5); 114 | 115 | if ((col >= 0) && (row >= 0)) { 116 | Particle *pt = cloth.getParticle(col, row); 117 | pt->correspondingLidarPointList.push_back(i); 118 | double pc2particleDist = SQUARE_DIST( 119 | pc_x, pc_z, 120 | pt->getPos().f[0], 121 | pt->getPos().f[2] 122 | ); 123 | 124 | if (pc2particleDist < pt->tmpDist) { 125 | pt->tmpDist = pc2particleDist; 126 | pt->nearestPointHeight = pc[i].y; 127 | pt->nearestPointIndex = i; 128 | } 129 | } 130 | } 131 | heightVal.resize(cloth.getSize()); 132 | 133 | // #ifdef CSF_USE_OPENMP 134 | // #pragma omp parallel for 135 | // #endif 136 | for (int i = 0; i < cloth.getSize(); i++) { 137 | Particle *pcur = cloth.getParticle1d(i); 138 | double nearestHeight = pcur->nearestPointHeight; 139 | 140 | if (nearestHeight > MIN_INF) { 141 | heightVal[i] = nearestHeight; 142 | } else { 143 | heightVal[i] = findHeightValByScanline(pcur, cloth); 144 | } 145 | } 146 | } 147 | -------------------------------------------------------------------------------- /src/Rasterization.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef _KNN_H_ 19 | #define _KNN_H_ 20 | 21 | #include "point_cloud.h" 22 | #include "Cloth.h" 23 | 24 | #define SQUARE_DIST(x1, y1, x2, y2) \ 25 | (((x1) - (x2)) * ((x1) - (x2)) + ((y1) - (y2)) * ((y1) - (y2))) 26 | 27 | 28 | class Rasterization { 29 | public: 30 | 31 | Rasterization() {} 32 | ~Rasterization() {} 33 | 34 | // for a cloth particle, if no corresponding lidar point are found. 35 | // the heightval are set as its neighbor's 36 | double static findHeightValByNeighbor(Particle *p); 37 | double static findHeightValByScanline(Particle *p, Cloth& cloth); 38 | 39 | void static RasterTerrian(Cloth & cloth, 40 | csf::PointCloud& pc, 41 | std::vector & heightVal); 42 | }; 43 | 44 | #endif // ifndef _KNN_H_ 45 | -------------------------------------------------------------------------------- /src/Vec3.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef _VEC3_H_ 19 | #define _VEC3_H_ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | // a minimal vector class of 3 doubles and overloaded math operators 29 | class Vec3 { 30 | public: 31 | 32 | double f[3]; 33 | 34 | Vec3(double x, double y, double z) { 35 | f[0] = x; 36 | f[1] = y; 37 | f[2] = z; 38 | } 39 | 40 | Vec3() {} 41 | 42 | double length() { 43 | return sqrt(f[0] * f[0] + f[1] * f[1] + f[2] * f[2]); 44 | } 45 | 46 | Vec3 normalized() { 47 | double l = length(); 48 | 49 | return Vec3(f[0] / l, f[1] / l, f[2] / l); 50 | } 51 | 52 | void operator+=(const Vec3& v) { 53 | f[0] += v.f[0]; 54 | f[1] += v.f[1]; 55 | f[2] += v.f[2]; 56 | } 57 | 58 | Vec3 operator/(const double& a) { 59 | return Vec3(f[0] / a, f[1] / a, f[2] / a); 60 | } 61 | 62 | Vec3 operator-(const Vec3& v) { 63 | return Vec3(f[0] - v.f[0], f[1] - v.f[1], f[2] - v.f[2]); 64 | } 65 | 66 | Vec3 operator+(const Vec3& v) { 67 | return Vec3(f[0] + v.f[0], f[1] + v.f[1], f[2] + v.f[2]); 68 | } 69 | 70 | Vec3 operator*(const double& a) { 71 | return Vec3(f[0] * a, f[1] * a, f[2] * a); 72 | } 73 | 74 | Vec3 operator-() { 75 | return Vec3(-f[0], -f[1], -f[2]); 76 | } 77 | 78 | Vec3 cross(const Vec3& v) { 79 | return Vec3( 80 | f[1] * v.f[2] - f[2] * v.f[1], 81 | f[2] * v.f[0] - f[0] * v.f[2], 82 | f[0] * v.f[1] - f[1] * v.f[0] 83 | ); 84 | } 85 | 86 | double dot(const Vec3& v) { 87 | return f[0] * v.f[0] + f[1] * v.f[1] + f[2] * v.f[2]; 88 | } 89 | }; 90 | 91 | 92 | #endif // ifndef _VEC3_H_ 93 | -------------------------------------------------------------------------------- /src/XYZReader.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "XYZReader.h" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | void read_xyz(std::string fname, csf::PointCloud& pointcloud) { 27 | std::ifstream fin(fname.c_str(), std::ios::in); 28 | char line[500]; 29 | std::string x, y, z; 30 | 31 | while (fin.getline(line, sizeof(line))) { 32 | std::stringstream words(line); 33 | 34 | words >> x; 35 | words >> y; 36 | words >> z; 37 | 38 | csf::Point point; 39 | point.x = atof(x.c_str()); 40 | point.y = -atof(z.c_str()); 41 | point.z = atof(y.c_str()); 42 | 43 | pointcloud.push_back(point); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /src/XYZReader.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef XYZ_READER_H_ 19 | #define XYZ_READER_H_ 20 | 21 | 22 | #include 23 | #include 24 | 25 | 26 | #include "point_cloud.h" 27 | 28 | void read_xyz(std::string fname, csf::PointCloud& pointcloud); 29 | 30 | 31 | #endif // ifndef XYZ_READER_H_ 32 | -------------------------------------------------------------------------------- /src/c2cdist.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "c2cdist.h" 19 | #include 20 | 21 | 22 | void c2cdist::calCloud2CloudDist(Cloth & cloth, 23 | csf::PointCloud & pc, 24 | std::vector& groundIndexes, 25 | std::vector& offGroundIndexes) { 26 | groundIndexes.resize(0); 27 | offGroundIndexes.resize(0); 28 | 29 | for (std::size_t i = 0; i < pc.size(); i++) { 30 | double pc_x = pc[i].x; 31 | double pc_z = pc[i].z; 32 | 33 | double deltaX = pc_x - cloth.origin_pos.f[0]; 34 | double deltaZ = pc_z - cloth.origin_pos.f[2]; 35 | 36 | int col0 = int(deltaX / cloth.step_x); 37 | int row0 = int(deltaZ / cloth.step_y); 38 | int col1 = col0 + 1; 39 | int row1 = row0; 40 | int col2 = col0 + 1; 41 | int row2 = row0 + 1; 42 | int col3 = col0; 43 | int row3 = row0 + 1; 44 | 45 | double subdeltaX = (deltaX - col0 * cloth.step_x) / cloth.step_x; 46 | double subdeltaZ = (deltaZ - row0 * cloth.step_y) / cloth.step_y; 47 | 48 | double fxy 49 | = cloth.getParticle(col0, row0)->pos.f[1] * (1 - subdeltaX) * (1 - subdeltaZ) + 50 | cloth.getParticle(col3, row3)->pos.f[1] * (1 - subdeltaX) * subdeltaZ + 51 | cloth.getParticle(col2, row2)->pos.f[1] * subdeltaX * subdeltaZ + 52 | cloth.getParticle(col1, row1)->pos.f[1] * subdeltaX * (1 - subdeltaZ); 53 | double height_var = fxy - pc[i].y; 54 | 55 | if (std::fabs(height_var) < class_treshold) { 56 | groundIndexes.push_back(i); 57 | } else { 58 | offGroundIndexes.push_back(i); 59 | } 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/c2cdist.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #ifndef _C2CDIST_H_ 19 | #define _C2CDIST_H_ 20 | 21 | 22 | #include "Cloth.h" 23 | #include "point_cloud.h" 24 | 25 | 26 | class c2cdist { 27 | public: 28 | 29 | c2cdist(double threshold) : class_treshold(threshold) {} 30 | 31 | ~c2cdist() {} 32 | 33 | public: 34 | 35 | void calCloud2CloudDist(Cloth & cloth, 36 | csf::PointCloud & pc, 37 | std::vector& groundIndexes, 38 | std::vector& offGroundIndexes); 39 | 40 | private: 41 | 42 | double class_treshold; // 43 | }; 44 | 45 | 46 | #endif // ifndef _C2CDIST_H_ 47 | -------------------------------------------------------------------------------- /src/point_cloud.cpp: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | #include "point_cloud.h" 19 | 20 | 21 | void csf::PointCloud::computeBoundingBox(Point& bbMin, Point& bbMax) { 22 | if (empty()) { 23 | bbMin = bbMax = Point(); 24 | return; 25 | } 26 | 27 | bbMin = bbMax = at(0); 28 | 29 | for (std::size_t i = 1; i < size(); i++) { // zwm 30 | const csf::Point& P = at(i); 31 | 32 | for (int d = 0; d < 3; ++d) { 33 | if (P.u[d] < bbMin.u[d]) { 34 | bbMin.u[d] = P.u[d]; 35 | } else if (P.u[d] > bbMax.u[d]) { 36 | bbMax.u[d] = P.u[d]; 37 | } 38 | } 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/point_cloud.h: -------------------------------------------------------------------------------- 1 | // ====================================================================================== 2 | // Copyright 2017 State Key Laboratory of Remote Sensing Science, 3 | // Institute of Remote Sensing Science and Engineering, Beijing Normal University 4 | 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // ====================================================================================== 17 | 18 | // ####################################################################################### 19 | // # # 20 | // # CSF: Airborne LiDAR filtering based on Cloth Simulation # 21 | // # # 22 | // # Please cite the following paper, If you use this software in your work. # 23 | // # # 24 | // # Zhang W, Qi J, Wan P, Wang H, Xie D, Wang X, Yan G. An Easy-to-Use Airborne LiDAR # 25 | // # Data Filtering Method Based on Cloth Simulation. Remote Sensing. 2016; 8(6):501. # 26 | // # (http://ramm.bnu.edu.cn/) # 27 | // # # 28 | // # Wuming Zhang; Jianbo Qi; Peng Wan; Hongtao Wang # 29 | // # # 30 | // # contact us: 2009zwm@gmail.com; wpqjbzwm@126.com # 31 | // # # 32 | // ####################################################################################### 33 | 34 | #ifndef _POINT_CLOUD_H_ 35 | #define _POINT_CLOUD_H_ 36 | 37 | #include 38 | 39 | namespace csf { 40 | 41 | struct Point { 42 | union { 43 | struct { 44 | double x; 45 | double y; 46 | double z; 47 | }; 48 | double u[3]; 49 | }; 50 | 51 | Point() : x(0), y(0), z(0) {} 52 | Point(double x, double y, double z) : x(x), y(y), z(z) {} 53 | }; 54 | 55 | class PointCloud : public std::vector{ 56 | public: 57 | 58 | void computeBoundingBox(Point& bbMin, Point& bbMax); 59 | }; 60 | 61 | } 62 | 63 | 64 | #endif // ifndef _POINT_CLOUD_H_ 65 | --------------------------------------------------------------------------------