├── .vscode └── settings.json ├── 备份 ├── main.cpp ├── ztKdTree.h └── ztKdTree.cpp ├── 20180825_gpu_knn ├── ztGpuKnn.cu ├── ztGpuKnn.h ├── ztKdTree.h ├── ztStatisticFilterNoisePoint.cpp ├── ztStatisticFilterNoisePoint.h ├── 20180825_gpu_knn.vcxproj ├── main.cpp └── ztKdTree.cpp ├── max_heap.cpp ├── 20180825_gpu_knn.sln ├── .gitattributes └── .gitignore /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "C_Cpp.errorSquiggles": "Disabled" 3 | } -------------------------------------------------------------------------------- /备份/main.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/备份/main.cpp -------------------------------------------------------------------------------- /备份/ztKdTree.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/备份/ztKdTree.h -------------------------------------------------------------------------------- /备份/ztKdTree.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/备份/ztKdTree.cpp -------------------------------------------------------------------------------- /20180825_gpu_knn/ztGpuKnn.cu: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/20180825_gpu_knn/ztGpuKnn.cu -------------------------------------------------------------------------------- /20180825_gpu_knn/ztGpuKnn.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/20180825_gpu_knn/ztGpuKnn.h -------------------------------------------------------------------------------- /20180825_gpu_knn/ztKdTree.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/20180825_gpu_knn/ztKdTree.h -------------------------------------------------------------------------------- /20180825_gpu_knn/ztStatisticFilterNoisePoint.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/20180825_gpu_knn/ztStatisticFilterNoisePoint.cpp -------------------------------------------------------------------------------- /20180825_gpu_knn/ztStatisticFilterNoisePoint.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whuxzhsun/quick-kdtree-with-form-of-array/HEAD/20180825_gpu_knn/ztStatisticFilterNoisePoint.h -------------------------------------------------------------------------------- /max_heap.cpp: -------------------------------------------------------------------------------- 1 | 2 | void buildMaxHeap(int arr[], int n) 3 | { 4 | for (int i = n / 2 - 1; i >= 0; i--) 5 | { 6 | for (int son = i * 2; son <= n; son *= 2) 7 | { 8 | if (son + 1 < n && arr[son] < arr[son + 1]) 9 | son++; 10 | 11 | if (arr[i] < arr[son]) // 如果父节点小于子节点，则交换 12 | { 13 | int temp = arr[i]; 14 | arr[i] = arr[son]; 15 | arr[son] = temp; 16 | } 17 | } 18 | } 19 | } 20 | 21 | // 删除堆顶，将最后一个元素移到堆顶，并且将最后一个元素置为空 22 | void pop(int arr[], int n) 23 | { 24 | arr[0] = arr[n - 1]; 25 | arr[n - 1] = -1; 26 | } 27 | 28 | // 遍历堆，寻找合适的插入位置 29 | void push(int arr[], int n, int x) 30 | { 31 | int i = n - 1; 32 | while (i != 0 && x > arr[i / 2]) 33 | { 34 | arr[i] = arr[i / 2]; 35 | i /= 2; 36 | } 37 | 38 | arr[i] = x; 39 | } -------------------------------------------------------------------------------- /20180825_gpu_knn.sln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2013 4 | VisualStudioVersion = 12.0.40629.0 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "20180825_gpu_knn", "20180825_gpu_knn\20180825_gpu_knn.vcxproj", "{28396411-C6EA-425E-A5F2-38A0B06CB064}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|Win32 = Debug|Win32 11 | Debug|x64 = Debug|x64 12 | Release|Win32 = Release|Win32 13 | Release|x64 = Release|x64 14 | EndGlobalSection 15 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 16 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Debug|Win32.ActiveCfg = Debug|Win32 17 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Debug|Win32.Build.0 = Debug|Win32 18 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Debug|x64.ActiveCfg = Debug|x64 19 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Debug|x64.Build.0 = Debug|x64 20 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Release|Win32.ActiveCfg = Release|Win32 21 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Release|Win32.Build.0 = Release|Win32 22 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Release|x64.ActiveCfg = Release|x64 23 | {28396411-C6EA-425E-A5F2-38A0B06CB064}.Release|x64.Build.0 = Release|x64 24 | EndGlobalSection 25 | GlobalSection(SolutionProperties) = preSolution 26 | HideSolutionNode = FALSE 27 | EndGlobalSection 28 | EndGlobal 29 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Set default behavior to automatically normalize line endings. 3 | ############################################################################### 4 | * text=auto 5 | 6 | ############################################################################### 7 | # Set default behavior for command prompt diff. 8 | # 9 | # This is need for earlier builds of msysgit that does not have it on by 10 | # default for csharp files. 11 | # Note: This is only used by command line 12 | ############################################################################### 13 | #*.cs diff=csharp 14 | 15 | ############################################################################### 16 | # Set the merge driver for project and solution files 17 | # 18 | # Merging from the command prompt will add diff markers to the files if there 19 | # are conflicts (Merging from VS is not affected by the settings below, in VS 20 | # the diff markers are never inserted). Diff markers may cause the following 21 | # file extensions to fail to load in VS. An alternative would be to treat 22 | # these files as binary and thus will always conflict and require user 23 | # intervention with every merge. To do so, just uncomment the entries below 24 | ############################################################################### 25 | #*.sln merge=binary 26 | #*.csproj merge=binary 27 | #*.vbproj merge=binary 28 | #*.vcxproj merge=binary 29 | #*.vcproj merge=binary 30 | #*.dbproj merge=binary 31 | #*.fsproj merge=binary 32 | #*.lsproj merge=binary 33 | #*.wixproj merge=binary 34 | #*.modelproj merge=binary 35 | #*.sqlproj merge=binary 36 | #*.wwaproj merge=binary 37 | 38 | ############################################################################### 39 | # behavior for image files 40 | # 41 | # image files are treated as binary by default. 42 | ############################################################################### 43 | #*.jpg binary 44 | #*.png binary 45 | #*.gif binary 46 | 47 | ############################################################################### 48 | # diff behavior for common document formats 49 | # 50 | # Convert binary document formats to text before diffing them. This feature 51 | # is only available from the command line. Turn it on by uncommenting the 52 | # entries below. 53 | ############################################################################### 54 | #*.doc diff=astextplain 55 | #*.DOC diff=astextplain 56 | #*.docx diff=astextplain 57 | #*.DOCX diff=astextplain 58 | #*.dot diff=astextplain 59 | #*.DOT diff=astextplain 60 | #*.pdf diff=astextplain 61 | #*.PDF diff=astextplain 62 | #*.rtf diff=astextplain 63 | #*.RTF diff=astextplain 64 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ## Ignore Visual Studio temporary files, build results, and 2 | ## files generated by popular Visual Studio add-ons. 3 | 4 | # User-specific files 5 | *.suo 6 | *.user 7 | *.sln.docstates 8 | 9 | # Build results 10 | 11 | [Dd]ebug/ 12 | [Rr]elease/ 13 | x64/ 14 | build/ 15 | [Bb]in/ 16 | [Oo]bj/ 17 | ipch/ 18 | Win32/ 19 | 20 | # Enable "build/" folder in the NuGet Packages folder since NuGet packages use it for MSBuild targets 21 | !packages/*/build/ 22 | 23 | # MSTest test Results 24 | [Tt]est[Rr]esult*/ 25 | [Bb]uild[Ll]og.* 26 | 27 | *_i.c 28 | *_p.c 29 | *.ilk 30 | *.meta 31 | *.obj 32 | *.pch 33 | *.pdb 34 | *.pgc 35 | *.pgd 36 | *.rsp 37 | *.sbr 38 | *.tlb 39 | *.tli 40 | *.tlh 41 | *.tmp 42 | *.tmp_proj 43 | *.log 44 | *.vspscc 45 | *.vssscc 46 | .builds 47 | *.pidb 48 | *.log 49 | *.scc 50 | 51 | # Visual C++ cache files 52 | ipch/ 53 | *.aps 54 | *.ncb 55 | *.opensdf 56 | *.sdf 57 | *.cachefile 58 | 59 | # Visual Studio profiler 60 | *.psess 61 | *.vsp 62 | *.vspx 63 | 64 | # Guidance Automation Toolkit 65 | *.gpState 66 | 67 | # ReSharper is a .NET coding add-in 68 | _ReSharper*/ 69 | *.[Rr]e[Ss]harper 70 | 71 | # TeamCity is a build add-in 72 | _TeamCity* 73 | 74 | # DotCover is a Code Coverage Tool 75 | *.dotCover 76 | 77 | # NCrunch 78 | *.ncrunch* 79 | .*crunch*.local.xml 80 | 81 | # Installshield output folder 82 | [Ee]xpress/ 83 | 84 | # DocProject is a documentation generator add-in 85 | DocProject/buildhelp/ 86 | DocProject/Help/*.HxT 87 | DocProject/Help/*.HxC 88 | DocProject/Help/*.hhc 89 | DocProject/Help/*.hhk 90 | DocProject/Help/*.hhp 91 | DocProject/Help/Html2 92 | DocProject/Help/html 93 | 94 | # Click-Once directory 95 | publish/ 96 | 97 | # Publish Web Output 98 | *.Publish.xml 99 | 100 | # NuGet Packages Directory 101 | ## TODO: If you have NuGet Package Restore enabled, uncomment the next line 102 | #packages/ 103 | 104 | # Windows Azure Build Output 105 | csx 106 | *.build.csdef 107 | 108 | # Windows Store app package directory 109 | AppPackages/ 110 | 111 | # Others 112 | sql/ 113 | *.Cache 114 | ClientBin/ 115 | [Ss]tyle[Cc]op.* 116 | ~$* 117 | *~ 118 | *.dbmdl 119 | *.[Pp]ublish.xml 120 | *.pfx 121 | *.publishsettings 122 | 123 | # RIA/Silverlight projects 124 | Generated_Code/ 125 | 126 | # Backup & report files from converting an old project file to a newer 127 | # Visual Studio version. Backup files are not needed, because we have git ;-) 128 | _UpgradeReport_Files/ 129 | Backup*/ 130 | UpgradeLog*.XML 131 | UpgradeLog*.htm 132 | 133 | # SQL Server files 134 | App_Data/*.mdf 135 | App_Data/*.ldf 136 | 137 | 138 | #LightSwitch generated files 139 | GeneratedArtifacts/ 140 | _Pvt_Extensions/ 141 | ModelManifest.xml 142 | 143 | # ========================= 144 | # Windows detritus 145 | # ========================= 146 | 147 | # Windows image file caches 148 | Thumbs.db 149 | ehthumbs.db 150 | 151 | # Folder config file 152 | Desktop.ini 153 | 154 | # Recycle Bin used on file shares 155 | $RECYCLE.BIN/ 156 | 157 | # Mac desktop service store files 158 | .DS_Store 159 | -------------------------------------------------------------------------------- /20180825_gpu_knn/20180825_gpu_knn.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Debug 10 | x64 11 | 12 | 13 | Release 14 | Win32 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {28396411-C6EA-425E-A5F2-38A0B06CB064} 23 | _20180825_gpu_knn 24 | 25 | 26 | 27 | Application 28 | true 29 | MultiByte 30 | v120 31 | 32 | 33 | Application 34 | true 35 | MultiByte 36 | v120 37 | 38 | 39 | Application 40 | false 41 | true 42 | MultiByte 43 | v120 44 | 45 | 46 | Application 47 | false 48 | true 49 | MultiByte 50 | v120 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | true 71 | 72 | 73 | true 74 | 75 | 76 | E:\SourceCode\boost_1_62_0_64;E:\SourceCode\las_lib2;$(IncludePath) 77 | E:\SourceCode\boost_1_62_0_64\stage\lib_2013_64;E:\SourceCode\las_lib2\release_64_vs2013_boost_1_62_0_64;$(LibraryPath) 78 | 79 | 80 | 81 | Level3 82 | Disabled 83 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 84 | 85 | 86 | true 87 | Console 88 | cudart.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) 89 | 90 | 91 | echo copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 92 | copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 93 | 94 | 95 | 96 | 97 | Level3 98 | Disabled 99 | WIN32;WIN64;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 100 | 101 | 102 | true 103 | Console 104 | cudart.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) 105 | 106 | 107 | echo copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 108 | copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 109 | 110 | 111 | 64 112 | 113 | 114 | 115 | 116 | Level3 117 | MaxSpeed 118 | true 119 | true 120 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 121 | 122 | 123 | true 124 | true 125 | true 126 | Console 127 | cudart.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) 128 | 129 | 130 | echo copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 131 | copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 132 | 133 | 134 | 135 | 136 | Level3 137 | Disabled 138 | true 139 | true 140 | WIN32;WIN64;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 141 | true 142 | 143 | 144 | true 145 | true 146 | true 147 | Console 148 | cudart.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;liblas.lib;%(AdditionalDependencies) 149 | 150 | 151 | echo copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 152 | copy "$(CudaToolkitBinDir)\cudart*.dll" "$(OutDir)" 153 | 154 | 155 | 64 156 | false 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | -------------------------------------------------------------------------------- /20180825_gpu_knn/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "ztKdTree.h" 9 | #include "ztGpuKnn.h" 10 | #include 11 | #include 12 | #include "ztStatisticFilterNoisePoint.h" 13 | 14 | using namespace std; 15 | 16 | struct LasFormat3Point 17 | { 18 | char scanAng; 19 | unsigned char c; 20 | unsigned short id; 21 | unsigned short scandir; 22 | int r, g, b, i; 23 | double x, y, z, t; 24 | }; 25 | 26 | 27 | int testKnn(); 28 | int testGpuKnn(); 29 | int testFilter(); 30 | 31 | int main() 32 | { 33 | // testKnn(); 34 | testFilter(); 35 | 36 | // double sum = 0; 37 | 38 | // clock_t t = clock(); 39 | // 40 | // #pragma omp parallel for num_threads(omp_get_num_procs()), reduction(+:sum) 41 | // for (int j = 0; j < 10000000; j++) 42 | // { 43 | // sum += j; 44 | // 45 | // /*printf("\n%d\n", sum);*/ 46 | // } 47 | // 48 | // printf("\tcost time of computing: %.3f\n", (clock() - t) / 1000.0); 49 | 50 | // bool *test = new bool[8]; 51 | // 52 | // memset(test, 1, 8 * sizeof(bool)); 53 | // 54 | // for (int i = 0; i < 8; i++) 55 | // { 56 | // if (test[i]) 57 | // { 58 | // printf("%d\n", i); 59 | // } 60 | // } 61 | // 62 | // delete[] test; 63 | 64 | return 0; 65 | } 66 | 67 | int testKnn() 68 | { 69 | printf("Test CPU:\n"); 70 | std::string inFile("D:\\20180828_R1000_sanding\\las\\180828_022843_3.las"); 71 | 72 | std::ifstream ifs(inFile, std::ios::in | std::ios::binary); 73 | if (!ifs.is_open()) 74 | { 75 | std::cout << "can't open " << inFile << endl; 76 | 77 | return 1; 78 | } 79 | 80 | liblas::Reader reader(ifs); 81 | liblas::Header inHeader = reader.GetHeader(); 82 | 83 | liblas::Point inPt(&inHeader); 84 | 85 | int m = inHeader.GetPointRecordsCount() / 1000000 + 1; 86 | int n = inHeader.GetPointRecordsCount() / m; 87 | n = (n / ALLTHREADS) * ALLTHREADS; 88 | printf("number of points: %d\n", n); 89 | 90 | unsigned int pointCount = 0; 91 | 92 | for (int j = 0; j < m; j++) 93 | { 94 | clock_t start = clock(); 95 | 96 | float *pts = new float[n * 3]; 97 | 98 | for (int k = 0; k < n; k++) 99 | { 100 | reader.ReadNextPoint(); 101 | inPt = reader.GetPoint(); 102 | 103 | pts[k * 3 + 0] = float(inPt.GetX() - inHeader.GetOffsetX()); 104 | pts[k * 3 + 1] = float(inPt.GetY() - inHeader.GetOffsetY()); 105 | pts[k * 3 + 2] = float(inPt.GetZ() - inHeader.GetOffsetZ()); 106 | } 107 | 108 | clock_t end = clock(); 109 | 110 | printf("\tcost time of reading points: %.3f\n", (end - start) / 1000.0); 111 | 112 | start = clock(); 113 | 114 | zt::ZtKDTree kdt; 115 | kdt.setSize(3, n); 116 | printf_s("\tnumber of points in kdtree: %d\n", n); 117 | kdt.setData(pts); 118 | kdt.buildTree(); 119 | 120 | end = clock(); 121 | 122 | printf("\tcost time of bulid kdtree: %.3f\n", (end - start) / 1000.0); 123 | 124 | start = clock(); 125 | 126 | FILE *fp; 127 | fopen_s(&fp, "D:\\20180419_R1000_ariborne_test_mta\\index_me.txt", "w"); 128 | if (!fp) 129 | { 130 | printf("Can.t create file!"); 131 | return 1; 132 | } 133 | 134 | float sPt[3] = { 135 | -inHeader.GetOffsetX() + 458127.4, 136 | -inHeader.GetOffsetY() + 4403518.666, 137 | -inHeader.GetOffsetZ() + 23.217 138 | }; 139 | 140 | std::vector knn; 141 | int nn = kdt.findNearestRange(sPt, 1.0, knn); 142 | 143 | printf("\tnumber of searched point %d\n", nn); 144 | for (int k = 0; k < nn; k++) 145 | { 146 | fprintf_s(fp, "%.3lf\t%.3lf\t%.3lf\n", 147 | pts[knn[k] * 3 + 0] + inHeader.GetOffsetX(), 148 | pts[knn[k] * 3 + 1] + inHeader.GetOffsetY(), 149 | pts[knn[k] * 3 + 2] + inHeader.GetOffsetZ()); 150 | } 151 | 152 | // printf("number of core = %d\n", omp_get_num_procs()); 153 | // #pragma omp parallel for num_threads(omp_get_num_procs()/* / 2*/) 154 | // for (int i = 0; i < n; i += 1) 155 | // { 156 | // float sPt[3] = { pts[i * 3 + 0], 157 | // pts[i * 3 + 1], 158 | // pts[i * 3 + 2] }; 159 | // /*int nst = kdt.findNearest(sPt);*/ 160 | // 161 | // int knn[50]; 162 | // float dist[50]; 163 | // kdt.findKNearestsNTP(sPt, 50, knn, dist); 164 | // 165 | // /*fprintf_s(fp, "%-6d\t%-6d\n", i, nst);*/ 166 | // // fprintf_s(fp, "%d\n", i); 167 | // // for (int k = 0; k < 10; k++) 168 | // // { 169 | // // double dx, dy, dz; 170 | // // dx = sPt[0] - pts[knn[k] * 3 + 0]; 171 | // // dy = sPt[1] - pts[knn[k] * 3 + 1]; 172 | // // dz = sPt[2] - pts[knn[k] * 3 + 2]; 173 | // // fprintf_s(fp, "%-6d\t%5.3f\t", knn[k], 174 | // // sqrt(dx * dx + dy * dy + dz * dz)); 175 | // // } 176 | // // fprintf_s(fp, "\n"); 177 | // } 178 | fclose(fp); 179 | 180 | printf("\tcost time of knn: %.3f\n", (clock() - start) / 1000.0); 181 | 182 | delete[] pts; 183 | } 184 | 185 | return 0; 186 | } 187 | 188 | int testGpuKnn() 189 | { 190 | /* std::string inFile("D:\\20180419_R1000_ariborne_test_mta\\180419_024813_0.las"); 191 | 192 | std::ifstream ifs(inFile, std::ios::in | std::ios::binary); 193 | if (!ifs.is_open()) 194 | { 195 | std::cout << "can't open " << inFile << endl; 196 | 197 | return 1; 198 | } 199 | 200 | liblas::Reader reader(ifs); 201 | liblas::Header inHeader = reader.GetHeader(); 202 | 203 | liblas::Point inPt(&inHeader); 204 | 205 | int m = inHeader.GetPointRecordsCount() / 1000000 + 1; 206 | int n = inHeader.GetPointRecordsCount() / m; 207 | 208 | int jump = n / ALLTHREADS; 209 | n = (n / ALLTHREADS) * ALLTHREADS; 210 | printf("number of points: %d\n", n); 211 | 212 | unsigned int pointCount = 0; 213 | 214 | for (int j = 0; j < 1; j++) 215 | { 216 | clock_t start = clock(); 217 | 218 | float *pts = new float[n * 3]; 219 | 220 | for (int k = 0; k < n; k++) 221 | { 222 | reader.ReadNextPoint(); 223 | inPt = reader.GetPoint(); 224 | 225 | pts[k * 3 + 0] = float(inPt.GetX() - inHeader.GetOffsetX()); 226 | pts[k * 3 + 1] = float(inPt.GetY() - inHeader.GetOffsetY()); 227 | pts[k * 3 + 2] = float(inPt.GetZ() - inHeader.GetOffsetZ()); 228 | } 229 | 230 | clock_t end = clock(); 231 | 232 | printf("\tcost time of reading points: %.3f\n", (end - start) / 1000.0); 233 | 234 | start = clock(); 235 | 236 | zt::ZtKDTree kdt; 237 | kdt.setSize(3, n); 238 | kdt.setData(pts); 239 | kdt.buildTree(); 240 | 241 | end = clock(); 242 | 243 | printf("\tcost time of bulid kdtree: %.3f\n", (end - start) / 1000.0); 244 | 245 | start = clock(); 246 | 247 | 248 | 249 | FILE *fp; 250 | fopen_s(&fp, "D:\\20180419_R1000_ariborne_test_mta\\index_me_gpu.txt", "w"); 251 | if (!fp) 252 | { 253 | printf("Can't create file!"); 254 | return 1; 255 | } 256 | 257 | clock_t t1 = clock(); 258 | 259 | int nn = 10; 260 | if (kdt.gpuInit(nn)) 261 | { 262 | printf("Failed to init gpu!\n"); 263 | return 1; 264 | } 265 | 266 | printf("cost time of init gpu: %.3f\n", (clock() - t1) / 1000.0); 267 | 268 | for (int i = 0; i < jump; i += 1) 269 | { 270 | float *sPts = new float[ALLTHREADS * 3]; 271 | int *res = new int[ALLTHREADS * nn]; 272 | 273 | for (int j = 0; j < ALLTHREADS; j++) 274 | { 275 | sPts[j * 3 + 0] = pts[(j * jump + i) * 3 + 0]; 276 | sPts[j * 3 + 1] = pts[(j * jump + i) * 3 + 1]; 277 | sPts[j * 3 + 2] = pts[(j * jump + i) * 3 + 2]; 278 | } 279 | 280 | if (kdt.gpuFindKNearests(sPts, nn, res)) 281 | { 282 | printf("Failed to use gpu!\n"); 283 | return 1; 284 | } 285 | 286 | for (int j = 0; j < ALLTHREADS; j++) 287 | { 288 | fprintf_s(fp, "%d\n", j * jump + i); 289 | for (int k = 0; k < nn; k++) 290 | { 291 | double dx, dy, dz; 292 | dx = sPts[j * 3 + 0] - pts[res[j * nn + k] * 3 + 0]; 293 | dy = sPts[j * 3 + 1] - pts[res[j * nn + k] * 3 + 1]; 294 | dz = sPts[j * 3 + 2] - pts[res[j * nn + k] * 3 + 2]; 295 | fprintf_s(fp, "%-6d\t%5.3f\t", res[j * nn + k], 296 | sqrt(dx * dx + dy * dy + dz * dz)); 297 | } 298 | fprintf_s(fp, "\n"); 299 | } 300 | 301 | delete[] res; 302 | delete[] sPts; 303 | } 304 | fclose(fp); 305 | 306 | printf("\tcost time of knn: %.3f\n", (clock() - start) / 1000.0); 307 | 308 | delete[] pts; 309 | } 310 | //*/ 311 | return 0; 312 | } 313 | 314 | int testFilter() 315 | { 316 | std::string inFile("D:\\20180828_R1000_sanding\\las\\180828_022843_5.las"); 317 | std::string outFile("D:\\20180828_R1000_sanding\\test\\180828_022843_5_ft.las"); 318 | 319 | std::ifstream ifs(inFile, std::ios::in | std::ios::binary); 320 | if (!ifs.is_open()) 321 | { 322 | std::cout << "can't open " << inFile << endl; 323 | 324 | return 1; 325 | } 326 | 327 | liblas::Reader reader(ifs); 328 | liblas::Header inHeader = reader.GetHeader(); 329 | 330 | liblas::Point inPt(&inHeader); 331 | 332 | int m = inHeader.GetPointRecordsCount() / 1000000 + 1; 333 | int n = inHeader.GetPointRecordsCount() / m; 334 | 335 | printf("number of points: %d\n", m * n); 336 | 337 | std::ofstream ofs(outFile, std::ios::out | std::ios::binary); 338 | if (!ofs.is_open()) 339 | { 340 | std::cout << "can't open " << outFile << endl; 341 | return 1; 342 | } 343 | 344 | liblas::Header outHeader; 345 | outHeader.SetVersionMajor(1); 346 | outHeader.SetVersionMinor(2); 347 | outHeader.SetDataFormatId(liblas::ePointFormat3); 348 | outHeader.SetScale(0.001, 0.001, 0.001); 349 | outHeader.SetOffset(inHeader.GetOffsetX(), inHeader.GetOffsetY(), inHeader.GetOffsetZ()); 350 | 351 | liblas::Writer writer(ofs, outHeader); 352 | liblas::Point outPt(&outHeader); 353 | 354 | unsigned int pointCount = 0; 355 | unsigned int filterCount = 0; 356 | 357 | clock_t allStart = clock(); 358 | for (int j = 0; j < m; j++) 359 | { 360 | clock_t start = clock(); 361 | 362 | float *pts = new float[n * 3]; 363 | bool *res = new bool[n]; 364 | 365 | // std::vector vpts; 366 | // vpts.reserve(n); 367 | 368 | std::vector srcPts; 369 | srcPts.reserve(n); 370 | 371 | LasFormat3Point lf3p; 372 | for (int k = 0; k < n; k++) 373 | { 374 | reader.ReadNextPoint(); 375 | 376 | inPt = reader.GetPoint(); 377 | lf3p.x = inPt[0]; 378 | lf3p.y = inPt[1]; 379 | lf3p.z = inPt[2]; 380 | lf3p.r = inPt.GetColor().GetRed(); 381 | lf3p.g = inPt.GetColor().GetGreen(); 382 | lf3p.b = inPt.GetColor().GetBlue(); 383 | lf3p.i = inPt.GetIntensity(); 384 | lf3p.t = inPt.GetTime(); 385 | lf3p.c = inPt.GetClassification().GetClass(); 386 | lf3p.id = inPt.GetPointSourceID(); 387 | lf3p.scanAng = inPt.GetScanAngleRank(); 388 | lf3p.scandir = inPt.GetScanDirection(); 389 | 390 | srcPts.push_back(lf3p); 391 | 392 | pts[k * 3 + 0] = float(inPt[0] - inHeader.GetOffsetX()); 393 | pts[k * 3 + 1] = float(inPt[1] - inHeader.GetOffsetY()); 394 | pts[k * 3 + 2] = float(inPt[2] - inHeader.GetOffsetZ()); 395 | } 396 | 397 | clock_t end = clock(); 398 | 399 | printf("\ncost time of reading points: %.3f\n", (end - start) / 1000.0); 400 | 401 | start = clock(); 402 | 403 | ZtStatisticFilterNoisePoint fnp; 404 | fnp.setParameter(25, 1.0); 405 | fnp.applyFilter_2(n, pts, res); 406 | 407 | end = clock(); 408 | 409 | printf("\tcost time of filtering point: %.3f\n", (end - start) / 1000.0); 410 | 411 | start = clock(); 412 | 413 | for (int i = 0; i < n; i++) 414 | { 415 | if (!res[i]) 416 | { 417 | filterCount++; 418 | outPt.SetClassification(7); 419 | } 420 | else 421 | { 422 | outPt.SetClassification(1); 423 | } 424 | 425 | pointCount++; 426 | 427 | /*outPt = vpts[i];*/ 428 | 429 | outPt.SetX(srcPts[i].x); 430 | outPt.SetY(srcPts[i].y); 431 | outPt.SetZ(srcPts[i].z); 432 | outPt.SetColor(liblas::Color(srcPts[i].r, srcPts[i].g, srcPts[i].b)); 433 | outPt.SetIntensity(srcPts[i].i); 434 | outPt.SetTime(srcPts[i].t); 435 | outPt.SetClassification(srcPts[i].c); 436 | outPt.SetPointSourceID(srcPts[i].id); 437 | outPt.SetScanAngleRank(srcPts[i].scanAng); 438 | outPt.SetScanDirection(srcPts[i].scandir); 439 | 440 | writer.WritePoint(outPt); 441 | } 442 | 443 | printf("\tcost time of writing point: %.3f\n", (clock() - start) / 1000.0); 444 | 445 | delete[] pts; 446 | } 447 | 448 | outHeader.SetMax(inHeader.GetMaxX(), inHeader.GetMaxY(), inHeader.GetMaxZ()); 449 | outHeader.SetMin(inHeader.GetMinX(), inHeader.GetMinY(), inHeader.GetMinZ()); 450 | outHeader.SetPointRecordsCount(pointCount); 451 | 452 | writer.SetHeader(outHeader); 453 | writer.WriteHeader(); 454 | 455 | printf("\nreserve points : %d, remove points : %d\n", pointCount, filterCount); 456 | printf("\ncost time of filtering all points: %.3f\n", (clock() - allStart) / 1000.0); 457 | 458 | return 0; 459 | } -------------------------------------------------------------------------------- /20180825_gpu_knn/ztKdTree.cpp: -------------------------------------------------------------------------------- 1 | #include "ztKdTree.h" 2 | #include 3 | #include 4 | #include 5 | /*#include "ztGpuKnn.h"*/ 6 | 7 | namespace zt 8 | { 9 | ZtKDTree::ZtKDTree() 10 | : nDimension(0), treeSize(0), offset(0), 11 | tree(0), treePtr(0), data(0), dataPtr(0), isGpuEnable(false) 12 | { 13 | 14 | } 15 | 16 | ZtKDTree::ZtKDTree(int dimension, unsigned int sz) 17 | : nDimension(dimension), treeSize(sz), offset(0), 18 | tree(0), treePtr(0), data(0), dataPtr(0) 19 | { 20 | nDimension = dimension; 21 | treeSize = sz; 22 | 23 | if (nDimension > 0 && treeSize > 0) 24 | { 25 | offset = new double[nDimension]; 26 | 27 | tree = new int *[4]; 28 | treePtr = new int[4 * treeSize]; 29 | for (int i = 0; i < 4; i++) 30 | { 31 | tree[i] = treePtr + i * treeSize; 32 | } 33 | 34 | data = new float *[nDimension]; 35 | dataPtr = new float[nDimension * sz]; 36 | for (int i = 0; i < nDimension; i++) 37 | { 38 | data[i] = dataPtr + i * treeSize; 39 | } 40 | } 41 | } 42 | 43 | ZtKDTree::~ZtKDTree() 44 | { 45 | if (offset) 46 | { 47 | delete[] offset; 48 | } 49 | if (tree) 50 | { 51 | delete[] tree; 52 | } 53 | if (treePtr) 54 | { 55 | delete[] treePtr; 56 | } 57 | if (data) 58 | { 59 | delete[] data; 60 | } 61 | if (dataPtr) 62 | { 63 | delete[] dataPtr; 64 | } 65 | } 66 | 67 | int ZtKDTree::setSize(int dimension, unsigned int sz) 68 | { 69 | nDimension = dimension; // 数据的维度 70 | treeSize = sz; // 数据的总数 71 | 72 | if (nDimension > 0 && treeSize > 0) 73 | { 74 | offset = new double[nDimension]; 75 | 76 | tree = new int *[4]; 77 | treePtr = new int[4 * treeSize]; 78 | for (int i = 0; i < 4; i++) 79 | { 80 | tree[i] = treePtr + i * treeSize; 81 | } 82 | 83 | data = new float *[nDimension]; 84 | dataPtr = new float[nDimension * sz]; 85 | for (int i = 0; i < nDimension; i++) 86 | { 87 | data[i] = dataPtr + i * treeSize; 88 | } 89 | } 90 | 91 | return 0; 92 | } 93 | 94 | int ZtKDTree::setOffset(double oft[]) 95 | { 96 | for (int i = 0; i < nDimension; i++) 97 | { 98 | offset[i] = oft[i]; 99 | } 100 | 101 | return 0; 102 | } 103 | 104 | /* 105 | * indata是一维数组表示多维数组 106 | * 数据排布方式：{[x,y,z……], [x,y,z……], ……} 107 | * dataPtr也是一维数组表示多维数组 108 | * 数据排布方式：{[x1, x2, x3……], [y1, y2, y3……], [z1, z2, z3……], ……} 109 | */ 110 | int ZtKDTree::setData(float *indata) 111 | { 112 | for (unsigned int i = 0; i < treeSize; i++) 113 | { 114 | for (int j = 0; j < nDimension; j++) 115 | { 116 | dataPtr[j * treeSize + i] = indata[i * nDimension + j]; 117 | } 118 | } 119 | 120 | return 0; 121 | } 122 | 123 | int ZtKDTree::setData(double *indata) 124 | { 125 | for (unsigned int i = 0; i < treeSize; i++) 126 | { 127 | for (int j = 0; j < nDimension; j++) 128 | { 129 | dataPtr[j * treeSize + i] = float(indata[i * nDimension + j] - offset[j]); 130 | } 131 | } 132 | 133 | return 0; 134 | } 135 | 136 | int ZtKDTree::buildTree() 137 | { 138 | std::vector vtr(treeSize); 139 | 140 | for (int i = 0; i < treeSize; i++) 141 | { 142 | vtr[i] = i; 143 | } 144 | 145 | std::random_shuffle(vtr.begin(), vtr.end()); 146 | 147 | treeRoot = buildTree(&vtr[0], treeSize, -1); // 根节点的父节点是-1 148 | 149 | return treeRoot; 150 | } 151 | 152 | int ZtKDTree::buildTree(int *indices, int count, int parent) 153 | { 154 | if (count == 1) 155 | { 156 | int rd = indices[0]; 157 | tree[0][rd] = 0; 158 | tree[1][rd] = parent; 159 | tree[2][rd] = -1; 160 | tree[3][rd] = -1; 161 | 162 | return rd; 163 | } 164 | else 165 | { 166 | float key = 0; 167 | int split = chooseSplitDimension(indices, count, key); 168 | int idx = chooseMiddleNode(indices, count, split, key); 169 | 170 | // rd 是实际点的下标， idx是点的索引数组的下标 171 | int rd = indices[idx]; 172 | 173 | tree[0][rd] = split; // 分割维度 174 | tree[1][rd] = parent; 175 | 176 | if (idx > 0) 177 | { 178 | tree[2][rd] = buildTree(indices, idx, rd); 179 | } 180 | else 181 | { 182 | tree[2][rd] = -1; 183 | } 184 | 185 | if (idx + 1 < count) 186 | { 187 | tree[3][rd] = buildTree(indices + idx + 1, count - idx - 1, rd); 188 | } 189 | else 190 | { 191 | tree[3][rd] = -1; 192 | } 193 | 194 | return rd; 195 | } 196 | } 197 | 198 | int ZtKDTree::findNearest(float *p) 199 | { 200 | int nearestNode; 201 | float minDist = 9999999, distance = 0; 202 | 203 | std::stack paths; 204 | 205 | int node = treeRoot; 206 | while (node > -1) 207 | { 208 | paths.emplace(node); 209 | node = p[tree[0][node]] <= data[tree[0][node]][node] ? tree[2][node] : tree[3][node]; 210 | } 211 | 212 | // 回溯路径 213 | while (!paths.empty()) 214 | { 215 | node = paths.top(); 216 | paths.pop(); 217 | 218 | distance = computeDistance(p, node); 219 | 220 | if (distance < minDist) 221 | { 222 | minDist = distance; 223 | nearestNode = node; 224 | } 225 | 226 | // 左右节点都为空，则为叶子节点 227 | if (tree[2][node] + tree[3][node] == -2) 228 | { 229 | continue; 230 | } 231 | else 232 | { 233 | //$$$$$$$$$$$$$$$$$$ 搜索策略 $$$$$$$$$$$$$$$$$$$$$// 234 | // // 235 | // 当前加入路径的节点都是与搜索点在同一空间中的节点 // 236 | // 回溯路径时，如果父节点与当前最大搜索半径相交， // 237 | // 则遍历兄弟空间并加入路径 // 238 | // // 239 | //$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$// 240 | 241 | int dim = tree[0][node]; 242 | 243 | if (fabs(p[dim] - data[dim][node]) < minDist) 244 | { 245 | if (p[dim] < data[dim][node] && tree[3][node] != -1) 246 | { 247 | /*paths.push(tree[3][node]);*/ 248 | int reNode = tree[3][node]; 249 | while (reNode > -1) 250 | { 251 | paths.emplace(reNode); 252 | 253 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 254 | } 255 | } 256 | else if (p[dim] > data[dim][node] && tree[2][node] != -1) 257 | { 258 | /*paths.push(tree[2][node]);*/ 259 | int reNode = tree[2][node]; 260 | while (reNode > -1) 261 | { 262 | paths.emplace(reNode); 263 | 264 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 265 | } 266 | } 267 | } 268 | } 269 | } 270 | 271 | return nearestNode; 272 | } 273 | 274 | int ZtKDTree::findNearest(double *p) 275 | { 276 | float *pf = new float[nDimension]; 277 | 278 | for (int i = 0; i < nDimension; i++) 279 | { 280 | pf[i] = p[i] - offset[i]; 281 | } 282 | 283 | return findNearest(pf); 284 | } 285 | 286 | struct NearestNode 287 | { 288 | int node; 289 | float distance; 290 | NearestNode() 291 | { 292 | node = 0; 293 | distance = 0.0f; 294 | } 295 | NearestNode(int n, float d) 296 | { 297 | node = n; 298 | distance = d; 299 | } 300 | }; 301 | 302 | struct cmp // 将最大的元素放在队首 303 | { 304 | bool operator()(NearestNode a, NearestNode b) 305 | { 306 | return a.distance < b.distance; 307 | } 308 | }; 309 | 310 | int ZtKDTree::findKNearestsSTL(float *p, int k, int *res) 311 | { 312 | std::priority_queue, cmp> kNeighbors; 313 | std::stack paths; 314 | 315 | // 记录查找路径 316 | int node = treeRoot; 317 | while (node > -1) 318 | { 319 | paths.emplace(node); 320 | 321 | node = p[tree[0][node]] <= data[tree[0][node]][node] ? tree[2][node] : tree[3][node]; 322 | } 323 | 324 | // 预先加入一个极大节点 325 | kNeighbors.emplace(-1, 9999999); 326 | 327 | // 回溯路径 328 | float distance = 0; 329 | while (!paths.empty()) 330 | { 331 | node = paths.top(); 332 | paths.pop(); 333 | 334 | distance = computeDistance(p, node); 335 | if (kNeighbors.size() < k) 336 | { 337 | kNeighbors.emplace(node, distance); 338 | } 339 | else 340 | { 341 | if (distance < kNeighbors.top().distance) 342 | { 343 | kNeighbors.pop(); 344 | kNeighbors.emplace(node, distance); 345 | } 346 | } 347 | 348 | if (tree[2][node] + tree[3][node] > -2) 349 | { 350 | int dim = tree[0][node]; 351 | if (p[dim] > data[dim][node]) 352 | { 353 | if (p[dim] - data[dim][node] < kNeighbors.top().distance && tree[2][node] > -1) 354 | { 355 | int reNode = tree[2][node]; 356 | while (reNode > -1) 357 | { 358 | paths.emplace(reNode); 359 | 360 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 361 | } 362 | } 363 | } 364 | else 365 | { 366 | if (data[dim][node] - p[dim] < kNeighbors.top().distance && tree[3][node] > -1) 367 | { 368 | int reNode = tree[3][node]; 369 | while (reNode > -1) 370 | { 371 | paths.emplace(reNode); 372 | 373 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 374 | } 375 | } 376 | } 377 | } 378 | } 379 | 380 | if (!res) 381 | { 382 | res = new int[k]; 383 | } 384 | 385 | int i = kNeighbors.size(); 386 | while (!kNeighbors.empty()) 387 | { 388 | res[--i] = kNeighbors.top().node; 389 | kNeighbors.pop(); 390 | } 391 | 392 | return 0; 393 | } 394 | 395 | int ZtKDTree::findKNearestsNTP(float *p, int k, int *res, float *dit) 396 | { 397 | // 数组形式的最大堆 398 | int *kNeighbors = new int[k]; 399 | float *kNDistance = new float[k]; 400 | int _currentNNode = 0; 401 | 402 | // 数组形式的路径堆栈 403 | int paths[256]; 404 | int _currentPath = 0; 405 | 406 | // 记录查找路径 407 | int node = treeRoot; 408 | while (node > -1) 409 | { 410 | paths[_currentPath++] = node; 411 | if (_currentPath > 255) 412 | { 413 | return 1; 414 | } 415 | 416 | node = p[tree[0][node]] <= data[tree[0][node]][node] ? tree[2][node] : tree[3][node]; 417 | } 418 | 419 | kNeighbors[_currentNNode] = -1; 420 | kNDistance[_currentNNode++] = 9999999; 421 | 422 | // 回溯路径 423 | float distance = 0; 424 | while (_currentPath > 0) 425 | { 426 | if (_currentPath > 255) 427 | { 428 | return 1; 429 | } 430 | 431 | node = paths[_currentPath-- - 1]; 432 | 433 | distance = computeDistance(p, node); 434 | if (_currentNNode < k) 435 | { 436 | kNeighbors[_currentNNode] = node; 437 | kNDistance[_currentNNode++] = distance; 438 | 439 | // 当达到k个节点后，建立最大堆 440 | if (_currentNNode == k) 441 | { 442 | for (int i = _currentNNode / 2 - 1; i >= 0; i--) 443 | { 444 | int parent = i; 445 | 446 | for (int son = i * 2 + 1; son <= _currentNNode; son = son * 2 + 1) 447 | { 448 | if (son + 1 < _currentNNode && kNDistance[son] < kNDistance[son + 1]) 449 | son++; 450 | 451 | if (kNDistance[parent] < kNDistance[son]) // 如果父节点小于子节点，则交换 452 | { 453 | float tempD = kNDistance[parent]; 454 | int tempI = kNeighbors[parent]; 455 | kNDistance[parent] = kNDistance[son]; 456 | kNeighbors[parent] = kNeighbors[son]; 457 | kNDistance[son] = tempD; 458 | kNeighbors[son] = tempI; 459 | } 460 | 461 | parent = son; 462 | } 463 | } 464 | } 465 | } 466 | else 467 | { 468 | if (distance < kNDistance[0]) 469 | { 470 | // pop 471 | kNeighbors[0] = kNeighbors[_currentNNode - 1]; 472 | kNDistance[0] = kNDistance[_currentNNode - 1]; 473 | 474 | // 删除堆顶后，要重构最大堆 475 | int parent = 0; 476 | int son = parent * 2 + 1; 477 | for (; son < _currentNNode - 1; son = son * 2 + 1) 478 | { 479 | if (son + 1 < _currentNNode - 1 && kNDistance[son] < kNDistance[son + 1]) 480 | son++; 481 | 482 | if (kNDistance[parent] < kNDistance[son]) // 如果父节点小于子节点，则交换 483 | { 484 | float tempD = kNDistance[parent]; 485 | int tempI = kNeighbors[parent]; 486 | kNDistance[parent] = kNDistance[son]; 487 | kNeighbors[parent] = kNeighbors[son]; 488 | kNDistance[son] = tempD; 489 | kNeighbors[son] = tempI; 490 | } 491 | 492 | parent = son; 493 | } 494 | 495 | // push 496 | son = _currentNNode - 1; 497 | parent = (son - 1) / 2; 498 | while (son != 0 && distance > kNDistance[parent]) 499 | { 500 | kNeighbors[son] = kNeighbors[parent]; 501 | kNDistance[son] = kNDistance[parent]; 502 | son = parent; 503 | parent = (son - 1) / 2; 504 | } 505 | 506 | kNDistance[son] = distance; 507 | kNeighbors[son] = node; 508 | } 509 | } 510 | 511 | if (tree[2][node] + tree[3][node] > -2) 512 | { 513 | int dim = tree[0][node]; 514 | if (p[dim] > data[dim][node]) 515 | { 516 | if (p[dim] - data[dim][node] < kNDistance[0] && tree[2][node] > -1) 517 | { 518 | int reNode = tree[2][node]; 519 | while (reNode > -1) 520 | { 521 | paths[_currentPath++] = reNode; 522 | if (_currentPath > 255) 523 | { 524 | return 1; 525 | } 526 | 527 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 528 | } 529 | } 530 | } 531 | else 532 | { 533 | if (data[dim][node] - p[dim] < kNDistance[0] && tree[3][node] > -1) 534 | { 535 | int reNode = tree[3][node]; 536 | while (reNode > -1) 537 | { 538 | paths[_currentPath++] = reNode; 539 | if (_currentPath > 255) 540 | { 541 | return 1; 542 | } 543 | 544 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 545 | } 546 | } 547 | } 548 | } 549 | } 550 | 551 | // 进行堆排序 552 | for (int i = _currentNNode - 1; i > 0; i--) 553 | { 554 | int tempI = kNeighbors[0]; 555 | float tempD = kNDistance[0]; 556 | kNeighbors[0] = kNeighbors[i]; 557 | kNDistance[0] = kNDistance[i]; 558 | kNeighbors[i] = tempI; 559 | kNDistance[i] = tempD; 560 | 561 | int parent = 0; 562 | int son = parent * 2 + 1; 563 | for (; son < i; son = parent * 2 + 1) 564 | { 565 | if (son + 1 < i && kNDistance[son] < kNDistance[son + 1]) 566 | son++; 567 | 568 | if (kNDistance[parent] < kNDistance[son]) 569 | { 570 | tempD = kNDistance[parent]; 571 | tempI = kNeighbors[parent]; 572 | kNDistance[parent] = kNDistance[son]; 573 | kNeighbors[parent] = kNeighbors[son]; 574 | kNDistance[son] = tempD; 575 | kNeighbors[son] = tempI; 576 | } 577 | 578 | parent = son; 579 | } 580 | } 581 | 582 | if (_currentNNode < k) 583 | { 584 | return 1; 585 | } 586 | 587 | if (!res) 588 | { 589 | res = new int[k]; 590 | } 591 | 592 | if (!dit) 593 | { 594 | dit = new float[k]; 595 | } 596 | 597 | int i = k; 598 | while (i != 0) 599 | { 600 | i--; 601 | res[i] = kNeighbors[i]; 602 | dit[i] = kNDistance[i]; 603 | } 604 | 605 | delete[] kNeighbors; 606 | delete[] kNDistance; 607 | 608 | return 0; 609 | } 610 | 611 | int ZtKDTree::findKNearests(double *p, int k, int *res) 612 | { 613 | float *pf = new float[nDimension]; 614 | 615 | for (int i = 0; i < nDimension; i++) 616 | { 617 | pf[i] = p[i] - offset[i]; 618 | } 619 | 620 | findKNearestsSTL(pf, k, res); 621 | 622 | return 0; 623 | } 624 | 625 | int ZtKDTree::findNearestRange(float *p, float range, std::vector &res) 626 | { 627 | std::queue kNeighbors; 628 | std::stack paths; 629 | 630 | // 记录查找路径 631 | int node = treeRoot; 632 | while (node > -1) 633 | { 634 | paths.emplace(node); 635 | 636 | node = p[tree[0][node]] <= data[tree[0][node]][node] ? tree[2][node] : tree[3][node]; 637 | } 638 | 639 | // 回溯路径 640 | float distance = 0; 641 | while (!paths.empty()) 642 | { 643 | node = paths.top(); 644 | paths.pop(); 645 | 646 | distance = computeDistance(p, node); 647 | if (distance < range) 648 | { 649 | kNeighbors.emplace(node, distance); 650 | } 651 | 652 | if (tree[2][node] + tree[3][node] > -2) 653 | { 654 | int dim = tree[0][node]; 655 | if (p[dim] > data[dim][node]) 656 | { 657 | if (p[dim] - data[dim][node] < range && tree[2][node] > -1) 658 | { 659 | int reNode = tree[2][node]; 660 | while (reNode > -1) 661 | { 662 | paths.emplace(reNode); 663 | 664 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 665 | } 666 | } 667 | } 668 | else 669 | { 670 | if (data[dim][node] - p[dim] < range && tree[3][node] > -1) 671 | { 672 | int reNode = tree[3][node]; 673 | while (reNode > -1) 674 | { 675 | paths.emplace(reNode); 676 | 677 | reNode = p[tree[0][reNode]] <= data[tree[0][reNode]][reNode] ? tree[2][reNode] : tree[3][reNode]; 678 | } 679 | } 680 | } 681 | } 682 | } 683 | 684 | res.reserve(kNeighbors.size()); 685 | 686 | int i = kNeighbors.size(); 687 | int n = i; 688 | while (!kNeighbors.empty()) 689 | { 690 | res.push_back(kNeighbors.front().node); 691 | kNeighbors.pop(); 692 | } 693 | 694 | return n; 695 | } 696 | 697 | int ZtKDTree::findNearestRange(double *p, float range, std::vector &res) 698 | { 699 | float *pf = new float[nDimension]; 700 | 701 | for (int i = 0; i < nDimension; i++) 702 | { 703 | pf[i] = p[i] - offset[i]; 704 | } 705 | 706 | return findNearestRange(pf, range, res); 707 | } 708 | /* 709 | int ZtKDTree::gpuInit(int nn) 710 | { 711 | if (getCudaDeviceCount() > 0) 712 | { 713 | if (setCudaStatus(0)) 714 | { 715 | return 1; 716 | } 717 | } 718 | 719 | char name[128]; 720 | getCudaDeviceNames(0, name); 721 | printf("cuda name = %s\n", name); 722 | 723 | if (initCudaForKdtree(treeSize, nDimension, nn, treePtr, dataPtr)) 724 | { 725 | return 1; 726 | } 727 | 728 | isGpuEnable = true; 729 | 730 | return 0; 731 | } 732 | 733 | int ZtKDTree::gpuFindKNearests(float *p, int k,int *res) 734 | { 735 | if (gpuSearchKnnKdtree(treeRoot, nDimension, treeSize, k, p, res)) 736 | { 737 | return 1; 738 | } 739 | 740 | return 0; 741 | } 742 | 743 | int ZtKDTree::gpuFindKNearests(double *p, int k, int *res) 744 | { 745 | float *pf = new float[nDimension]; 746 | 747 | for (int i = 0; i < nDimension; i++) 748 | { 749 | pf[i] = p[i] - offset[i]; 750 | } 751 | 752 | gpuFindKNearests(pf, k, res); 753 | 754 | return 0; 755 | } 756 | //*/ 757 | /* 758 | * 建树功能函数 759 | */ 760 | int ZtKDTree::chooseSplitDimension(int *ids, int sz, float &key) 761 | { 762 | int split = 0; 763 | 764 | float *var = new float[nDimension]; 765 | float *mean = new float[nDimension]; 766 | 767 | int cnt = sz/*std::min((int)SAMPLE_MEAN, sz)*/;/* cnt = sz;*/ 768 | double rt = 1.0 / cnt; 769 | 770 | for (int i = 0; i < nDimension; i++) 771 | { 772 | double sum1 = 0, sum2 = 0; 773 | for (int j = 0; j < cnt; j++) 774 | { 775 | sum1 += data[i][ids[j]] * data[i][ids[j]]; 776 | sum2 += data[i][ids[j]]; 777 | } 778 | var[i] = rt * sum1 - rt * sum2 * rt * sum2; 779 | mean[i] = rt * sum2; 780 | } 781 | 782 | double max = -9999999; 783 | 784 | for (int i = 0; i < nDimension; i++) 785 | { 786 | if (var[i] > max) 787 | { 788 | key = mean[i]; 789 | max = var[i]; 790 | split = i; 791 | } 792 | } 793 | 794 | delete[] var; 795 | delete[] mean; 796 | 797 | return split; 798 | } 799 | 800 | int ZtKDTree::chooseMiddleNode(int *ids, int sz, int dim, float key) 801 | { 802 | int left = 0; 803 | int right = sz - 1; 804 | 805 | while (1) 806 | { 807 | while (left <= right && data[dim][ids[left]] <= key) //左边找比key大的值 808 | ++left; 809 | 810 | while (left <= right && data[dim][ids[right]] >= key) //右边找比key小的值 811 | --right; 812 | 813 | if (left > right) 814 | break; 815 | 816 | std::swap(ids[left], ids[right]); 817 | ++left; 818 | --right; 819 | } 820 | 821 | // int lim1 = left; 822 | // 823 | // right = sz - 1; 824 | // while (1) 825 | // { 826 | // while (left <= right && data[dim][ids[left]] <= key) //左边找比key大的值 827 | // ++left; 828 | // 829 | // while (left <= right && data[dim][ids[right]] > key) //右边找比key小的值 830 | // --right; 831 | // 832 | // if (left > right) 833 | // break; 834 | // 835 | // std::swap(ids[left], ids[right]); 836 | // left++; 837 | // right--; 838 | // } 839 | // 840 | // int index = 0, lim2 = left; 841 | // 842 | // if (lim1 > sz / 2) 843 | // index = lim1; 844 | // else if (lim2 < sz / 2) 845 | // index = lim2; 846 | // else 847 | // index = sz / 2; 848 | // 849 | // if ((lim1 == sz) || (lim2 == 0)) 850 | // index = sz / 2; 851 | // 852 | // index = left; 853 | 854 | // 找出左子树的最大值作为根节点 855 | float max = -9999999; 856 | int maxIndex = 0; 857 | for (int i = 0; i < left; i++) 858 | { 859 | if (data[dim][ids[i]] > max) 860 | { 861 | max = data[dim][ids[i]]; 862 | maxIndex = i; 863 | } 864 | } 865 | 866 | if (left > 0) 867 | { 868 | if (maxIndex != left - 1) 869 | { 870 | std::swap(ids[maxIndex], ids[left - 1]); 871 | } 872 | } 873 | 874 | return left - 1 > 0 ? left - 1 : 0; 875 | } 876 | 877 | float ZtKDTree::computeDistance(float *p, int n) 878 | { 879 | float sum = 0; 880 | 881 | for (int i = 0; i < nDimension; i++) 882 | { 883 | sum += (p[i] - data[i][n]) * (p[i] - data[i][n]); 884 | } 885 | 886 | return sqrt(sum); 887 | } 888 | 889 | int ZtKDTree::outKdTree(const char *outPath) 890 | { 891 | FILE *fp; 892 | fopen_s(&fp, outPath, "w"); 893 | if (!fp) 894 | { 895 | printf("Failed to create: %s", outPath); 896 | return 1; 897 | } 898 | 899 | for (int i = 0; i < treeSize; i++) 900 | { 901 | fprintf_s(fp, "%d\t%7.3f\t%7.3f\t%7.3f\t%d\t%6d\t%6d\n", 902 | i, data[0][i], data[1][i], data[2][i], 903 | tree[0][i], tree[2][i], tree[3][i]); 904 | } 905 | 906 | fclose(fp); 907 | 908 | return 0; 909 | } 910 | } 911 | --------------------------------------------------------------------------------