├── .DS_Store ├── .vscode └── settings.json ├── Data ├── ControlPoint.DAT └── SamePoints.DAT ├── README.md └── codes ├── .DS_Store ├── .vscode ├── c_cpp_properties.json ├── launch.json ├── settings.json └── tasks.json ├── CMakeLists.txt ├── Common.h ├── DUtils ├── Random.cpp ├── Random.h ├── Timestamp.cpp └── Timestamp.h ├── build.sh ├── dphfjh.h ├── gsfpc.h ├── main.cpp └── qfjh.h /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HarborC/GrossErrorDetection/e7127de15b953379b624bafb7b7c727274fd335b/.DS_Store -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "vector": "cpp", 4 | "iosfwd": "cpp", 5 | "iterator": "cpp" 6 | } 7 | } -------------------------------------------------------------------------------- /Data/ControlPoint.DAT: -------------------------------------------------------------------------------- 1 | 200000004 445182.0000 3362694.0000 27.8000 3 2 | 200000003 445184.0000 3360834.0000 26.2000 3 3 | 200000001 444584.0000 3360823.0000 27.2000 3 4 | 200000002 444582.0000 3362683.0000 28.2000 3 5 | 200000006 445780.0000 3362690.0000 26.5000 3 6 | -99 0.0000 0.0000 0.0000 0 7 | -------------------------------------------------------------------------------- /Data/SamePoints.DAT: -------------------------------------------------------------------------------- 1 | 1000 10 12000 0 2 | 205021 0 3 | 45 -8.9500 -49.2331 4 | 76 -2.4186 -49.4256 5 | 81 0.1083 -50.0485 6 | 101 13.0325 -48.9356 7 | 130 24.1175 -49.7302 8 | 173 13.1472 -47.4680 9 | 187 19.3470 -47.8368 10 | 218 33.1226 -47.4660 11 | 220 -8.8816 -46.1174 12 | 235 -0.4066 -46.8407 13 | 259 13.4727 -47.1290 14 | 285 25.2330 -46.4014 15 | 294 26.8829 -46.4918 16 | 307 32.8730 -46.2047 17 | 330 -2.1858 -45.5704 18 | 378 -9.0486 -43.8312 19 | 400 -2.5556 -42.9733 20 | 404 -0.4684 -43.7657 21 | 452 13.3746 -42.9556 22 | 474 19.2719 -44.2037 23 | 502 27.1727 -43.1181 24 | 524 32.7047 -44.2279 25 | 597 12.7952 -42.3583 26 | 628 24.7699 -41.7192 27 | 646 32.8387 -41.5111 28 | 647 33.0720 -42.1067 29 | 654 -8.7488 -40.1620 30 | 681 0.0580 -39.9650 31 | 717 13.2146 -39.8912 32 | 732 19.0178 -41.1005 33 | 756 26.4777 -40.3620 34 | 791 -2.6029 -39.1820 35 | 821 12.8820 -38.2758 36 | 874 33.2662 -39.3961 37 | 879 -8.9357 -36.5899 38 | 973 19.7132 -38.0006 39 | 1014 33.2795 -37.3270 40 | 1030 -2.3041 -35.5093 41 | 1041 0.4311 -36.3915 42 | 1078 12.8610 -36.1293 43 | 1079 13.0918 -36.5488 44 | 1110 26.9345 -36.2995 45 | 1130 33.2095 -35.7139 46 | 1219 19.1690 -34.3208 47 | 1227 24.5908 -34.8428 48 | 1252 32.9435 -34.5001 49 | 1259 -9.0937 -33.0781 50 | 1276 -0.0839 -32.8779 51 | 1306 13.0640 -32.1081 52 | 1316 14.0891 -32.7417 53 | 1339 24.3937 -32.2572 54 | 1340 26.2302 -32.8644 55 | 1378 -2.4830 -31.7940 56 | 1465 19.6660 -30.7641 57 | 1505 33.3440 -30.9194 58 | 1511 -8.8989 -29.2784 59 | 1529 -0.3452 -30.0615 60 | 1595 13.5636 -29.7418 61 | 1649 33.1987 -29.6989 62 | 1661 -2.0969 -28.5741 63 | 1702 12.9114 -27.9040 64 | 1735 19.6663 -28.0614 65 | 1755 24.6157 -28.1058 66 | 1763 -9.1043 -26.4869 67 | 1774 -0.2907 -26.2087 68 | 1832 33.2008 -26.6554 69 | 1837 33.0642 -26.5228 70 | 1858 -2.4005 -24.7649 71 | 1913 13.6870 -25.1122 72 | 1933 24.7450 -25.6625 73 | 1938 27.0964 -24.4688 74 | 1953 32.7722 -24.7039 75 | 2020 12.7866 -24.0217 76 | 2021 13.4962 -23.3161 77 | 2043 19.2681 -23.9521 78 | 2085 -9.1350 -22.5226 79 | 2106 -2.1575 -22.1432 80 | 2116 -0.1229 -22.5984 81 | 2191 25.5390 -21.5870 82 | 2198 26.7252 -22.2934 83 | 2217 32.6157 -22.3796 84 | 2267 13.4182 -20.5626 85 | 2290 20.3363 -21.0410 86 | 2314 32.2621 -20.7416 87 | 2316 -9.3149 -18.2855 88 | 2338 -2.6584 -18.1816 89 | 2351 -0.1411 -19.1921 90 | 2391 13.2014 -18.6508 91 | 2392 13.1274 -18.9246 92 | 2453 33.1533 -18.8760 93 | 2546 20.0216 -16.9428 94 | 2563 25.5914 -17.9902 95 | 2607 -9.0820 -15.9365 96 | 2632 -0.3792 -16.0454 97 | 2666 13.5100 -15.9128 98 | 2715 27.1874 -15.3927 99 | 2729 32.9207 -16.0887 100 | 2744 -2.7234 -14.6918 101 | 2767 13.1870 -14.5993 102 | 2789 24.0025 -14.4101 103 | 2811 32.9073 -14.2230 104 | 2844 -0.3463 -12.0450 105 | 2906 20.2677 -13.3213 106 | 2932 26.5910 -12.0615 107 | 2962 -9.3383 -10.7983 108 | 2975 -3.0082 -10.4855 109 | 3030 12.9301 -11.6937 110 | 3077 24.4063 -10.9167 111 | 3104 32.8857 -11.1627 112 | 3107 -9.1581 -9.0600 113 | 3183 19.9363 -10.2381 114 | 3208 27.6824 -9.2693 115 | 3231 32.8293 -10.3645 116 | 3242 -3.1243 -7.5803 117 | 3250 0.2280 -8.2511 118 | 3281 13.8007 -8.7601 119 | 3299 24.2872 -8.5884 120 | 3343 -0.3732 -5.8701 121 | 3389 13.6161 -6.9694 122 | 3409 18.4969 -6.5330 123 | 3451 32.6224 -6.3598 124 | 3457 -9.0447 -5.0577 125 | 3518 12.9794 -4.8438 126 | 3519 13.4007 -5.5311 127 | 3567 26.7843 -5.5140 128 | 3615 -2.4234 -4.0779 129 | 3683 20.0447 -3.3331 130 | 3695 24.3172 -3.8602 131 | 3715 33.1849 -3.9980 132 | 3717 -8.9829 -2.3301 133 | 3744 -0.2785 -2.1587 134 | 3798 27.7335 -1.8375 135 | 3808 32.7685 -2.5233 136 | 3878 13.2395 -1.3185 137 | 3914 25.4106 -0.0789 138 | 3934 33.3874 -0.1790 139 | 3940 33.2588 -0.0222 140 | 3967 -3.0393 0.3247 141 | 3973 -0.7172 1.8216 142 | 4017 13.1416 1.0429 143 | 4036 19.3856 0.6127 144 | 4064 27.0947 1.3048 145 | 4086 -9.2874 2.4702 146 | 4107 -2.5930 2.4225 147 | 4135 13.3571 2.5108 148 | 4277 20.0499 4.3676 149 | 4306 32.7504 4.5069 150 | 4312 -9.2380 6.0302 151 | 4336 0.3422 5.0880 152 | 4352 10.4688 6.3426 153 | 4361 14.5990 4.9519 154 | 4395 26.8563 4.9706 155 | 4451 -2.9028 6.4492 156 | 4543 20.1408 7.7325 157 | 4553 24.2806 6.7947 158 | 4602 33.1181 7.5718 159 | 4606 -9.3397 9.1413 160 | 4630 -2.8042 9.3666 161 | 4641 -0.2560 9.0450 162 | 4685 13.2764 8.6339 163 | 4730 26.6494 8.1170 164 | 4838 19.6945 10.0063 165 | 4848 24.0680 9.7901 166 | 4873 33.2437 9.9237 167 | 4875 32.6158 10.5165 168 | 4914 -0.0342 12.2810 169 | 4976 12.9939 11.0085 170 | 4986 13.3040 12.4835 171 | 5044 27.8640 12.1044 172 | 5066 -9.1283 12.6412 173 | 5093 -2.6506 13.0582 174 | 5161 13.1125 13.7273 175 | 5211 23.6105 13.7913 176 | 5238 32.8545 13.1699 177 | 5264 -0.0212 15.2319 178 | 5346 19.8985 14.4470 179 | 5381 -9.2825 15.8813 180 | 5390 -2.2920 15.8655 181 | 5434 13.5628 16.0975 182 | 5460 24.1464 17.1811 183 | 5474 26.6582 16.1428 184 | 5488 -9.6346 18.4138 185 | 5555 13.2846 17.7825 186 | 5581 20.0167 17.8548 187 | 5613 32.5515 17.5112 188 | 5643 0.0648 19.0393 189 | 5679 13.4085 19.1646 190 | 5732 24.5906 20.4013 191 | 5736 26.6291 18.7750 192 | 5757 32.5798 19.9087 193 | 5777 -3.1871 20.5280 194 | 5889 -9.6265 23.0006 195 | 5918 0.5471 22.3077 196 | 5947 13.2434 21.7515 197 | 5975 27.5562 22.8741 198 | 5981 32.6193 22.0787 199 | 6005 -2.7181 23.8259 200 | 6074 23.6087 24.0012 201 | 6133 0.0751 26.2647 202 | 6162 13.4157 26.1603 203 | 6186 20.1215 25.7347 204 | 6202 26.7817 25.7502 205 | 6218 32.9854 24.9922 206 | 6221 -9.8917 27.3359 207 | 6228 -3.6386 26.7079 208 | 6298 24.1264 27.1670 209 | 6326 32.9004 27.6150 210 | 6336 -0.5250 29.3327 211 | 6367 19.6620 28.2454 212 | 6395 26.6757 29.1362 213 | 6412 -9.6826 30.1294 214 | 6419 -3.6775 30.6454 215 | 6455 10.4761 30.2692 216 | 6463 13.5657 29.5681 217 | 6527 -9.7697 32.1895 218 | 6597 19.8164 31.4872 219 | 6612 24.1136 31.4238 220 | 6644 33.0508 31.8120 221 | 6658 -2.9736 33.5758 222 | 6663 -0.5884 33.2880 223 | 6695 13.6053 32.8441 224 | 6737 26.6528 33.3160 225 | 6767 32.4322 33.9246 226 | 6806 19.3624 35.0566 227 | 6820 24.2289 34.2545 228 | 6836 -9.0399 36.8265 229 | 6864 0.4018 36.3165 230 | 6889 9.5564 37.1446 231 | 6894 13.2504 35.9555 232 | 6945 -9.7691 38.2689 233 | 6959 -3.2025 37.4780 234 | 7019 13.6829 38.6581 235 | 7042 19.7659 38.0988 236 | 7053 24.4880 37.8276 237 | 7065 27.6516 37.4631 238 | 7089 33.3116 38.3144 239 | 7107 -0.3196 39.9926 240 | 7190 26.7247 39.7729 241 | 7231 -3.1584 40.9819 242 | 7243 10.0353 40.5251 243 | 7260 24.1115 41.5886 244 | 7279 32.2012 41.4376 245 | 7280 -8.9174 42.7040 246 | 7362 19.5019 43.4001 247 | 7384 27.0722 43.2344 248 | 7416 -3.4825 44.2070 249 | 7446 0.3745 43.3757 250 | 7501 13.7293 43.4389 251 | 7556 32.7556 44.7833 252 | 7578 -3.6373 46.1527 253 | 7623 13.8603 45.8170 254 | 7636 19.9560 45.0264 255 | 7646 24.3267 45.1318 256 | 7651 27.1503 46.1703 257 | 7658 32.5410 45.6193 258 | 7791 -8.5794 51.3631 259 | 200000004 23.1705 38.0374 260 | 200000003 25.6305 -34.8180 261 | 200000006 -0.3455 36.7411 262 | -99 0.0000 0.0000 263 | 205022 0 264 | 45 -33.9071 -49.9019 265 | 76 -27.3943 -50.1453 266 | 81 -24.8850 -51.0277 267 | 101 -12.0078 -49.7705 268 | 130 -1.0083 -50.6426 269 | 173 -11.8738 -48.3097 270 | 187 -5.7154 -48.7240 271 | 218 7.9495 -48.4586 272 | 220 -33.8070 -46.8447 273 | 235 -25.3544 -47.6281 274 | 259 -11.5439 -47.9754 275 | 285 0.1428 -47.3422 276 | 294 1.7793 -47.4445 277 | 307 7.7154 -47.2057 278 | 330 -27.1146 -46.1619 279 | 378 -33.9440 -44.5144 280 | 400 -27.4607 -43.7105 281 | 404 -25.3890 -44.5181 282 | 452 -11.5971 -43.9912 283 | 474 -5.7486 -45.1113 284 | 502 2.1051 -44.0950 285 | 524 7.5727 -45.2398 286 | 597 -12.1655 -43.2231 287 | 628 -0.2620 -42.6859 288 | 646 7.7373 -42.5437 289 | 647 7.9613 -43.1374 290 | 654 -33.6055 -40.8557 291 | 681 -24.8192 -40.7348 292 | 717 -11.7162 -40.7729 293 | 732 -5.9717 -42.0229 294 | 756 1.4496 -41.3521 295 | 791 -27.4567 -39.9328 296 | 821 -12.0325 -38.9353 297 | 874 8.1831 -40.3357 298 | 879 -33.7490 -37.2932 299 | 973 -5.2340 -38.9480 300 | 1014 8.2223 -38.3934 301 | 1030 -27.1130 -36.2762 302 | 1041 -24.3998 -37.1778 303 | 1078 -12.0153 -37.2992 304 | 1079 -11.7927 -37.4447 305 | 1110 1.9499 -37.3186 306 | 1130 8.1718 -36.5430 307 | 1219 -5.7364 -35.2834 308 | 1227 -0.3584 -35.8506 309 | 1252 7.9199 -35.8072 310 | 1259 -33.8651 -33.7898 311 | 1276 -24.8725 -33.7596 312 | 1306 -11.7746 -33.0267 313 | 1316 -10.7607 -33.6660 314 | 1339 -0.5282 -33.2786 315 | 1340 1.2892 -33.7608 316 | 1378 -27.2506 -32.5707 317 | 1465 -5.2046 -31.7517 318 | 1505 8.3583 -31.8634 319 | 1511 -33.6232 -30.0028 320 | 1529 -25.0987 -30.8646 321 | 1595 -11.2537 -30.6763 322 | 1649 8.2299 -30.8195 323 | 1661 -26.8285 -29.3648 324 | 1702 -11.8811 -28.8414 325 | 1735 -5.1749 -29.0645 326 | 1755 -0.2609 -29.1555 327 | 1763 -33.8050 -27.2181 328 | 1774 -24.9994 -27.0265 329 | 1832 8.2666 -27.6180 330 | 1837 8.1325 -27.6642 331 | 1858 -27.0900 -25.5659 332 | 1913 -11.0807 -26.0723 333 | 1933 -0.1067 -26.7284 334 | 1938 2.2436 -25.5657 335 | 1953 7.8626 -25.8563 336 | 2020 -11.9608 -24.9788 337 | 2021 -11.2453 -24.2838 338 | 2043 -5.5211 -24.9737 339 | 2085 -33.7848 -23.2630 340 | 2106 -26.8170 -22.9569 341 | 2116 -24.7949 -23.4312 342 | 2191 0.7265 -22.6868 343 | 2198 1.8968 -23.4015 344 | 2217 7.7301 -23.5468 345 | 2267 -11.2916 -21.5434 346 | 2290 -4.4302 -22.0914 347 | 2314 7.4012 -21.9160 348 | 2316 -33.9134 -18.9568 349 | 2338 -27.2769 -19.0035 350 | 2351 -24.7748 -20.0374 351 | 2391 -11.4839 -19.6401 352 | 2392 -11.5621 -19.9104 353 | 2453 8.3055 -20.0730 354 | 2546 -4.6917 -18.0130 355 | 2563 0.8190 -19.1130 356 | 2607 -33.6548 -16.6957 357 | 2632 -24.9740 -16.8993 358 | 2666 -11.1418 -16.8393 359 | 2715 2.4331 -16.5508 360 | 2729 8.1081 -17.3026 361 | 2744 -27.2991 -15.5255 362 | 2767 -11.4482 -15.6080 363 | 2789 -0.7127 -15.5398 364 | 2811 8.1152 -15.4508 365 | 2844 -24.8913 -12.7476 366 | 2906 -4.4026 -14.4148 367 | 2932 1.8804 -13.2335 368 | 2962 -33.8527 -11.5700 369 | 2975 -27.5320 -11.3296 370 | 3030 -11.6699 -12.7146 371 | 3077 -0.2740 -12.1830 372 | 3104 8.1287 -12.4124 373 | 3107 -33.6540 -10.0122 374 | 3183 -4.6961 -11.3464 375 | 3208 2.9959 -10.4723 376 | 3231 8.0813 -11.6193 377 | 3242 -27.6113 -8.4339 378 | 3250 -24.2725 -9.1407 379 | 3281 -10.7698 -9.8051 380 | 3299 -0.3659 -9.7570 381 | 3343 -24.8453 -6.7626 382 | 3389 -10.9325 -8.0202 383 | 3409 -6.0842 -7.6451 384 | 3451 7.9571 -7.6368 385 | 3457 -33.4869 -5.8478 386 | 3518 -11.5425 -5.8988 387 | 3519 -11.1282 -6.5883 388 | 3567 2.1396 -6.5255 389 | 3615 -26.8684 -4.9515 390 | 3683 -4.5096 -4.4828 391 | 3695 -0.2789 -5.0585 392 | 3715 8.4950 -5.3023 393 | 3717 -33.3944 -3.1303 394 | 3744 -24.7105 -3.0653 395 | 3798 3.1250 -3.0905 396 | 3808 8.1038 -3.7848 397 | 3878 -11.2418 -2.1402 398 | 3914 0.8504 -1.3476 399 | 3934 8.7524 -1.5133 400 | 3940 8.6281 -1.3548 401 | 3967 -27.4378 -0.5572 402 | 3973 -25.1059 0.9062 403 | 4017 -11.3166 0.1138 404 | 4036 -5.1167 -0.5799 405 | 4064 2.5301 0.0386 406 | 4086 -33.6431 1.6606 407 | 4107 -26.9674 1.5275 408 | 4135 -11.0833 1.4121 409 | 4277 -4.4086 3.1719 410 | 4306 8.1697 3.1481 411 | 4312 -33.5509 5.2103 412 | 4336 -24.0108 4.1459 413 | 4352 -13.9131 5.2623 414 | 4361 -9.8232 3.8242 415 | 4395 2.3620 3.6854 416 | 4451 -27.2295 5.5450 417 | 4543 -4.2849 6.5172 418 | 4553 -0.1978 5.2797 419 | 4602 8.5688 6.1877 420 | 4606 -33.6138 8.0473 421 | 4630 -27.0947 8.4506 422 | 4641 -24.5587 8.1130 423 | 4685 -11.0959 7.5056 424 | 4730 2.1691 6.8131 425 | 4838 -4.7013 8.7840 426 | 4848 -0.3653 8.5098 427 | 4873 8.7196 8.5207 428 | 4875 8.1049 9.1153 429 | 4914 -24.2925 11.3164 430 | 4976 -11.3459 9.9878 431 | 4986 -11.0240 11.0704 432 | 5044 3.4226 10.7567 433 | 5066 -33.3654 11.8002 434 | 5093 -26.8917 12.1274 435 | 5161 -11.2010 12.5738 436 | 5211 -0.7423 12.2211 437 | 5238 8.3741 11.7493 438 | 5264 -24.2436 14.2564 439 | 5346 -4.4442 13.1960 440 | 5381 -33.4812 15.0349 441 | 5390 -26.4992 14.9204 442 | 5434 -10.7271 14.9279 443 | 5460 -0.2031 15.8535 444 | 5474 2.2755 14.7860 445 | 5488 -33.8041 17.5652 446 | 5555 -10.9813 16.6056 447 | 5581 -4.2873 16.5825 448 | 5613 8.1227 16.0618 449 | 5643 -24.1182 18.0476 450 | 5679 -10.8430 17.9790 451 | 5732 0.2763 18.8967 452 | 5736 2.2735 17.4015 453 | 5757 8.1767 18.4417 454 | 5777 -27.3371 19.4632 455 | 5889 -33.7423 22.1382 456 | 5918 -23.6025 21.2059 457 | 5947 -10.9739 20.7918 458 | 5975 3.2370 21.4594 459 | 5981 8.2430 20.5957 460 | 6005 -26.8308 22.8578 461 | 6074 -0.6565 22.6382 462 | 6133 -24.0227 25.1540 463 | 6162 -10.7543 24.9385 464 | 6186 -4.0971 24.4135 465 | 6202 2.5094 24.3292 466 | 6218 8.6401 23.4821 467 | 6221 -33.9409 26.4666 468 | 6228 -27.7121 25.7425 469 | 6298 -0.1051 25.7770 470 | 6326 8.5840 26.0886 471 | 6336 -24.5849 28.3114 472 | 6367 -4.5233 26.9168 473 | 6395 2.4458 27.6946 474 | 6412 -33.6977 29.4736 475 | 6419 -27.7111 29.6685 476 | 6455 -13.6267 29.0719 477 | 6463 -10.5625 28.4300 478 | 6527 -33.7595 31.2251 479 | 6597 -4.3313 30.1375 480 | 6612 -0.0628 30.2038 481 | 6644 8.7804 30.2519 482 | 6658 -26.9831 32.4201 483 | 6663 -24.6012 32.2506 484 | 6695 -10.4864 31.5843 485 | 6737 2.4723 31.8992 486 | 6767 8.2209 32.3605 487 | 6806 -4.7361 33.6937 488 | 6820 0.0800 32.8174 489 | 6836 -32.9946 35.9137 490 | 6864 -23.5841 35.2545 491 | 6889 -14.4608 35.9284 492 | 6894 -10.7991 34.5807 493 | 6945 -33.6800 37.3675 494 | 6959 -27.1652 36.4697 495 | 7019 -10.3415 37.3667 496 | 7042 -4.2765 36.9291 497 | 7053 0.3776 36.2902 498 | 7065 3.5075 35.9504 499 | 7089 9.1227 36.5804 500 | 7107 -24.2324 38.9285 501 | 7190 2.6174 38.2592 502 | 7231 -27.0523 39.9614 503 | 7243 -13.9421 39.4451 504 | 7260 0.0469 40.1071 505 | 7279 8.0547 39.8233 506 | 7280 -32.7883 41.7747 507 | 7362 -4.4931 41.7652 508 | 7384 3.0147 41.6912 509 | 7416 -27.3472 43.1797 510 | 7446 -23.5126 42.2191 511 | 7501 -10.2247 42.1217 512 | 7556 8.6607 43.1336 513 | 7578 -27.4792 45.1228 514 | 7623 -10.0653 44.4852 515 | 7636 -4.0212 43.5950 516 | 7646 0.3146 43.4736 517 | 7651 3.1279 44.6076 518 | 7658 8.4600 43.9690 519 | 7791 -32.3637 50.4034 520 | 7871 13.3751 -49.9459 521 | 7907 21.9614 -49.0515 522 | 7916 27.1501 -50.0011 523 | 7926 33.2997 -50.1134 524 | 8069 21.5956 -46.6481 525 | 8088 27.0491 -46.9617 526 | 8103 33.3153 -46.1533 527 | 8245 26.7846 -43.2915 528 | 8342 21.9163 -42.6280 529 | 8393 33.1300 -42.2582 530 | 8492 26.9075 -39.7718 531 | 8561 20.9517 -38.5684 532 | 8585 33.0469 -38.5151 533 | 8755 21.6916 -36.0483 534 | 8778 26.9517 -35.9044 535 | 8792 33.0300 -35.8838 536 | 8882 32.7774 -33.6650 537 | 8933 22.5710 -32.1778 538 | 8937 27.4822 -32.5215 539 | 9115 21.9626 -28.9711 540 | 9127 26.4267 -29.3151 541 | 9148 33.1078 -29.0272 542 | 9300 22.4327 -26.0221 543 | 9305 26.4647 -25.8754 544 | 9432 33.3930 -25.2935 545 | 9559 33.2648 -23.0308 546 | 9638 21.5885 -22.1653 547 | 9649 26.5594 -22.0834 548 | 9793 22.2160 -18.2835 549 | 9816 26.9569 -18.4475 550 | 9961 33.2379 -17.7423 551 | 10035 22.2178 -15.1801 552 | 10049 26.8117 -15.3925 553 | 10067 33.1284 -15.4901 554 | 10193 27.2066 -12.2506 555 | 10279 22.4988 -11.5358 556 | 10409 26.5611 -9.2896 557 | 10438 33.2950 -10.3395 558 | 10439 33.3453 -9.3809 559 | 10489 22.2217 -7.6960 560 | 10614 32.8062 -5.9744 561 | 10691 22.2752 -4.8956 562 | 10705 27.0587 -4.9296 563 | 10822 22.6409 -1.3380 564 | 10833 26.6323 -1.6264 565 | 10936 33.3967 -0.8525 566 | 11027 22.8750 1.6266 567 | 11136 26.9567 2.2266 568 | 11153 33.0321 2.2024 569 | 11214 33.1456 4.6946 570 | 11274 22.8935 5.5620 571 | 11280 27.1163 5.5740 572 | 11394 26.9899 8.1894 573 | 11494 23.1564 8.7903 574 | 11711 27.0365 12.3728 575 | 11734 33.6670 12.2794 576 | 11823 22.7754 12.7823 577 | 11971 33.2866 14.5782 578 | 12017 23.3390 16.7692 579 | 12033 27.8206 15.7805 580 | 12200 22.7442 20.1838 581 | 12216 27.0951 19.1719 582 | 12241 33.3899 19.9782 583 | 12419 22.9072 22.9001 584 | 12426 27.0850 22.9923 585 | 12601 33.2013 23.4224 586 | 12703 22.6794 26.3879 587 | 12754 33.3421 24.9814 588 | 12866 27.1543 26.4620 589 | 12974 26.6851 29.3062 590 | 12990 33.3031 29.3630 591 | 13088 23.1277 30.2983 592 | 13247 33.3710 32.5144 593 | 13314 22.4468 33.4008 594 | 13330 26.9359 32.9315 595 | 13468 22.9195 37.0016 596 | 13481 26.9973 36.5440 597 | 13496 33.1233 36.9763 598 | 13680 27.2422 39.6826 599 | 13700 33.1242 39.6916 600 | 13733 22.6260 40.4944 601 | 13815 27.0244 42.8876 602 | 13907 23.2238 43.9468 603 | 13943 33.2181 44.2044 604 | 14004 31.9208 45.5625 605 | 14045 26.5342 46.5403 606 | 14166 23.4071 50.8763 607 | 14183 26.7276 50.4077 608 | 14188 -2.9393 51.4061 609 | 200000004 -0.9251 36.5936 610 | 200000003 0.6750 -35.8350 611 | 200000005 23.9269 -35.4661 612 | 200000001 23.9264 -35.4986 613 | 200000002 22.3804 36.7441 614 | -99 0.0000 0.0000 615 | 205023 0 616 | 101 -33.5524 -50.8292 617 | 130 -22.5672 -51.5532 618 | 173 -33.4437 -49.0994 619 | 187 -27.2941 -49.6980 620 | 218 -13.6566 -49.2483 621 | 259 -33.1173 -49.0296 622 | 285 -21.4752 -48.2361 623 | 294 -19.8375 -48.3163 624 | 307 -13.9132 -47.9955 625 | 452 -33.2472 -44.8798 626 | 474 -27.3925 -46.0859 627 | 502 -19.5706 -45.0758 628 | 524 -14.0919 -46.0303 629 | 597 -33.8246 -44.2898 630 | 628 -21.9604 -43.5847 631 | 646 -13.9756 -43.1930 632 | 647 -13.7407 -43.9218 633 | 717 -33.4156 -41.8326 634 | 732 -27.6765 -43.0034 635 | 756 -20.2766 -42.2257 636 | 821 -33.7626 -40.2307 637 | 874 -13.5689 -41.2270 638 | 973 -26.9841 -39.9173 639 | 1014 -13.5675 -39.1712 640 | 1078 -33.7691 -38.0956 641 | 1079 -33.5435 -38.5114 642 | 1110 -19.8474 -38.1860 643 | 1130 -13.6452 -37.5685 644 | 1219 -27.5560 -36.2634 645 | 1227 -22.1768 -36.7517 646 | 1252 -13.9145 -36.5875 647 | 1306 -33.6090 -34.0977 648 | 1316 -32.5857 -34.7219 649 | 1339 -22.3976 -34.1837 650 | 1340 -20.5694 -34.7761 651 | 1465 -27.0890 -32.5811 652 | 1505 -13.5469 -32.8050 653 | 1595 -33.1350 -31.7444 654 | 1649 -13.6971 -31.5935 655 | 1702 -33.7934 -29.9202 656 | 1735 -27.1100 -30.0411 657 | 1755 -22.2048 -30.0586 658 | 1832 -13.7142 -28.5711 659 | 1837 -13.8507 -28.3672 660 | 1913 -33.0439 -27.1418 661 | 1933 -22.0976 -27.6307 662 | 1938 -19.7662 -26.4316 663 | 1953 -14.1568 -26.6364 664 | 2020 -33.9404 -25.9149 665 | 2021 -33.2392 -25.3600 666 | 2043 -27.5269 -25.9601 667 | 2191 -21.3393 -23.5786 668 | 2198 -20.1574 -24.2732 669 | 2217 -14.3342 -24.3285 670 | 2267 -33.3337 -22.6245 671 | 2290 -26.4952 -23.0642 672 | 2314 -14.6891 -22.7038 673 | 2391 -33.5586 -20.7271 674 | 2392 -33.6321 -20.9989 675 | 2453 -13.8191 -20.8472 676 | 2546 -26.8246 -18.9951 677 | 2563 -21.3128 -20.0067 678 | 2666 -33.2644 -17.9266 679 | 2715 -19.7474 -17.4195 680 | 2729 -14.0662 -18.0804 681 | 2767 -33.5932 -16.7025 682 | 2789 -22.9026 -16.4605 683 | 2811 -14.0917 -16.2307 684 | 2906 -26.5955 -15.2105 685 | 2932 -20.3592 -14.1156 686 | 3030 -33.8622 -13.8186 687 | 3077 -22.5250 -13.1016 688 | 3104 -14.1351 -13.1907 689 | 3183 -26.9437 -12.3384 690 | 3208 -19.2943 -11.3383 691 | 3231 -14.1978 -12.3999 692 | 3281 -33.0176 -10.9011 693 | 3299 -22.6617 -10.6799 694 | 3389 -33.2097 -9.0339 695 | 3409 -28.3934 -8.6674 696 | 3451 -14.3567 -8.6820 697 | 3518 -33.8539 -7.0166 698 | 3519 -33.4296 -7.6974 699 | 3567 -20.2229 -7.6159 700 | 3683 -26.8795 -5.4848 701 | 3695 -22.6568 -5.9871 702 | 3715 -13.9097 -6.0816 703 | 3798 -19.3036 -3.9643 704 | 3808 -14.3263 -4.5731 705 | 3878 -33.6249 -3.2616 706 | 3914 -21.5956 -2.2294 707 | 3934 -13.7109 -2.2915 708 | 3940 -13.8378 -2.1347 709 | 4017 -33.7377 -1.1737 710 | 4036 -27.5530 -1.5994 711 | 4064 -19.9534 -0.8511 712 | 4135 -33.5291 0.2843 713 | 4277 -26.9099 2.1577 714 | 4306 -14.3816 2.3548 715 | 4361 -32.3192 2.7099 716 | 4395 -20.1588 2.7894 717 | 4543 -26.8526 5.3193 718 | 4553 -22.7639 4.3342 719 | 4602 -14.0374 5.3960 720 | 4685 -33.6515 6.3601 721 | 4730 -20.4309 5.9070 722 | 4838 -27.3058 7.7508 723 | 4848 -22.9815 7.5550 724 | 4873 -13.9297 7.7284 725 | 4875 -14.5540 8.3122 726 | 4976 -33.9433 8.8312 727 | 4986 -33.6480 10.1778 728 | 5044 -19.2490 9.8661 729 | 5161 -33.8481 11.3292 730 | 5211 -23.3906 11.2517 731 | 5238 -14.3307 10.9446 732 | 5346 -27.1257 12.1552 733 | 5434 -33.4192 13.7652 734 | 5460 -22.9512 14.8839 735 | 5474 -20.4629 13.8651 736 | 5555 -33.6982 15.4340 737 | 5581 -27.0263 15.5351 738 | 5613 -14.6602 15.2475 739 | 5679 -33.5860 16.8047 740 | 5732 -22.5254 17.8044 741 | 5736 -20.5166 16.4740 742 | 5757 -14.6514 17.7147 743 | 5947 -33.7649 19.7308 744 | 5975 -19.6284 20.5412 745 | 5981 -14.6239 19.7738 746 | 6074 -23.5253 21.6431 747 | 6162 -33.6187 23.7405 748 | 6186 -26.9841 23.3467 749 | 6202 -20.4027 23.3894 750 | 6218 -14.2806 22.6626 751 | 6298 -23.0301 24.7830 752 | 6326 -14.3814 25.2619 753 | 6367 -27.4530 25.8334 754 | 6395 -20.5229 26.7455 755 | 6463 -33.4871 27.3375 756 | 6597 -27.3175 29.0469 757 | 6612 -23.0588 29.0012 758 | 6644 -14.2626 29.4199 759 | 6695 -33.4694 30.3672 760 | 6737 -20.5706 31.1095 761 | 6767 -14.8311 31.5133 762 | 6806 -27.7782 32.5832 763 | 6820 -22.9702 31.8055 764 | 6894 -33.8278 33.3461 765 | 7019 -33.4255 36.1290 766 | 7042 -27.3505 35.6003 767 | 7053 -22.7381 35.3471 768 | 7065 -19.6134 34.7378 769 | 7089 -14.0254 35.7398 770 | 7190 -20.5417 37.2813 771 | 7260 -23.1335 38.8726 772 | 7279 -15.1558 38.9527 773 | 7362 -27.6695 40.6309 774 | 7384 -20.1920 40.7111 775 | 7501 -33.3756 40.8256 776 | 7556 -14.5900 42.2662 777 | 7623 -33.2598 43.2223 778 | 7636 -27.2321 42.4624 779 | 7646 -22.9139 42.4303 780 | 7651 -20.1321 43.6192 781 | 7658 -14.8077 43.0930 782 | 7871 -8.2056 -50.6618 783 | 7907 0.3681 -49.6509 784 | 7916 5.5840 -50.5314 785 | 7926 11.7532 -50.5606 786 | 8069 -0.0411 -47.2471 787 | 8088 5.4282 -47.4855 788 | 8103 11.6984 -46.5890 789 | 8245 5.0886 -43.8467 790 | 8342 0.2056 -43.2143 791 | 8393 11.4312 -42.6852 792 | 8492 5.1514 -40.4865 793 | 8561 -0.8328 -39.1612 794 | 8585 11.2790 -38.9345 795 | 8755 -0.1411 -36.6272 796 | 8778 5.1209 -36.4058 797 | 8792 11.2112 -36.2970 798 | 8882 10.9115 -34.0765 799 | 8933 0.6661 -32.7384 800 | 8937 5.5890 -32.7796 801 | 9115 -0.0019 -29.5363 802 | 9127 4.4772 -30.0275 803 | 9148 11.1536 -29.4235 804 | 9300 0.4167 -26.3915 805 | 9305 4.4453 -26.3684 806 | 9432 11.3718 -25.6787 807 | 9559 11.1976 -23.4137 808 | 9638 -0.5122 -22.7329 809 | 9649 4.4603 -22.5709 810 | 9793 0.0479 -18.8353 811 | 9816 4.7974 -18.9240 812 | 9961 11.0726 -18.1174 813 | 10035 -0.0117 -15.7317 814 | 10049 4.5938 -15.8673 815 | 10067 10.9233 -15.8627 816 | 10193 4.9314 -12.7176 817 | 10279 0.2055 -12.0790 818 | 10409 4.2312 -9.7637 819 | 10438 10.9946 -10.7031 820 | 10439 11.0270 -9.7416 821 | 10489 -0.1428 -8.2423 822 | 10614 10.4223 -6.3419 823 | 10691 -0.1411 -5.5941 824 | 10705 4.6470 -5.3922 825 | 10822 0.1588 -1.8747 826 | 10833 4.1574 -2.0698 827 | 10936 10.9220 -1.4815 828 | 11027 0.3367 1.0948 829 | 11136 4.4131 1.7648 830 | 11153 10.4946 1.8488 831 | 11214 10.5580 4.3445 832 | 11274 0.2836 5.0278 833 | 11280 4.5119 5.1157 834 | 11394 4.3353 7.4779 835 | 11494 0.4870 8.2601 836 | 11711 4.3059 11.9119 837 | 11734 10.9273 11.9381 838 | 11823 0.0337 12.2426 839 | 11971 10.5056 14.2339 840 | 12017 0.5270 15.9848 841 | 12033 5.0156 15.3343 842 | 12200 -0.1306 19.6268 843 | 12216 4.2303 18.7102 844 | 12241 10.5126 19.6362 845 | 12419 -0.0173 22.3560 846 | 12426 4.1573 22.3464 847 | 12601 10.2598 23.0763 848 | 12703 -0.3054 25.8362 849 | 12754 10.3621 24.6365 850 | 12866 4.1618 25.9965 851 | 12974 3.6426 28.8309 852 | 12990 10.2151 29.0159 853 | 13088 0.0678 29.7504 854 | 13247 10.2342 32.1675 855 | 13314 -0.6701 32.8354 856 | 13330 3.8275 32.4939 857 | 13468 -0.2581 36.4397 858 | 13481 3.8154 36.0664 859 | 13496 9.9092 36.7899 860 | 13680 3.9969 39.2057 861 | 13700 9.8457 39.3364 862 | 13733 -0.6071 39.9216 863 | 13815 3.7506 42.4016 864 | 13907 -0.0690 43.3805 865 | 13943 9.9203 43.8455 866 | 14004 8.5972 45.1759 867 | 14045 3.1775 46.0361 868 | 14166 -0.0268 50.3020 869 | 14183 3.3009 49.9043 870 | 14188 -26.3088 50.2653 871 | 200000004 -24.0398 35.6627 872 | 200000003 -21.1455 -36.7200 873 | 200000005 2.0812 -36.0446 874 | 200000001 2.0830 -36.0448 875 | 200000002 -0.7804 36.3268 876 | -99 0.0000 0.0000 877 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GrossErrorDetection 2 | 3 | 这是袁修孝老师的《测量数据处理与理论分析》课上的最终作业. 4 | 5 | 代码中包含有： 6 | 1.单片后方交会 7 | 2.相对定向 8 | 3.光束法平差 9 | 4.选权迭代法（丹麦法） 10 | -------------------------------------------------------------------------------- /codes/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HarborC/GrossErrorDetection/e7127de15b953379b624bafb7b7c727274fd335b/codes/.DS_Store -------------------------------------------------------------------------------- /codes/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Mac", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "${workspaceFolder}", 8 | "/Library/Developer/CommandLineTools/usr/include/c++/v1", 9 | "/usr/local/include", 10 | "/Library/Developer/CommandLineTools/usr/lib/clang/9.0.0/include", 11 | "/Library/Developer/CommandLineTools/usr/include", 12 | "/usr/include", 13 | "/usr/local/Cellar/eigen/3.3.7/include/eigen3" 14 | ], 15 | "defines": [], 16 | "macFrameworkPath": [ 17 | "/Library/Developer/CommandLineTools/SDKs/MacOSX.sdk/System/Library/Frameworks" 18 | ], 19 | "compilerPath": "/usr/bin/clang", 20 | "cStandard": "c11", 21 | "cppStandard": "c++17", 22 | "intelliSenseMode": "clang-x64", 23 | "configurationProvider": "vector-of-bool.cmake-tools" 24 | } 25 | ], 26 | "version": 4 27 | } -------------------------------------------------------------------------------- /codes/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // 使用 IntelliSense 了解相关属性。 3 | // 悬停以查看现有属性的描述。 4 | // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | 8 | { 9 | "name": "M", 10 | "type": "cppdbg", 11 | "request": "launch", 12 | "program": "${workspaceFolder}/build/main", 13 | "args": ["30"], 14 | "stopAtEntry": false, 15 | "cwd": "${workspaceFolder}", 16 | "environment": [], 17 | "externalConsole": true, 18 | "MIMode": "lldb", 19 | "preLaunchTask": "build" 20 | } 21 | ] 22 | } 23 | -------------------------------------------------------------------------------- /codes/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "utility": "cpp", 4 | "ostream": "cpp", 5 | "__bit_reference": "cpp", 6 | "__config": "cpp", 7 | "__debug": "cpp", 8 | "__errc": "cpp", 9 | "__functional_base": "cpp", 10 | "__hash_table": "cpp", 11 | "__locale": "cpp", 12 | "__mutex_base": "cpp", 13 | "__node_handle": "cpp", 14 | "__nullptr": "cpp", 15 | "__split_buffer": "cpp", 16 | "__string": "cpp", 17 | "__threading_support": "cpp", 18 | "__tree": "cpp", 19 | "__tuple": "cpp", 20 | "algorithm": "cpp", 21 | "array": "cpp", 22 | "atomic": "cpp", 23 | "bit": "cpp", 24 | "bitset": "cpp", 25 | "cctype": "cpp", 26 | "chrono": "cpp", 27 | "clocale": "cpp", 28 | "cmath": "cpp", 29 | "codecvt": "cpp", 30 | "complex": "cpp", 31 | "condition_variable": "cpp", 32 | "cstdarg": "cpp", 33 | "cstddef": "cpp", 34 | "cstdint": "cpp", 35 | "cstdio": "cpp", 36 | "cstdlib": "cpp", 37 | "cstring": "cpp", 38 | "ctime": "cpp", 39 | "cwchar": "cpp", 40 | "cwctype": "cpp", 41 | "deque": "cpp", 42 | "exception": "cpp", 43 | "fstream": "cpp", 44 | "functional": "cpp", 45 | "initializer_list": "cpp", 46 | "iomanip": "cpp", 47 | "ios": "cpp", 48 | "iosfwd": "cpp", 49 | "iostream": "cpp", 50 | "istream": "cpp", 51 | "iterator": "cpp", 52 | "limits": "cpp", 53 | "list": "cpp", 54 | "locale": "cpp", 55 | "map": "cpp", 56 | "memory": "cpp", 57 | "mutex": "cpp", 58 | "new": "cpp", 59 | "numeric": "cpp", 60 | "optional": "cpp", 61 | "queue": "cpp", 62 | "random": "cpp", 63 | "ratio": "cpp", 64 | "set": "cpp", 65 | "sstream": "cpp", 66 | "stack": "cpp", 67 | "stdexcept": "cpp", 68 | "streambuf": "cpp", 69 | "string": "cpp", 70 | "string_view": "cpp", 71 | "system_error": "cpp", 72 | "thread": "cpp", 73 | "tuple": "cpp", 74 | "type_traits": "cpp", 75 | "typeinfo": "cpp", 76 | "unordered_map": "cpp", 77 | "vector": "cpp", 78 | "hash_map": "cpp", 79 | "*.tcc": "cpp", 80 | "memory_resource": "cpp", 81 | "cinttypes": "cpp" 82 | } 83 | } -------------------------------------------------------------------------------- /codes/.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | // See https://go.microsoft.com/fwlink/?LinkId=733558 3 | // for the documentation about the tasks.json format 4 | "version": "2.0.0", 5 | "tasks": [ 6 | { 7 | "label": "build", 8 | "type": "shell", 9 | "command": "sh build.sh" 10 | }, 11 | 12 | { 13 | "label": "c++", 14 | "command": "g++", 15 | "type": "shell", 16 | "args": [ 17 | "main1.cc", 18 | "-g" 19 | ] 20 | } 21 | ] 22 | } -------------------------------------------------------------------------------- /codes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(OuterPoint) 4 | 5 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 6 | 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") 10 | SET(CMAKE_BUILD_TYPE Debug) 11 | MESSAGE(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE}) 12 | 13 | set(HDRS_DUTILS 14 | DUtils/Random.h 15 | DUtils/Timestamp.h) 16 | set(SRCS_DUTILS 17 | DUtils/Random.cpp 18 | DUtils/Timestamp.cpp) 19 | 20 | find_package(OpenCV REQUIRED) 21 | find_package(Eigen3 REQUIRED) 22 | include_directories( 23 | ${PROJECT_SOURCE_DIR} 24 | ${EIGEN3_INCLUDE_DIR} 25 | ) 26 | 27 | add_library(DUTILS SHARED ${SRCS_DUTILS}) 28 | 29 | add_executable(main ./main.cpp) 30 | target_link_libraries(main ${Eigen_LIBS} ${OpenCV_LIBS} DUTILS) 31 | 32 | -------------------------------------------------------------------------------- /codes/Common.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "DUtils/Random.h" 13 | 14 | Eigen::Matrix3d getR(float phi, float omiga, float kapa){ 15 | Eigen::Matrix3d R; 16 | R< 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /codes/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /codes/DUtils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | -------------------------------------------------------------------------------- /codes/DUtils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | using namespace std; 15 | 16 | namespace DUtils { 17 | 18 | /// Timestamp 19 | class Timestamp 20 | { 21 | public: 22 | 23 | /// Options to initiate a timestamp 24 | enum tOptions 25 | { 26 | NONE = 0, 27 | CURRENT_TIME = 0x1, 28 | ZERO = 0x2 29 | }; 30 | 31 | public: 32 | 33 | /** 34 | * Creates a timestamp 35 | * @param option option to set the initial time stamp 36 | */ 37 | Timestamp(Timestamp::tOptions option = NONE); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | virtual ~Timestamp(void); 43 | 44 | /** 45 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 46 | * when initiated with the ZERO flag 47 | * @return true iif secs == usecs == 0 48 | */ 49 | bool empty() const; 50 | 51 | /** 52 | * Sets this instance to the current time 53 | */ 54 | void setToCurrentTime(); 55 | 56 | /** 57 | * Sets the timestamp from seconds and microseconds 58 | * @param secs: seconds 59 | * @param usecs: microseconds 60 | */ 61 | inline void setTime(unsigned long secs, unsigned long usecs){ 62 | m_secs = secs; 63 | m_usecs = usecs; 64 | } 65 | 66 | /** 67 | * Returns the timestamp in seconds and microseconds 68 | * @param secs seconds 69 | * @param usecs microseconds 70 | */ 71 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 72 | { 73 | secs = m_secs; 74 | usecs = m_usecs; 75 | } 76 | 77 | /** 78 | * Sets the timestamp from a string with the time in seconds 79 | * @param stime: string such as "1235603336.036609" 80 | */ 81 | void setTime(const string &stime); 82 | 83 | /** 84 | * Sets the timestamp from a number of seconds from the epoch 85 | * @param s seconds from the epoch 86 | */ 87 | void setTime(double s); 88 | 89 | /** 90 | * Returns this timestamp as the number of seconds in (long) float format 91 | */ 92 | double getFloatTime() const; 93 | 94 | /** 95 | * Returns this timestamp as the number of seconds in fixed length string format 96 | */ 97 | string getStringTime() const; 98 | 99 | /** 100 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 101 | * If the order is swapped, a negative number is returned 102 | * @param t: timestamp to subtract from this timestamp 103 | * @return difference in seconds 104 | */ 105 | double operator- (const Timestamp &t) const; 106 | 107 | /** 108 | * Returns a copy of this timestamp + s seconds + us microseconds 109 | * @param s seconds 110 | * @param us microseconds 111 | */ 112 | Timestamp plus(unsigned long s, unsigned long us) const; 113 | 114 | /** 115 | * Returns a copy of this timestamp - s seconds - us microseconds 116 | * @param s seconds 117 | * @param us microseconds 118 | */ 119 | Timestamp minus(unsigned long s, unsigned long us) const; 120 | 121 | /** 122 | * Adds s seconds to this timestamp and returns a reference to itself 123 | * @param s seconds 124 | * @return reference to this timestamp 125 | */ 126 | Timestamp& operator+= (double s); 127 | 128 | /** 129 | * Substracts s seconds to this timestamp and returns a reference to itself 130 | * @param s seconds 131 | * @return reference to this timestamp 132 | */ 133 | Timestamp& operator-= (double s); 134 | 135 | /** 136 | * Returns a copy of this timestamp + s seconds 137 | * @param s: seconds 138 | */ 139 | Timestamp operator+ (double s) const; 140 | 141 | /** 142 | * Returns a copy of this timestamp - s seconds 143 | * @param s: seconds 144 | */ 145 | Timestamp operator- (double s) const; 146 | 147 | /** 148 | * Returns whether this timestamp is at the future of t 149 | * @param t 150 | */ 151 | bool operator> (const Timestamp &t) const; 152 | 153 | /** 154 | * Returns whether this timestamp is at the future of (or is the same as) t 155 | * @param t 156 | */ 157 | bool operator>= (const Timestamp &t) const; 158 | 159 | /** 160 | * Returns whether this timestamp and t represent the same instant 161 | * @param t 162 | */ 163 | bool operator== (const Timestamp &t) const; 164 | 165 | /** 166 | * Returns whether this timestamp is at the past of t 167 | * @param t 168 | */ 169 | bool operator< (const Timestamp &t) const; 170 | 171 | /** 172 | * Returns whether this timestamp is at the past of (or is the same as) t 173 | * @param t 174 | */ 175 | bool operator<= (const Timestamp &t) const; 176 | 177 | /** 178 | * Returns the timestamp in a human-readable string 179 | * @param machine_friendly if true, the returned string is formatted 180 | * to yyyymmdd_hhmmss, without weekday or spaces 181 | * @note This has not been tested under Windows 182 | * @note The timestamp is truncated to seconds 183 | */ 184 | string Format(bool machine_friendly = false) const; 185 | 186 | /** 187 | * Returns a string version of the elapsed time in seconds, with the format 188 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 189 | * @param s: elapsed seconds (given by getFloatTime) to format 190 | */ 191 | static string Format(double s); 192 | 193 | 194 | protected: 195 | /// Seconds 196 | unsigned long m_secs; // seconds 197 | /// Microseconds 198 | unsigned long m_usecs; // microseconds 199 | }; 200 | 201 | } 202 | 203 | #endif 204 | 205 | -------------------------------------------------------------------------------- /codes/build.sh: -------------------------------------------------------------------------------- 1 | cd ./Thirdparty/g2o/ 2 | mkdir build 3 | cd ./build 4 | cmake .. 5 | make -j2 6 | cd ../../../ 7 | 8 | mkdir build 9 | cd ./build 10 | cmake .. 11 | make -------------------------------------------------------------------------------- /codes/dphfjh.h: -------------------------------------------------------------------------------- 1 | // 单片后方交会 2 | #include "Common.h" 3 | 4 | using namespace std; 5 | using namespace Eigen; 6 | 7 | void dphfjh(vector> images, unordered_map control_pt, int N) 8 | { 9 | // 单片后方交会 10 | // 注意控制点是左手坐标系，所以X，Y要调换 11 | vector cpt; 12 | Eigen::Vector3d mean_pt = Eigen::Vector3d(0,0,0); 13 | for(unordered_map::iterator it = control_pt.begin(); it != control_pt.end(); it++){ 14 | if(images[2].find(it->first) != images[2].end()){ 15 | cpt.push_back(it->first); 16 | mean_pt += it->second; 17 | } 18 | } 19 | mean_pt /= cpt.size(); 20 | 21 | float phi3 = 0; 22 | float omiga3 = 0; 23 | float kapa3 = 0; 24 | float Xs3 = mean_pt(1); 25 | float Ys3 = mean_pt(0); 26 | float Zs3 = 2660; 27 | float f = 100.5; 28 | 29 | for(int it = 0; it R; 31 | R = getR(phi3, omiga3, kapa3); 32 | 33 | Eigen::Matrix A; 34 | Eigen::Matrix L; 35 | Eigen::Matrix delta; 36 | A.resize(cpt.size()*2, 6); 37 | L.resize(cpt.size()*2, 1); 38 | 39 | for(int i = 0; i> images, vector idx12, vector idx13, unordered_map> same_pt, int N){ 5 | 6 | // 光束法区域网平差 7 | float f = 100.5; 8 | float phi[3] = {0.0, 0.0, 0.0}; 9 | float omiga[3] = {0.0, 0.0, 0.0}; 10 | float kapa[3] = {0.0, 0.0, 0.0}; 11 | float Xs[3] = {0.0, 0.0, 0.0}; 12 | float Ys[3] = {0.0, 0.0, 0.0}; 13 | float Zs[3] = {0.0, 0.0, 0.0}; 14 | 15 | for(int j = 0; j Ps(same_pt.size()); 26 | int nn = 0; 27 | for(unordered_map>::iterator it = same_pt.begin(); it != same_pt.end(); it++, nn++){ 28 | int Pid = it->first; 29 | vector sets = it->second; 30 | 31 | int cam1 = sets[0]; int cam2 = sets[1]; 32 | 33 | Eigen::Vector2d x1 = images[cam1][Pid]; Eigen::Vector2d x2 = images[cam2][Pid]; 34 | float Bx = Xs[cam2] - Xs[cam1]; 35 | float Bz = Zs[cam2] - Zs[cam1]; 36 | float N1 = (Bx*(-f) - Bz*x2(0))/(x1(0)*(-f) - x2(0)*(-f)); 37 | float N2 = (Bx*(-f) - Bz*x1(0))/(x1(0)*(-f) - x2(0)*(-f)); 38 | 39 | Ps[nn]=Eigen::Vector3d(Xs[cam1]+N1*x1(0),Ys[cam1]+N1*x1(1),Zs[cam1]+N1*(-f)); 40 | } 41 | Eigen::Matrix A; 42 | Eigen::Matrix L; 43 | A.setZero(nl*2, 11 + same_pt.size()*3); 44 | L.setZero(nl*2, 1); 45 | Eigen::Matrix P; 46 | P.setZero(nl*2, nl*2); 47 | for(int i = 0; i delta; 52 | delta.resize(11 + same_pt.size()*3,1); 53 | Eigen::Matrix H; 54 | H.resize(11 + same_pt.size()*3,11 + same_pt.size()*3); 55 | Eigen::Matrix v; 56 | v.resize(nl*2,1); 57 | 58 | for(int itime = 0; itime R(3); 61 | for(int i = 0; i<3; i++){ 62 | R[i] = getR(phi[i], omiga[i], kapa[i]); 63 | } 64 | 65 | A.setZero(nl*2, 11 + same_pt.size()*3); 66 | L.setZero(nl*2, 1); 67 | 68 | int npixel = 0; 69 | int nP = 0; 70 | for(unordered_map>::iterator it = same_pt.begin(); it != same_pt.end(); it++, nP++){ 71 | int Pid = it->first; 72 | vector sets = it->second; 73 | for(int ic = 0; ic3){ 164 | // P(i,i) = pi*exp(-Vi/3.0); 165 | //} 166 | if(Vi>=4.13){ 167 | P(i,i) = 1.0/(Vi*Vi); 168 | }else{ 169 | P(i,i) = 1.0; 170 | } 171 | } 172 | 173 | //cerr<>::iterator it = same_pt.begin(); it != same_pt.end(); it++, nP++){ 180 | int Pid = it->first; 181 | vector sets = it->second; 182 | for(int ic = 0; ic4*0.0028){ 185 | //if(P(npixel*2+1)<1.0){ 186 | cerr<<"camid = "<> images(3); 17 | unordered_map control_pt; 18 | int nl = 0; 19 | 20 | // 读取同名像点 21 | ifstream in("../Data/SamePoints.DAT"); 22 | if(!in){ 23 | cout<<"文件打开失败！"<>n1; 39 | if(n1 <0){ 40 | break; 41 | } 42 | ss>>n2; ss>>n3; 43 | Eigen::Vector2d pt(n2, n3); 44 | images[i][n1] = pt; 45 | nl++; 46 | } 47 | } 48 | 49 | in.close(); 50 | 51 | /*for(int i = 0;i<3;i++){ 52 | cout<<"Image No."<::iterator it = images[i].begin(); it != images[i].end(); it++){ 54 | cout<<"id: "<<(*it).first<<" coord: x = "<second(0)<<" y = "<second(1)<>n1; 72 | if(n1 < 0) 73 | break; 74 | ss>>n2; ss>>n3; ss>>n4; 75 | Eigen::Vector3d c_pt(n2, n3, n4); 76 | control_pt[n1] = c_pt; 77 | } 78 | 79 | in2.close(); 80 | 81 | /*for(unordered_map::iterator it = control_pt.begin(); it != control_pt.end(); it++){ 82 | cout<<"id: "<first<<" Control Point: X = "<second(0)<<" Y = "<second(1)<<" Z = "<second(2)< idx12, idx23, idx13; 88 | unordered_map> same_pt; 89 | 90 | for(unordered_map::iterator it = images[0].begin(); it != images[0].end(); it++){ 91 | int index = it->first; 92 | if(images[1].find(index) != images[1].end()){ 93 | idx12.push_back(index); 94 | } 95 | if(images[2].find(index) != images[2].end()){ 96 | idx13.push_back(index); 97 | } 98 | if(same_pt.find(index) == same_pt.end()){ 99 | vector v; 100 | v.push_back(0); 101 | same_pt[index] = v; 102 | } 103 | } 104 | for(unordered_map::iterator it = images[1].begin(); it != images[1].end(); it++){ 105 | int index = it->first; 106 | if(images[2].find(index) != images[2].end()){ 107 | idx23.push_back(index); 108 | } 109 | if(same_pt.find(index) == same_pt.end()){ 110 | vector v; 111 | v.push_back(1); 112 | same_pt[index] = v; 113 | }else{ 114 | same_pt[index].push_back(1); 115 | } 116 | } 117 | for(unordered_map::iterator it = images[2].begin(); it != images[2].end(); it++){ 118 | int index = it->first; 119 | if(same_pt.find(index) == same_pt.end()){ 120 | vector v; 121 | v.push_back(2); 122 | same_pt[index] = v; 123 | }else{ 124 | same_pt[index].push_back(2); 125 | } 126 | } 127 | 128 | int n1, n2; 129 | n1 = n2 = 0; 130 | for(unordered_map>::iterator it = same_pt.begin(); it != same_pt.end(); it++){ 131 | if(it->second.size() == 2){ 132 | n1++; 133 | }else if (it->second.size() == 3){ 134 | n2++; 135 | } 136 | } 137 | 138 | /*for(unordered_map>::iterator it = same_pt.begin(); it != same_pt.end(); it++){ 139 | cout<<"id = "<first<<" images = "; 140 | for(int j = 0;jsecond.size(); j++){ 141 | cout<second[j]<<" "; 142 | } 143 | cout< idx, int c1_, int c2_, vector> images, int N){ 5 | 6 | // Generate sets of 8 points for each RANSAC iteration 7 | int mMaxIterations = 1; 8 | vector idx_ = idx; 9 | int cam1 = c1_; 10 | int cam2 = c2_; 11 | int nmatches = idx_.size(); 12 | 13 | /*vector vAllIndices; 14 | vector vAvailableIndices; 15 | for(int i=0; i > mvSets = vector< vector >(mMaxIterations,vector(5,0)); 20 | 21 | DUtils::Random::SeedRandOnce(0); 22 | 23 | for(int it=0; it set = mvSets[it]; 42 | 43 | float phi = 0; 44 | float omiga = 0; 45 | float kapa = 0; 46 | float f = 100.5; 47 | float u = 0; 48 | float v = 0; 49 | float Bx = 0, By, Bz; 50 | 51 | //for(int j = 0; j<5; j++){ 52 | //Bx += images[cam1][idx_[set[j]]](0)-images[cam2][idx_[set[j]]](0); 53 | for(int j = 0; j L; 63 | Eigen::Matrix A; 64 | Eigen::Matrix P; 65 | Eigen::Matrix delta; 66 | Eigen::Matrix H; 67 | Eigen::Matrix re; 68 | A.setZero(idx_.size(),5); 69 | L.setZero(idx_.size(),1); 70 | P.setZero(idx_.size(),idx_.size()); 71 | delta.resize(idx_.size(),1); 72 | H.resize(5,5); 73 | re.resize(idx_.size(),1); 74 | for(int i = 0;i R; 83 | R<3){ 123 | P(i,i) = pi*exp(1-(Vi/3.0)*(Vi/3.0)); 124 | } 125 | //if(Vi>=4.13){ 126 | // P(i,i) = 1.0/(Vi*Vi); 127 | //}else{ 128 | // P(i,i) = 1.0; 129 | //} 130 | } 131 | 132 | } 133 | 134 | for(size_t i = 0; i4*0.0028){ 136 | //if(P(npixel*2+1)<1.0){ 137 | cerr<<"id = "<