12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add test code

release/4.3a0
Nghia Ho 2018-12-13 17:28:52 -08:00
parent ff05eee0ca
commit 224af650bb
2 changed files with 1034 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

812
examples/Data/ISAM2_SmartFactor_valgrind.txt Normal file
View File

@ -0,0 +1,812 @@
i 1 0.485693 0.334961 10.0536 0.00585324 0.00159634 0.00532113
i 1 0.449805 0.346924 10.0536 0.00744958 0.00159634 0.00478901
i 1 0.430664 0.351709 10.0249 0.00904591 0.00159634 0.0042569
i 1 0.442627 0.346924 9.99858 0.0106423 0.00159634 0.00478901
i 1 0.449805 0.344531 9.99619 0.0117065 0.00159634 0.00478901
i 1 0.44502 0.330176 10.0034 0.0127707 0.00159634 0.00478901
i 1 0.411523 0.325391 9.99141 0.0138349 0.00159634 0.0042569
i 1 0.411523 0.322998 9.98184 0.014367 0.00106423 0.00372479
i 1 0.413916 0.325391 9.97944 0.0148992 0.000532113 0.00266056
i 1 0.413916 0.332568 9.9938 0.0159634 0 0.00212845
i 1 0.385205 0.327783 9.97944 0.0175597 0.000532113 0.00159634
i 1 0.358887 0.332568 9.93398 0.0186239 0.00106423 0.00106423
i 1 0.368457 0.327783 9.8981 0.0196882 0.00212845 0.00159634
i 1 0.37085 0.342139 9.88613 0.0212845 0.00266056 0.00212845
i 1 0.361279 0.346924 9.88374 0.0223487 0.00319268 0.00266056
i 1 0.337354 0.356494 9.86221 0.023413 0.00266056 0.00266056
i 1 0.330176 0.37085 9.82632 0.0244772 0.00212845 0.00266056
i 1 0.349316 0.38042 9.80957 0.0250093 0.00159634 0.00266056
i 1 0.361279 0.392383 9.81675 0.0255414 0.00159634 0.00266056
i 1 0.344531 0.399561 9.80239 0.0266056 0.00159634 0.00266056
i 1 0.339746 0.394775 9.78086 0.0276699 0.00212845 0.00212845
i 1 0.330176 0.387598 9.74736 0.028202 0.00266056 0.00212845
i 1 0.332568 0.382812 9.74736 0.028202 0.00319268 0.00212845
i 1 0.313428 0.373242 9.75933 0.028202 0.00319268 0.00212845
i 1 0.282324 0.361279 9.74497 0.0276699 0.00319268 0.00266056
i 1 0.284717 0.363672 9.74497 0.028202 0.00319268 0.00319268
i 1 0.303857 0.358887 9.74497 0.0287341 0.00266056 0.00372479
i 1 0.327783 0.363672 9.77129 0.0303304 0.00319268 0.00478901
i 2 0.313428 0.368457 9.77129 0.0313946 0.00372479 0.00532113
i 2 0.313428 0.37085 9.74019 0.0324589 0.00478901 0.00585324
i 2 0.322998 0.368457 9.71387 0.0335231 0.00532113 0.00691746
i 2 0.344531 0.382812 9.70669 0.032991 0.00585324 0.00744958
i 2 0.342139 0.38999 9.72344 0.032991 0.00585324 0.0085138
i 2 0.322998 0.399561 9.71147 0.0324589 0.00532113 0.0085138
i 2 0.322998 0.409131 9.69233 0.0324589 0.00478901 0.0085138
i 2 0.349316 0.409131 9.69233 0.0324589 0.0042569 0.0085138
i 2 0.358887 0.416309 9.71865 0.0324589 0.00372479 0.0085138
i 2 0.361279 0.418701 9.73779 0.032991 0.00372479 0.0085138
i 2 0.361279 0.430664 9.73779 0.0335231 0.0042569 0.00904591
i 2 0.382812 0.437842 9.72822 0.0340552 0.00478901 0.00904591
i 2 0.411523 0.452197 9.73301 0.0340552 0.00532113 0.00957803
i 2 0.416309 0.449805 9.74258 0.0340552 0.00585324 0.00957803
i 2 0.399561 0.45459 9.72583 0.0340552 0.00638535 0.00957803
i 2 0.406738 0.452197 9.7043 0.0335231 0.00691746 0.00904591
i 2 0.433057 0.45459 9.69233 0.0335231 0.00744958 0.0085138
i 2 0.456982 0.452197 9.7019 0.032991 0.00744958 0.0085138
i 2 0.461768 0.449805 9.69473 0.032991 0.00798169 0.00798169
i 2 0.452197 0.437842 9.65884 0.032991 0.00798169 0.00744958
i 2 0.466553 0.430664 9.62534 0.0335231 0.0085138 0.00691746
i 2 0.483301 0.428271 9.62534 0.032991 0.0085138 0.00585324
i 2 0.480908 0.433057 9.64448 0.032991 0.0085138 0.00532113
i 2 0.46416 0.437842 9.64688 0.032991 0.0085138 0.00478901
i 2 0.45459 0.449805 9.6397 0.032991 0.0085138 0.00478901
i 2 0.478516 0.461768 9.6397 0.032991 0.0085138 0.00478901
i 2 0.483301 0.471338 9.65405 0.032991 0.00904591 0.0042569
s 2 1 885 772.028 190.341
s 2 5 467 386.865 243.455
s 2 8 814 710.317 241.333
s 2 9 820 715.802 246.233
s 2 10 815 711.209 245.21
s 2 11 883 770.583 198.045
s 2 12 877 764.741 195.175
s 2 14 877 764.741 195.175
s 2 17 941 820.999 242.057
s 2 19 948 827.372 235.193
s 2 20 948 827.445 241.199
s 2 23 557 473.955 298.317
s 2 28 760 663.371 287.116
s 2 33 813 709.517 280.212
s 2 34 818 713.39 277.34
s 2 36 815 711.652 286.066
s 2 37 893 779.04 309.272
s 2 39 899 784.538 308.196
s 2 40 908 793.502 319.35
s 2 41 902 787.564 319.381
s 2 42 938 819.201 257.197
s 2 43 989 863.847 282.826
s 2 44 972 848.495 314.307
s 2 46 975 852.26 260.734
s 2 51 232 144.289 321.996
s 2 53 598 513.866 338.268
s 2 54 693 603.198 367.966
s 2 58 815 710.993 377.494
s 2 59 799 697.398 356.37
s 2 63 893 779.706 351.987
s 2 67 900 786.329 357.122
s 2 68 898 784.262 353.125
s 2 70 916 800.114 348.207
s 2 71 986 861.517 326.212
s 2 72 986 861.517 326.212
s 2 73 976 854.046 367.861
s 2 79 565 493.761 419.421
s 2 80 560 488.614 422.453
s 2 81 575 501.453 445.367
s 2 83 602 525.726 413.384
s 2 86 701 611.86 434.26
s 2 87 701 611.86 434.26
s 2 88 653 570.815 405.361
s 2 91 747 652.271 432.316
s 2 92 754 658.581 441.421
s 2 93 708 618.271 430.162
s 2 94 744 649.626 434.257
s 2 95 823 718.362 385.033
s 2 96 797 696.56 426.269
s 2 97 801 700.128 396.737
s 2 98 773 675.088 390.936
s 2 101 885 773.221 414.264
s 2 102 880 768.822 387.314
s 2 103 888 776.039 410.211
s 2 105 950 831.12 397.844
s 2 106 907 792.351 408.117
s 2 116 405 331.508 456.012
s 2 118 402 328.617 500.451
s 2 119 400 326.365 502.275
s 2 121 476 406.683 478.349
s 2 127 585 511.319 485.324
s 2 128 617 538.861 453.39
s 2 130 582 508.155 452.366
s 2 131 643 561.073 503.265
s 2 132 703 613.588 462.253
s 2 134 766 669.159 502.063
s 2 136 772 674.493 495.248
s 2 137 781 682.123 471.232
s 2 138 784 685.118 468.173
s 2 139 781 682.191 474.374
s 2 141 885 773.615 494.269
s 2 145 888 776.431 492.348
s 2 147 904 790.471 502.254
s 2 148 902 788.507 504.272
s 2 149 938 820.465 502.391
s 2 152 963 842.721 503.376
s 2 156 242 148.008 545.764
s 2 158 248 154.208 545.888
s 2 162 314 224.505 546.204
s 2 163 315 225.905 530.765
s 2 164 315 225.261 552.166
s 2 166 368 291.55 564.057
s 2 171 571 498.553 561.333
s 2 172 572 499.545 563.335
s 2 173 638 556.502 563.852
s 2 175 636 554.844 552.416
s 2 176 641 559.993 556.383
s 2 178 692 604.702 533.137
s 2 180 722 630.968 528.786
s 2 181 716 624.803 518.298
s 2 182 764 666.755 536.361
s 2 185 769 671.642 537.317
s 2 187 810 708.021 538.302
s 2 189 841 735.447 558.082
s 2 190 881 770.988 555.105
s 2 192 860 754.238 555.186
s 2 193 938 820.657 574.383
s 2 194 948 829.586 514.034
s 2 195 915 800.701 574.39
s 2 196 904 790.859 538.891
s 2 197 976 854.831 525.328
s 2 204 321 230.814 584.287
s 2 205 322 231.43 590.824
s 2 210 507 413.694 594.058
s 2 216 586 512.591 601.786
s 2 217 585 511.221 598.227
s 2 218 657 574.472 587.268
s 2 219 675 589.664 581.33
s 2 220 656 573.502 582.378
s 2 221 651 568.578 581.394
s 2 223 715 624.955 635.236
s 2 224 828 724.063 600.264
s 2 225 813 710.869 596.413
s 2 227 882 771.755 624.292
s 2 231 977 855.606 577.181
s 2 233 252 132.188 646.925
s 2 234 253 133.288 649.949
s 2 236 264 144.633 647.915
s 2 237 335 226.123 646.194
s 2 240 448 356.089 665.294
s 2 241 465 362.168 676.569
s 2 242 481 383.624 673.992
s 2 244 478 380.89 673.024
s 2 245 562 458.491 663.422
s 2 246 565 461.494 669.205
s 2 247 548 446.265 666.974
s 2 249 607 530.299 640.373
s 2 250 613 512.347 667.362
s 2 252 687 591.589 664.392
s 2 253 698 601.698 663.19
s 2 254 700 603.327 667.232
s 2 255 656 563.305 657.902
s 2 256 694 598.036 667.268
s 2 257 714 618.145 662.355
s 2 258 708 618.512 645.366
s 2 259 726 634.945 643.204
s 2 260 713 616.887 667.415
s 2 261 730 638.995 646.386
s 2 264 776 678.805 643.356
s 2 265 769 672.092 642.262
s 2 267 895 781.553 683.16
s 2 268 895 781.553 683.16
s 2 269 891 777.506 679.182
s 2 271 888 774.288 683.233
s 2 272 898 784.251 690.38
s 2 273 956 836.692 655.37
s 2 274 947 828.873 673.098
s 2 275 359 196.329 730.773
s 2 276 359 196.329 730.773
s 2 277 444 291.606 761.777
s 2 278 463 357.487 706.022
s 2 282 617 476.167 725.153
s 2 285 664 515.317 729.1
s 2 286 661 513.039 724.11
s 2 288 659 511.731 743.188
s 2 289 744 604.533 735.338
s 2 290 740 601.769 711.221
s 2 291 742 601.937 737.397
s 2 292 740 600.916 732.141
s 2 295 775 641.709 727.255
s 2 301 313 146.314 779.858
s 2 302 291 111.606 782.816
s 2 304 376 193.757 790.556
s 2 305 373 191.601 783.684
s 2 306 385 200.302 789.599
s 2 307 406 218.92 809.926
s 2 310 455 295.709 773.864
s 2 311 472 310.994 772.922
s 2 312 544 356.24 799.68
s 2 316 758 564.263 809.259
s 2 321 801 633.92 774.402
s 2 323 446 219.679 857.011
s 2 328 668 430.1 872.24
s 2 329 750 498.621 887.234
s 2 330 765 511.029 890.334
s 2 331 745 494.602 886.002
s 2 333 862 641.477 846.36
s 2 335 872 644.41 859.881
s 2 339 404 147.16 898.548
s 2 345 589 323.535 907.722
i 3 0.471338 0.478516 9.65645 0.032991 0.0101101 0.0042569
i 3 0.471338 0.490479 9.62773 0.0324589 0.0111744 0.00319268
i 3 0.480908 0.492871 9.58706 0.0319268 0.0117065 0.00266056
i 3 0.502441 0.504834 9.56553 0.0313946 0.0117065 0.00212845
i 3 0.502441 0.502441 9.55596 0.0313946 0.0122386 0.00266056
i 3 0.46416 0.507227 9.53203 0.0313946 0.0122386 0.00319268
i 3 0.456982 0.509619 9.50332 0.0319268 0.0122386 0.00372479
i 3 0.45459 0.516797 9.49136 0.0313946 0.0127707 0.00372479
i 3 0.466553 0.526367 9.5105 0.0308625 0.0133028 0.00372479
i 3 0.452197 0.52876 9.52485 0.0303304 0.014367 0.00319268
i 3 0.425879 0.535937 9.50571 0.0292662 0.0154313 0.00266056
i 3 0.428271 0.533545 9.50571 0.0292662 0.0164955 0.00212845
i 3 0.425879 0.545508 9.51528 0.0287341 0.0170276 0.00159634
i 3 0.411523 0.5479 9.54399 0.028202 0.0170276 0.00159634
i 3 0.382812 0.559863 9.56074 0.0276699 0.0170276 0.00106423
i 3 0.375635 0.571826 9.54878 0.0276699 0.0159634 0.000532113
i 3 0.387598 0.581396 9.54878 0.0266056 0.0148992 0.000532113
i 3 0.401953 0.586182 9.55835 0.0250093 0.014367 0.000532113
i 3 0.382812 0.590967 9.56074 0.0239451 0.0138349 0.000532113
i 3 0.363672 0.593359 9.52725 0.0228808 0.0138349 0.000532113
i 3 0.366064 0.598145 9.50571 0.0223487 0.0138349 0.000532113
i 3 0.37085 0.617285 9.5105 0.0223487 0.0138349 0
i 3 0.356494 0.624463 9.53921 0.0228808 0.014367 0
i 3 0.327783 0.634033 9.55596 0.0223487 0.0148992 0
i 3 0.308643 0.629248 9.56074 0.0218166 0.0159634 0
i 3 0.320605 0.638818 9.57031 0.0212845 0.0164955 0
i 3 0.322998 0.641211 9.59185 0.0202203 0.0170276 0
s 3 1 885 771.886 190.394
s 3 8 814 710.222 241.36
s 3 10 815 711.117 244.276
s 3 11 883 770.656 198.02
s 3 13 874 761.801 193.21
s 3 15 882 769.349 194.094
s 3 16 952 831.218 238.215
s 3 17 940 820.031 242.02
s 3 19 948 827.354 235.194
s 3 20 948 827.391 241.213
s 3 28 760 663.312 287.142
s 3 33 813 709.44 279.254
s 3 34 819 714.246 276.35
s 3 36 815 711.636 286.046
s 3 37 893 779.028 309.293
s 3 39 898 783.513 307.182
s 3 40 908 793.523 318.373
s 3 42 938 819.064 256.113
s 3 43 989 863.966 281.804
s 3 45 967 843.818 310.173
s 3 46 975 852.424 259.949
s 3 51 231 143.198 321.116
s 3 52 229 140.636 323.223
s 3 53 598 513.854 337.163
s 3 58 815 711.008 377.495
s 3 59 798 696.404 357.311
s 3 61 798 696.404 357.311
s 3 63 893 779.621 352
s 3 67 900 786.305 357.088
s 3 68 898 784.227 353.135
s 3 70 917 801.17 347.212
s 3 71 986 861.382 325.185
s 3 72 985 860.395 327.196
s 3 79 564 492.659 419.338
s 3 80 560 488.554 421.427
s 3 81 575 501.51 445.356
s 3 83 603 526.725 413.46
s 3 84 586 511.864 447.453
s 3 85 650 567.813 402.398
s 3 86 700 611.102 433.159
s 3 87 703 613.898 434.235
s 3 88 652 569.851 405.336
s 3 90 706 616.823 433.228
s 3 91 747 652.473 432.3
s 3 92 754 658.648 441.454
s 3 93 708 618.268 429.213
s 3 94 743 648.861 434.243
s 3 95 823 718.34 385.05
s 3 96 796 695.581 425.304
s 3 97 800 699.235 396.68
s 3 98 773 675.006 390.852
s 3 101 884 772.387 414.266
s 3 102 879 768.099 387.4
s 3 103 888 775.703 409.171
s 3 106 907 793.163 407.121
s 3 109 224 130.501 479.026
s 3 110 232 140.654 474.298
s 3 112 353 276.953 460.195
s 3 113 326 247.913 479.275
s 3 114 323 244.999 481.419
s 3 116 405 331.557 456.078
s 3 118 402 328.474 500.448
s 3 121 476 406.808 478.331
s 3 124 560 488.874 453.428
s 3 125 559 487.866 451.429
s 3 127 585 511.21 485.366
s 3 130 582 508.117 452.34
s 3 131 642 560.191 503.303
s 3 132 703 613.512 462.274
s 3 134 766 669.178 501.123
s 3 137 780 681.115 471.213
s 3 138 785 686.111 468.15
s 3 140 766 669.178 501.123
s 3 147 904 790.478 501.294
s 3 149 938 820.46 502.412
s 3 152 963 842.7 502.39
s 3 156 242 148.04 545.744
s 3 158 248 154.13 544.882
s 3 162 314 224.573 545.257
s 3 163 315 225.942 530.779
s 3 164 315 225.25 552.16
s 3 165 320 230.24 575.172
s 3 166 368 292.361 563.739
s 3 167 411 337.518 559.362
s 3 169 456 385.345 575.417
s 3 170 576 503.558 560.339
s 3 171 572 499.542 562.338
s 3 172 572 499.542 562.338
s 3 173 638 556.345 563.543
s 3 175 636 554.93 552.498
s 3 178 692 604.626 533.07
s 3 179 763 665.69 529.344
s 3 181 716 624.895 518.341
s 3 182 764 666.738 535.365
s 3 185 769 671.706 536.364
s 3 186 769 671.706 536.364
s 3 189 841 735.519 558.219
s 3 190 881 771.578 554.18
s 3 191 839 733.85 555.19
s 3 193 938 820.706 573.366
s 3 194 948 829.543 513.025
s 3 196 904 790.951 538.836
s 3 197 976 854.393 525.427
s 3 198 236 140.322 603.995
s 3 199 235 139.053 600.975
s 3 203 290 208.701 584.181
s 3 204 321 230.851 584.311
s 3 205 322 231.422 590.784
s 3 207 425 352.676 598.285
s 3 210 507 413.915 594.08
s 3 216 586 512.672 602.104
s 3 217 585 511.513 598.4
s 3 218 657 574.43 587.301
s 3 219 675 589.734 581.331
s 3 220 656 573.443 582.387
s 3 221 650 567.724 580.446
s 3 223 715 624.944 635.149
s 3 224 828 723.983 600.259
s 3 225 813 710.87 596.451
s 3 231 977 855.78 576.339
s 3 233 252 132.151 646.953
s 3 235 270 151.285 641.899
s 3 236 263 143.885 646.853
s 3 237 335 226.072 646.176
s 3 240 448 356.109 665.276
s 3 241 465 362.114 676.566
s 3 242 481 383.735 674.003
s 3 243 472 384.478 660.065
s 3 244 478 381.18 672.03
s 3 246 564 460.41 668.205
s 3 247 548 446.541 666.983
s 3 249 607 530.254 640.36
s 3 251 615 515.283 663.243
s 3 252 687 591.632 664.369
s 3 253 698 601.651 663.156
s 3 254 700 603.327 667.192
s 3 256 693 597.206 667.286
s 3 257 714 618.171 662.386
s 3 258 708 618.448 645.308
s 3 259 726 634.919 642.193
s 3 260 713 616.921 667.437
s 3 261 730 638.986 646.352
s 3 264 776 678.909 643.341
s 3 268 893 779.545 683.224
s 3 271 888 774.345 683.25
s 3 272 898 784.392 690.3
s 3 273 956 836.616 654.36
s 3 274 947 828.918 673.067
s 3 275 359 196.307 730.769
s 3 276 356 193.389 729.874
s 3 277 444 291.736 761.796
s 3 278 463 357.497 706.028
s 3 279 610 473.389 731.03
s 3 281 602 465.988 723.373
s 3 285 665 516.633 728.092
s 3 286 661 513.323 723.09
s 3 288 659 511.572 743.182
s 3 290 738 599.252 709.299
s 3 292 740 600.731 731.179
s 3 301 313 146.474 779.852
s 3 304 375 193.685 790.593
s 3 305 373 191.613 783.699
s 3 306 385 200.249 789.607
s 3 307 406 218.946 809.916
s 3 310 455 295.762 773.851
s 3 311 472 311.008 772.914
s 3 312 544 356.234 798.671
s 3 316 757 563.508 808.199
s 3 323 445 218.732 856.915
s 3 328 668 427.886 871.306
s 3 329 750 498.489 887.267
s 3 330 765 510.975 890.36
s 3 331 744 494.093 885.015
s 3 333 863 642.396 846.332
s 3 334 857 636.646 843.358
s 3 343 514 229.103 934.981
i 4 0.294287 0.660352 9.6062 0.0191561 0.0175597 -0.000532113
i 4 0.265576 0.674707 9.57988 0.0180918 0.0175597 0
i 4 0.256006 0.698633 9.56074 0.0170276 0.0170276 0
i 4 0.265576 0.715381 9.56553 0.0164955 0.0164955 0.000532113
i 4 0.265576 0.727344 9.58467 0.0164955 0.0154313 0.000532113
i 4 0.239258 0.732129 9.59424 0.0164955 0.0148992 0.00106423
i 4 0.22251 0.732129 9.57749 0.0164955 0.0138349 0.00159634
i 4 0.23208 0.729736 9.57988 0.0164955 0.0133028 0.00159634
i 4 0.227295 0.727344 9.58945 0.0159634 0.0133028 0.00212845
i 4 0.203369 0.736914 9.60142 0.0148992 0.0127707 0.00159634
i 4 0.179443 0.734522 9.59902 0.0138349 0.0122386 0.00159634
i 4 0.172266 0.741699 9.59424 0.0133028 0.0117065 0.00159634
i 4 0.193799 0.741699 9.63013 0.0127707 0.0111744 0.00212845
i 4 0.186621 0.741699 9.66841 0.0122386 0.0106423 0.00266056
i 4 0.172266 0.741699 9.68994 0.0122386 0.0101101 0.00319268
i 4 0.160303 0.748877 9.67798 0.0122386 0.0101101 0.00372479
i 4 0.181836 0.756055 9.67798 0.0117065 0.00957803 0.0042569
i 4 0.200977 0.775195 9.69951 0.0117065 0.00904591 0.00478901
i 4 0.186621 0.77998 9.7043 0.0111744 0.00957803 0.00478901
i 4 0.162695 0.777588 9.68755 0.0106423 0.0101101 0.00478901
i 4 0.155518 0.768018 9.66123 0.0106423 0.0111744 0.00532113
i 4 0.16748 0.748877 9.67319 0.0111744 0.0122386 0.00638535
i 4 0.162695 0.739307 9.7043 0.0111744 0.0127707 0.00798169
i 4 0.153125 0.732129 9.70908 0.0117065 0.0122386 0.00904591
i 4 0.150732 0.727344 9.69233 0.0122386 0.0117065 0.00957803
i 4 0.179443 0.739307 9.66841 0.0133028 0.0111744 0.0101101
s 4 1 885 771.994 190.366
s 4 8 814 710.223 241.363
s 4 10 815 711.068 244.306
s 4 11 882 769.488 198.098
s 4 12 876 763.708 195.193
s 4 13 874 761.731 192.256
s 4 15 882 769.341 194.117
s 4 16 952 831.207 237.222
s 4 17 940 820.094 241.983
s 4 18 945 824.618 239.15
s 4 19 948 827.346 234.198
s 4 22 432 359.221 264.591
s 4 28 760 663.293 287.118
s 4 33 813 709.512 279.215
s 4 34 818 713.314 276.343
s 4 36 815 711.529 285.057
s 4 37 893 778.938 308.242
s 4 39 898 783.55 307.164
s 4 40 907 792.695 318.382
s 4 41 902 787.654 318.396
s 4 42 938 819.106 256.202
s 4 43 989 863.908 281.834
s 4 44 971 847.662 314.346
s 4 45 968 844.579 310.273
s 4 46 975 852.254 260.043
s 4 52 229 140.68 323.217
s 4 53 598 514.121 337.175
s 4 58 815 710.937 376.471
s 4 59 798 696.344 357.345
s 4 61 796 694.481 358.445
s 4 63 893 779.58 351.02
s 4 67 899 785.244 357.125
s 4 68 898 784.179 353.158
s 4 70 916 800.164 347.205
s 4 71 987 862.374 323.193
s 4 72 984 859.517 326.195
s 4 78 349 274.104 444.811
s 4 79 564 492.65 419.414
s 4 81 575 501.478 444.409
s 4 83 601 524.76 412.446
s 4 84 586 511.86 446.439
s 4 85 650 567.837 402.416
s 4 86 700 611.156 433.123
s 4 87 702 612.913 434.209
s 4 90 706 616.807 433.199
s 4 91 746 651.408 432.284
s 4 93 707 617.559 430.146
s 4 94 743 648.795 434.231
s 4 95 823 718.332 385.052
s 4 96 796 695.579 425.228
s 4 97 800 699.055 396.852
s 4 98 773 674.916 390.759
s 4 102 879 768.454 386.971
s 4 103 888 776.252 409.162
s 4 106 907 793.277 407.058
s 4 109 224 130.613 478.998
s 4 110 232 140.621 473.277
s 4 112 353 277.014 460.171
s 4 114 323 244.961 480.36
s 4 115 349 272.766 463.341
s 4 116 405 331.398 455.088
s 4 118 401 327.557 499.323
s 4 119 401 327.557 499.323
s 4 120 395 321.112 493.435
s 4 121 476 406.743 478.377
s 4 127 586 512.557 483.379
s 4 130 582 508.157 452.348
s 4 131 642 560.134 502.312
s 4 132 703 613.486 461.235
s 4 134 764 667.34 501.126
s 4 136 773 675.456 494.29
s 4 137 780 681.045 471.208
s 4 138 784 685.045 467.118
s 4 139 781 682.087 473.259
s 4 140 766 669.129 501.112
s 4 141 884 772.588 493.247
s 4 143 860 752.712 506.715
s 4 149 938 820.417 502.393
s 4 152 963 842.708 502.408
s 4 156 242 148.092 545.745
s 4 158 248 154.161 544.898
s 4 162 314 224.577 545.216
s 4 163 315 225.946 530.744
s 4 164 315 225.235 552.134
s 4 165 320 230.327 575.153
s 4 167 411 337.774 559.343
s 4 169 456 385.542 575.339
s 4 170 575 502.563 557.259
s 4 171 572 499.587 562.29
s 4 172 572 499.587 562.29
s 4 173 638 556.429 563.579
s 4 175 636 554.961 551.494
s 4 177 641 559.492 561.262
s 4 178 692 604.626 533.116
s 4 179 763 665.738 529.302
s 4 181 716 624.827 517.256
s 4 182 764 666.839 535.368
s 4 185 769 671.733 536.347
s 4 186 769 671.733 536.347
s 4 187 810 708.024 538.294
s 4 189 841 735.558 558.172
s 4 190 881 772.053 554.336
s 4 191 840 734.563 554.241
s 4 192 860 753.225 554.14
s 4 193 938 820.664 573.398
s 4 194 948 829.589 513.021
s 4 196 904 790.866 537.836
s 4 197 976 854.583 524.319
s 4 198 236 140.38 603.999
s 4 201 288 206.836 586.271
s 4 204 321 230.91 583.331
s 4 207 425 352.585 598.27
s 4 210 507 413.476 594.048
s 4 216 586 512.578 601.8
s 4 217 585 511.154 598.205
s 4 218 657 574.497 587.3
s 4 220 656 573.483 582.382
s 4 221 651 568.555 580.399
s 4 223 715 624.942 635.151
s 4 224 827 722.978 600.292
s 4 231 977 855.254 576.338
s 4 234 252 132.144 646.968
s 4 240 448 356.099 665.285
s 4 242 481 383.757 674.017
s 4 244 478 381.174 672.047
s 4 246 564 460.379 668.196
s 4 247 548 446.621 666.998
s 4 249 607 530.219 640.38
s 4 250 613 512.601 667.348
s 4 251 616 516.659 662.172
s 4 252 687 591.584 663.361
s 4 253 698 601.767 663.178
s 4 254 700 603.389 667.207
s 4 256 693 597.195 667.276
s 4 258 708 618.612 644.382
s 4 259 726 634.907 642.21
s 4 260 713 616.921 667.416
s 4 261 730 638.979 646.36
s 4 267 895 781.565 682.132
s 4 268 893 779.549 683.206
s 4 272 898 784.406 690.256
s 4 273 956 836.687 654.365
s 4 274 947 828.881 671.937
s 4 275 359 196.281 730.773
s 4 276 356 193.356 729.874
s 4 277 444 291.69 761.788
s 4 278 463 357.45 704.999
s 4 281 602 466.206 723.378
s 4 283 621 486.721 706.163
s 4 286 661 513.021 723.096
s 4 288 657 510.171 742.231
s 4 290 738 599.306 709.267
s 4 292 740 600.83 731.158
s 4 301 313 146.359 779.839
s 4 303 308 140.181 777.88
s 4 310 454 294.447 773.873
i 5 0.200977 0.746484 9.66602 0.0133028 0.0106423 0.00957803
i 5 0.196191 0.756055 9.66602 0.0133028 0.0101101 0.00957803
i 5 0.189014 0.765625 9.65166 0.0122386 0.00957803 0.00957803
i 5 0.193799 0.772803 9.64209 0.0106423 0.00957803 0.00957803
i 5 0.217725 0.777588 9.66123 0.00904591 0.00904591 0.00957803
i 5 0.24165 0.777588 9.69233 0.00744958 0.00904591 0.0106423
i 5 0.234473 0.77041 9.7019 0.00691746 0.00957803 0.0111744
i 5 0.248828 0.765625 9.68276 0.00638535 0.00957803 0.0122386
i 5 0.275146 0.75127 9.67319 0.00691746 0.00957803 0.0122386
i 5 0.291895 0.748877 9.67319 0.00744958 0.00904591 0.0117065
i 5 0.270361 0.744092 9.68276 0.00798169 0.0085138 0.0106423
i 5 0.246436 0.746484 9.66602 0.00904591 0.00798169 0.0101101
i 5 0.234473 0.748877 9.64448 0.00957803 0.00798169 0.00904591
i 5 0.248828 0.758447 9.64209 0.0101101 0.00798169 0.00904591
i 5 0.253613 0.765625 9.6397 0.0106423 0.00798169 0.00904591
i 5 0.246436 0.753662 9.61816 0.0106423 0.00904591 0.00904591
i 5 0.248828 0.744092 9.58706 0.0106423 0.00904591 0.00957803
i 5 0.282324 0.720166 9.57271 0.0106423 0.0085138 0.0101101
i 5 0.30625 0.710596 9.58945 0.0101101 0.00744958 0.0106423
i 5 0.30625 0.708203 9.60142 0.00904591 0.00638535 0.0111744
i 5 0.294287 0.710596 9.59663 0.00798169 0.00585324 0.0117065
i 5 0.287109 0.720166 9.57749 0.00744958 0.00585324 0.0111744
i 5 0.30625 0.724951 9.59185 0.00691746 0.00585324 0.0111744
i 5 0.299072 0.722559 9.61577 0.00691746 0.00585324 0.0106423
i 5 0.267969 0.712988 9.61338 0.00638535 0.00532113 0.0101101
i 5 0.260791 0.703418 9.57988 0.00585324 0.00478901 0.00957803
i 5 0.272754 0.693848 9.55356 0.00585324 0.0042569 0.00957803
s 5 1 885 771.967 190.374
s 5 8 814 710.258 241.348
s 5 11 882 769.5 198.11
s 5 12 876 763.725 195.196
s 5 13 874 761.761 192.265
s 5 14 876 763.725 195.196
s 5 15 882 769.333 194.13
s 5 16 952 831.211 238.222
s 5 17 941 820.874 241.088
s 5 19 948 827.331 234.199
s 5 28 760 663.304 287.152
s 5 33 813 709.417 279.246
s 5 34 818 713.31 276.339
s 5 36 815 711.553 285.069
s 5 37 893 778.921 308.279
s 5 39 898 783.503 307.195
s 5 40 908 793.456 318.342
s 5 41 902 787.598 318.335
s 5 42 938 819.203 256.15
s 5 43 989 863.912 281.803
s 5 44 972 848.245 313.371
s 5 45 968 844.392 308.24
s 5 46 975 852.111 259.768
s 5 52 229 140.643 323.25
s 5 58 815 711.009 376.479
s 5 59 797 695.427 357.337
s 5 61 797 695.427 357.337
s 5 63 893 779.641 350.98
s 5 67 899 785.25 357.122
s 5 70 916 800.144 347.212
s 5 71 986 861.363 325.258
s 5 72 986 861.363 325.258
s 5 78 349 272.718 444.977
s 5 79 564 492.645 419.352
s 5 81 575 501.478 444.458
s 5 83 601 524.669 412.369
s 5 84 586 511.845 446.414
s 5 85 650 567.806 402.399
s 5 86 701 611.648 433.295
s 5 87 701 611.648 433.295
s 5 90 706 616.746 433.269
s 5 91 747 652.381 432.302
s 5 93 708 618.322 429.229
s 5 94 744 649.616 434.254
s 5 95 823 718.328 385.098
s 5 96 796 695.506 425.257
s 5 97 800 699.015 396.835
s 5 98 773 674.898 390.815
s 5 103 888 776.053 409.196
s 5 106 907 793.174 407.16
s 5 109 224 130.598 479.009
s 5 112 353 277.059 460.178
s 5 113 326 247.919 479.3
s 5 114 323 245.04 481.416
s 5 118 402 328.417 500.439
s 5 119 400 326.248 502.231
s 5 121 476 406.735 478.368
s 5 127 585 511.229 485.269
s 5 130 582 508.086 452.315
s 5 131 643 561.152 502.328
s 5 132 703 613.454 461.25
s 5 134 766 669.174 501.069
s 5 136 773 675.449 494.288
s 5 137 781 682.128 471.206
s 5 138 784 685.119 467.122
s 5 139 781 682.162 473.269
s 5 140 766 669.174 501.069
s 5 145 888 776.3 491.33
s 5 149 938 820.54 502.447
s 5 152 963 842.697 502.391
s 5 156 242 148.049 545.748
s 5 158 248 154.189 544.861
s 5 162 314 224.58 545.228
s 5 163 315 225.943 530.706
s 5 164 315 225.254 552.161
s 5 165 320 230.248 573.933
s 5 167 411 337.603 559.374
s 5 169 456 385.477 575.333
s 5 170 575 502.564 557.297
s 5 171 572 499.585 562.324
s 5 172 572 499.585 562.324
s 5 173 638 556.357 563.566
s 5 177 641 559.473 561.27
s 5 178 693 605.572 532.124
s 5 179 763 665.768 529.292
s 5 181 716 624.878 517.287
s 5 182 764 666.826 535.34
s 5 185 769 671.754 536.33
s 5 186 769 671.754 536.33
s 5 187 810 707.981 538.318
s 5 189 841 735.367 557.132
s 5 190 881 771.284 554.167
s 5 191 840 734.433 554.139
s 5 192 860 752.832 554.234
s 5 193 938 820.661 573.411
s 5 194 948 829.548 513.027
s 5 196 904 790.811 537.922
s 5 197 976 854.459 524.408
s 5 198 236 140.438 604.965
s 5 199 235 139.069 600.972
s 5 201 288 206.856 586.255
s 5 203 290 208.734 584.196
s 5 204 321 230.86 584.302
s 5 205 322 231.412 590.747
s 5 207 425 352.658 598.284
s 5 208 430 357.846 600.368
s 5 209 430 357.846 600.368
s 5 210 507 413.862 594.141
s 5 216 586 512.524 601.846
s 5 217 585 511.371 598.24
s 5 220 656 573.388 582.4
s 5 221 650 567.67 580.444
s 5 223 715 625.005 635.16
s 5 224 828 724.003 600.283
s 5 231 977 856.036 576.324
s 5 233 252 132.27 646.898
s 5 235 270 151.147 641.895
s 5 237 335 226.3 646.265
s 5 240 448 356.101 665.311
s 5 241 465 362.419 675.526
s 5 242 482 384.778 674.024
s 5 244 478 381.143 672.035
s 5 246 564 460.402 668.186
s 5 247 548 446.693 666.997
s 5 250 613 512.473 667.358
s 5 251 615 515.389 663.256
s 5 252 687 591.542 663.355
s 5 253 698 601.734 663.164
s 5 254 700 603.352 667.193
s 5 257 714 618.128 662.357
s 5 258 708 618.561 644.403
s 5 259 726 634.882 642.196
s 5 260 713 616.859 667.397
s 5 261 730 638.896 646.366
s 5 264 776 678.964 643.331
s 5 268 893 779.594 683.216
s 5 271 888 774.41 682.262
s 5 273 956 836.775 654.374
s 5 274 947 828.877 671.995
s 5 275 359 196.336 730.802
s 5 276 356 193.409 729.896
s 5 277 444 291.551 761.797
s 5 278 464 358.548 705.089
s 5 279 610 473.571 731.066
s 5 281 602 466.135 723.367
s 5 282 617 475.86 725.172
s 5 285 665 516.486 728.097
s 5 286 661 513.146 723.099
s 5 289 743 603.491 734.339
s 5 290 738 599.323 709.259
s 5 292 740 600.943 731.142
s 5 301 313 146.284 779.851
s 5 304 375 193.624 790.587
s 5 306 385 200.303 789.594
s 5 307 406 218.914 809.947
s 5 310 455 295.69 773.843
s 5 311 472 310.886 772.897
s 5 312 544 356.329 798.683
s 5 329 750 498.867 886.161
s 5 330 765 511.01 889.347
s 5 331 745 495.029 884.943
s 5 333 862 641.525 845.353
s 5 334 858 637.569 843.345
s 5 345 589 322.892 907.696
s 5 349 765 431.497 988.976

222
examples/ISAM2_SmartFactor_valgrind.cpp Normal file
View File

@ -0,0 +1,222 @@
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <iostream>
#include <vector>
#include <sstream>
#include <fstream>
using namespace std;
using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
struct IMUHelper
{
IMUHelper()
{
gtsam::Matrix33 measured_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(0.113, 2.0);
gtsam::Matrix33 measured_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(4.0e-5, 2.0);
gtsam::Matrix33 bias_acc_cov = gtsam::Matrix33::Identity(3,3) * pow( 0.00002, 2.0);
gtsam::Matrix33 bias_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(0.001, 2.0);
gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-9; // error committed in integrating position from velocities
gtsam::Matrix bias_acc_omega_int = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
{
auto gaussian = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) <<
gtsam::Vector3::Constant(5.0e-2),
gtsam::Vector3::Constant(5.0e-3)).finished());
auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
bias_noise_model = huber;
}
{
auto gaussian = gtsam::noiseModel::Isotropic::Sigma(3,0.01);
auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
velocity_noise_model = huber;
}
// expect IMU to be rotated in image space co-ords
auto p = boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(gtsam::Vector3(0.0, 9.8, 0.0));
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
// from SVD comparing gyro and vision estimates, no bias modelled
gtsam::Rot3 body_to_imu_rot(
3.612861008216737e-02, -9.987267865568920e-01, 3.520695026944293e-02,
4.541686330383411e-02, -3.355264881270600e-02, -9.984044913186698e-01,
9.983145957368207e-01, 3.766995581886975e-02, 4.414682737675374e-02);
gtsam::Point3 body_to_imu_trans(0.03, -0.025, -0.06);
p->body_P_sensor = gtsam::Pose3(body_to_imu_rot, body_to_imu_trans);
gtsam::Rot3 prior_rotation = gtsam::Rot3(gtsam::Matrix33::Identity());
gtsam::Pose3 prior_pose(prior_rotation, gtsam::Point3(0,0,0));
gtsam::Vector3 acc_bias(2.05998e-18, -0.0942015, 1.17663e-16);
gtsam::Vector3 gyro_bias(0,0,0);
prior_imu_bias = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias);
prev_state = gtsam::NavState(prior_pose, gtsam::Vector3(0,0,0));
prop_state = prev_state;
prev_bias = prior_imu_bias;
preintegrated = new gtsam::PreintegratedCombinedMeasurements(p, prior_imu_bias);
}
gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
gtsam::noiseModel::Robust::shared_ptr velocity_noise_model;
gtsam::noiseModel::Robust::shared_ptr bias_noise_model;
gtsam::NavState prev_state;
gtsam::NavState prop_state;
gtsam::imuBias::ConstantBias prev_bias;
gtsam::PreintegratedCombinedMeasurements *preintegrated;
};
int main(int argc, char* argv[]) {
if (argc != 2) {
cout << "./main [data.txt]\n";
return 0;
}
double fx = 8.2237229542137766e+02;
double fy = fx;
double cx = 5.3872524261474609e+02;
double cy = 5.7909587860107422e+02;
double baseline = 7.4342307430851651e-01;
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.1;
ISAM2 isam(parameters);
// Create a factor graph
std::map<size_t, gtsam::SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
NonlinearFactorGraph graph;
Values initialEstimate;
IMUHelper imu;
// Pose prior
auto priorPoseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(), priorPoseNoise);
initialEstimate.insert(X(0), Pose3::identity());
// Bias prior
graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.prior_imu_bias, imu.bias_noise_model));
initialEstimate.insert(B(0), imu.prior_imu_bias);
// Velocity prior
graph.add(PriorFactor<Vector3>(V(1), Vector3(0,0,0), imu.velocity_noise_model));
initialEstimate.insert(V(0), Vector3(0,0,0));
ifstream in(argv[1]);
if (!in) {
cerr << "error opening: " << argv[1] << "\n";
return 1;
}
int last_frame = 1;
int frame;
while (true) {
char line[1024];
in.getline(line, sizeof(line));
stringstream ss(line);
char type;
ss >> type;
ss >> frame;
if (frame != last_frame || in.eof()) {
cout << "Running isam for frame: " << last_frame << "\n";
initialEstimate.insert(X(last_frame), Pose3::identity());
initialEstimate.insert(V(last_frame), Vector3(0,0,0));
initialEstimate.insert(B(last_frame), imu.prev_bias);
CombinedImuFactor imu_factor(
X(last_frame-1), V(last_frame-1),
X(last_frame), V(last_frame),
B(last_frame-1), B(last_frame),
*imu.preintegrated);
graph.add(imu_factor);
isam.update(graph, initialEstimate);
Values currentEstimate = isam.calculateEstimate();
//currentEstimate.print("Current estimate: ");
imu.prop_state = imu.preintegrated->predict(imu.prev_state, imu.prev_bias);
imu.prev_state = NavState(currentEstimate.at<Pose3>(X(last_frame)), currentEstimate.at<Vector3>(V(last_frame)));
imu.prev_bias = currentEstimate.at<imuBias::ConstantBias>(B(last_frame));
imu.preintegrated->resetIntegrationAndSetBias(imu.prev_bias);
graph.resize(0);
initialEstimate.clear();
if (in.eof()) {
break;
}
}
if (type == 'i') {
double ax, ay, az;
double gx, gy, gz;
double dt = 1/800.0;
ss >> ax;
ss >> ay;
ss >> az;
ss >> gx;
ss >> gy;
ss >> gz;
Vector3 acc(ax, ay, az);
Vector3 gyr(gx, gy, gz);
imu.preintegrated->integrateMeasurement(acc, gyr, dt);
} else if (type == 's') {
int landmark;
double xl, xr, y;
ss >> landmark;
ss >> xl;
ss >> xr;
ss >> y;
if (smartFactors.count(landmark) == 0) {
auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(new SmartStereoProjectionPoseFactor(gaussian, params));
graph.push_back(smartFactors[landmark]);
}
smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
}
last_frame = frame;
}
return 0;
}