From ff05eee0ca8062221c88ab01b42154ff2a89ba86 Mon Sep 17 00:00:00 2001 From: Nghia Ho Date: Thu, 13 Dec 2018 17:03:31 -0800 Subject: [PATCH 1/6] fix for valgrind invalid read warning --- gtsam/nonlinear/ISAM2Clique.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index c55ca7959..48295f338 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -172,7 +172,7 @@ bool ISAM2Clique::valuesChanged(const KeySet& replaced, double threshold) const { auto frontals = conditional_->frontals(); if (replaced.exists(frontals.front())) return true; - auto diff = originalValues - delta.vector(frontals); + Vector diff = originalValues - delta.vector(frontals); return diff.lpNorm() >= threshold; } From 224af650bb337f40ac5e7d3a998353dc30991640 Mon Sep 17 00:00:00 2001 From: Nghia Ho Date: Thu, 13 Dec 2018 17:28:52 -0800 Subject: [PATCH 2/6] add test code --- examples/Data/ISAM2_SmartFactor_valgrind.txt | 812 +++++++++++++++++++ examples/ISAM2_SmartFactor_valgrind.cpp | 222 +++++ 2 files changed, 1034 insertions(+) create mode 100644 examples/Data/ISAM2_SmartFactor_valgrind.txt create mode 100644 examples/ISAM2_SmartFactor_valgrind.cpp diff --git a/examples/Data/ISAM2_SmartFactor_valgrind.txt b/examples/Data/ISAM2_SmartFactor_valgrind.txt new file mode 100644 index 000000000..feee7bc12 --- /dev/null +++ b/examples/Data/ISAM2_SmartFactor_valgrind.txt @@ -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 diff --git a/examples/ISAM2_SmartFactor_valgrind.cpp b/examples/ISAM2_SmartFactor_valgrind.cpp new file mode 100644 index 000000000..2d5c6c15b --- /dev/null +++ b/examples/ISAM2_SmartFactor_valgrind.cpp @@ -0,0 +1,222 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +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::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 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>(X(1), Pose3::identity(), priorPoseNoise); + initialEstimate.insert(X(0), Pose3::identity()); + + // Bias prior + graph.add(PriorFactor(B(1), imu.prior_imu_bias, imu.bias_noise_model)); + initialEstimate.insert(B(0), imu.prior_imu_bias); + + // Velocity prior + graph.add(PriorFactor(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(X(last_frame)), currentEstimate.at(V(last_frame))); + imu.prev_bias = currentEstimate.at(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; +} From 937cdcf4d98c14ab45de3a879c0ad351b8fbb315 Mon Sep 17 00:00:00 2001 From: Nghia Ho Date: Fri, 14 Dec 2018 12:12:33 -0800 Subject: [PATCH 3/6] shorten parameter values --- ...nd.txt => ISAM2_SmartFactorStereo_IMU.txt} | 0 examples/ISAM2_SmartFactorStereo_IMU.cpp | 236 ++++++++++++++++++ examples/ISAM2_SmartFactor_valgrind.cpp | 222 ---------------- 3 files changed, 236 insertions(+), 222 deletions(-) rename examples/Data/{ISAM2_SmartFactor_valgrind.txt => ISAM2_SmartFactorStereo_IMU.txt} (100%) create mode 100644 examples/ISAM2_SmartFactorStereo_IMU.cpp delete mode 100644 examples/ISAM2_SmartFactor_valgrind.cpp diff --git a/examples/Data/ISAM2_SmartFactor_valgrind.txt b/examples/Data/ISAM2_SmartFactorStereo_IMU.txt similarity index 100% rename from examples/Data/ISAM2_SmartFactor_valgrind.txt rename to examples/Data/ISAM2_SmartFactorStereo_IMU.txt diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp new file mode 100644 index 000000000..25675abf8 --- /dev/null +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -0,0 +1,236 @@ +/** + * @file ISAM2_SmartFactorStereo_IMU.cpp + * @brief test of iSAM2 with smart stereo factors and IMU preintegration, + * originally used to debug valgrind invalid reads with Eigen + * @author Nghia Ho + * + * Setup is a stationary stereo camera with an IMU attached. + * The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt + * It contains 5 frames of stereo matches and IMU data. + */ +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +struct IMUHelper { + IMUHelper() { + { + auto gaussian = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3)) + .finished()); + + auto huber = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), gaussian); + + biasNoiseModel = huber; + } + + { + auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01); + auto huber = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), gaussian); + + velocityNoiseModel = huber; + } + + // expect IMU to be rotated in image space co-ords + auto p = boost::make_shared( + Vector3(0.0, 9.8, 0.0)); + + p->accelerometerCovariance = + I_3x3 * pow(0.0565, 2.0); // acc white noise in continuous + p->integrationCovariance = + I_3x3 * 1e-9; // integration uncertainty continuous + p->gyroscopeCovariance = + I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuous + p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous + p->biasOmegaCovariance = + I_3x3 * pow(0.001, 2.0); // gyro bias in continuous + p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5; + + // body to IMU rotation + Rot3 iRb( + 0.036129, -0.998727, 0.035207, + 0.045417, -0.033553, -0.99840,4 + 0.998315, 0.037670, 0.044147); + + // body to IMU translation (meters) + Point3 iTb(0.03, -0.025, -0.06); + + // body in this example is the left camera + p->body_P_sensor = Pose3(iRb, iTb); + + Rot3 prior_rotation = Rot3(I_3x3); + Pose3 prior_pose(prior_rotation, Point3(0, 0, 0)); + + Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame + Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968); + + priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias); + + prevState = NavState(prior_pose, Vector3(0, 0, 0)); + propState = prevState; + prevBias = priorImuBias; + + preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias); + } + + imuBias::ConstantBias priorImuBias; // assume zero initial bias + noiseModel::Robust::shared_ptr velocityNoiseModel; + noiseModel::Robust::shared_ptr biasNoiseModel; + NavState prevState; + NavState propState; + imuBias::ConstantBias prevBias; + PreintegratedCombinedMeasurements* preintegrated; +}; + +int main(int argc, char* argv[]) { + if (argc != 2) { + cout << "./main [data.txt]\n"; + return 0; + } + + ifstream in(argv[1]); + + if (!in) { + cerr << "error opening: " << argv[1] << "\n"; + return 1; + } + + // Camera parameters + double fx = 822.37; + double fy = 822.37; + double cx = 538.73; + double cy = 579.10; + double baseline = 0.372; // meters + + 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 smartFactors; + NonlinearFactorGraph graph; + Values initialEstimate; + IMUHelper imu; + + // Pose prior - at identity + auto priorPoseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); + graph.emplace_shared>(X(1), Pose3::identity(), + priorPoseNoise); + initialEstimate.insert(X(0), Pose3::identity()); + + // Bias prior + graph.add(PriorFactor(B(1), imu.priorImuBias, + imu.biasNoiseModel)); + initialEstimate.insert(B(0), imu.priorImuBias); + + // Velocity prior - assume stationary + graph.add( + PriorFactor(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel)); + initialEstimate.insert(V(0), Vector3(0, 0, 0)); + + int lastFrame = 1; + int frame; + + while (true) { + char line[1024]; + + in.getline(line, sizeof(line)); + stringstream ss(line); + char type; + + ss >> type; + ss >> frame; + + if (frame != lastFrame || in.eof()) { + cout << "Running iSAM for frame: " << lastFrame << "\n"; + + initialEstimate.insert(X(lastFrame), Pose3::identity()); + initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0)); + initialEstimate.insert(B(lastFrame), imu.prevBias); + + CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1), + X(lastFrame), V(lastFrame), B(lastFrame - 1), + B(lastFrame), *imu.preintegrated); + + graph.add(imuFactor); + + isam.update(graph, initialEstimate); + + Values currentEstimate = isam.calculateEstimate(); + + imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias); + imu.prevState = NavState(currentEstimate.at(X(lastFrame)), + currentEstimate.at(V(lastFrame))); + imu.prevBias = currentEstimate.at(B(lastFrame)); + imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias); + + graph.resize(0); + initialEstimate.clear(); + + if (in.eof()) { + break; + } + } + + if (type == 'i') { // Process IMU measurement + double ax, ay, az; + double gx, gy, gz; + double dt = 1 / 800.0; // IMU at ~800Hz + + 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') { // Process stereo measurement + 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); + } else { + throw runtime_error("unexpected data type: " + type); + } + + lastFrame = frame; + } + + return 0; +} diff --git a/examples/ISAM2_SmartFactor_valgrind.cpp b/examples/ISAM2_SmartFactor_valgrind.cpp deleted file mode 100644 index 2d5c6c15b..000000000 --- a/examples/ISAM2_SmartFactor_valgrind.cpp +++ /dev/null @@ -1,222 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include - -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::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 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>(X(1), Pose3::identity(), priorPoseNoise); - initialEstimate.insert(X(0), Pose3::identity()); - - // Bias prior - graph.add(PriorFactor(B(1), imu.prior_imu_bias, imu.bias_noise_model)); - initialEstimate.insert(B(0), imu.prior_imu_bias); - - // Velocity prior - graph.add(PriorFactor(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(X(last_frame)), currentEstimate.at(V(last_frame))); - imu.prev_bias = currentEstimate.at(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; -} From 8b8947b95c5f3721069036fc831f13f1eddb13e0 Mon Sep 17 00:00:00 2001 From: Nghia Ho Date: Fri, 14 Dec 2018 12:15:14 -0800 Subject: [PATCH 4/6] fixed typo --- examples/ISAM2_SmartFactorStereo_IMU.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp index 25675abf8..43be2bd4d 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -29,7 +29,6 @@ struct IMUHelper { auto gaussian = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3)) .finished()); - auto huber = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), gaussian); @@ -62,7 +61,7 @@ struct IMUHelper { // body to IMU rotation Rot3 iRb( 0.036129, -0.998727, 0.035207, - 0.045417, -0.033553, -0.99840,4 + 0.045417, -0.033553, -0.998404, 0.998315, 0.037670, 0.044147); // body to IMU translation (meters) From 4da1b7189d78708d62fae91de46b674eb002da3b Mon Sep 17 00:00:00 2001 From: Nghia Ho Date: Fri, 14 Dec 2018 12:16:19 -0800 Subject: [PATCH 5/6] change help output text --- examples/ISAM2_SmartFactorStereo_IMU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp index 43be2bd4d..e376cfcff 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -96,7 +96,7 @@ struct IMUHelper { int main(int argc, char* argv[]) { if (argc != 2) { - cout << "./main [data.txt]\n"; + cout << "./ISAM2_SmartFactorStereo_IMU [data.txt]\n"; return 0; } From e8da58725f6767e4c41656e6521c6a39f633ab0d Mon Sep 17 00:00:00 2001 From: Nghia Ho Date: Fri, 14 Dec 2018 12:18:43 -0800 Subject: [PATCH 6/6] formatting --- examples/ISAM2_SmartFactorStereo_IMU.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp index e376cfcff..968f1edc7 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -59,10 +59,9 @@ struct IMUHelper { p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5; // body to IMU rotation - Rot3 iRb( - 0.036129, -0.998727, 0.035207, - 0.045417, -0.033553, -0.998404, - 0.998315, 0.037670, 0.044147); + Rot3 iRb(0.036129, -0.998727, 0.035207, + 0.045417, -0.033553, -0.998404, + 0.998315, 0.037670, 0.044147); // body to IMU translation (meters) Point3 iTb(0.03, -0.025, -0.06); @@ -112,7 +111,7 @@ int main(int argc, char* argv[]) { double fy = 822.37; double cx = 538.73; double cy = 579.10; - double baseline = 0.372; // meters + double baseline = 0.372; // meters Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));