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

Merge branch 'develop'

release/4.3a0
Luca 2014-08-19 20:51:16 -04:00
parent f5cc247b1c bc205cf6a4
commit fdb31dbd8d
33 changed files with 144102 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/Data/VO_calibration00.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

1
examples/Data/VO_calibration00s.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

135
examples/Data/VO_camera_poses00.txt Normal file
View File

@ -0,0 +1,135 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1
77 0.994568 0.0201446 -0.102122 -4.98771 -0.0190681 0.999752 0.0115061 -0.794955 0.102328 -0.00949629 0.994705 69.5653 0 0 0 1
78 0.99437 0.0229894 -0.10344 -5.07707 -0.0223435 0.999723 0.00739861 -0.804841 0.103581 -0.00504575 0.994608 70.3178 0 0 0 1
79 0.99423 0.0228747 -0.1048 -5.16223 -0.0229899 0.999736 0.000108822 -0.81531 0.104774 0.00230114 0.994493 71.0475 0 0 0 1
80 0.994077 0.0219938 -0.106431 -5.24094 -0.0227304 0.999725 -0.00571252 -0.825441 0.106276 0.00809789 0.994304 71.7584 0 0 0 1
81 0.994023 0.0228054 -0.106762 -5.32437 -0.0236929 0.999694 -0.00705161 -0.831667 0.106569 0.00953897 0.99426 72.4548 0 0 0 1
82 0.99386 0.0255543 -0.107653 -5.40648 -0.0260808 0.999654 -0.00348505 -0.846106 0.107527 0.00627133 0.994182 73.1337 0 0 0 1
83 0.993702 0.0257681 -0.109048 -5.48436 -0.02605 0.99966 -0.00116096 -0.865059 0.108981 0.00399435 0.994036 73.7942 0 0 0 1
84 0.99367 0.0225468 -0.110051 -5.5561 -0.0231761 0.999722 -0.00444219 -0.879535 0.10992 0.00696462 0.993916 74.4324 0 0 0 1
85 0.993802 0.0143509 -0.110234 -5.61528 -0.0155198 0.999832 -0.00975281 -0.89282 0.110075 0.0114032 0.993858 75.0504 0 0 0 1
86 0.993949 0.0102 -0.10937 -5.67796 -0.0118817 0.999821 -0.0147354 -0.904058 0.1092 0.0159457 0.993892 75.6535 0 0 0 1
87 0.994244 0.0126451 -0.106395 -5.74524 -0.014328 0.999784 -0.0150673 -0.916949 0.106181 0.016505 0.99421 76.2455 0 0 0 1
88 0.994592 0.0175824 -0.102356 -5.81375 -0.0189231 0.999747 -0.0121417 -0.930972 0.102117 0.0140129 0.994674 76.8222 0 0 0 1
89 0.995077 0.0159295 -0.0978149 -5.86699 -0.0169123 0.999814 -0.00922648 -0.942909 0.0976498 0.0108353 0.995162 77.3862 0 0 0 1
90 0.995665 0.0122046 -0.0922106 -5.90659 -0.0127358 0.999906 -0.00517412 -0.954487 0.0921387 0.00632606 0.995726 77.9355 0 0 0 1
91 0.996426 0.00781104 -0.084105 -5.94257 -0.00798227 0.999967 -0.0016998 -0.970792 0.0840889 0.00236507 0.996455 78.4692 0 0 0 1
92 0.997233 0.0114593 -0.0734453 -5.98049 -0.0116369 0.99993 -0.00198965 -0.981019 0.0734174 0.00283882 0.997297 78.9883 0 0 0 1
93 0.998165 0.0165636 -0.0582361 -6.0154 -0.0167456 0.999856 -0.0026387 -0.99003 0.058184 0.00360906 0.998299 79.4952 0 0 0 1
95 0.999635 0.0200255 -0.0181151 -6.02981 -0.0200623 0.999797 -0.00185529 -1.00502 0.0180742 0.00221804 0.999834 80.4727 0 0 0 1
97 0.999162 0.015548 0.037857 -5.96801 -0.0155684 0.999879 0.000243918 -1.02389 -0.0378486 -0.000833085 0.999283 81.4025 0 0 0 1
99 0.993959 0.0151454 0.108698 -5.84553 -0.0154328 0.999879 0.0018028 -1.04109 -0.108657 -0.00346942 0.994073 82.2952 0 0 0 1
101 0.980499 0.0151504 0.195937 -5.64466 -0.0157106 0.999876 0.00130478 -1.05761 -0.195893 -0.00435763 0.980616 83.1489 0 0 0 1
103 0.954186 0.0182833 0.298656 -5.36588 -0.0179595 0.999831 -0.00382887 -1.08348 -0.298675 -0.00171027 0.954353 83.9397 0 0 0 1
105 0.910736 0.0194893 0.412529 -4.99648 -0.0175815 0.99981 -0.00842014 -1.10057 -0.412615 0.000415655 0.910905 84.6633 0 0 0 1
107 0.848724 0.0183908 0.528517 -4.54701 -0.0135972 0.999824 -0.0129557 -1.12003 -0.528662 0.00380946 0.848824 85.2983 0 0 0 1
109 0.772259 0.0170098 0.63508 -4.0183 -0.0106749 0.999848 -0.0137989 -1.14244 -0.635218 0.00387688 0.772323 85.8601 0 0 0 1
111 0.684256 0.0156411 0.729074 -3.42903 -0.0102231 0.999877 -0.0118561 -1.16079 -0.72917 0.000659179 0.684332 86.3474 0 0 0 1
113 0.590745 0.011826 0.806772 -2.77931 -0.000173089 0.999894 -0.0145301 -1.17886 -0.806858 0.00844396 0.590684 86.7443 0 0 0 1
115 0.496173 0.0169181 0.868059 -2.10955 -0.00504039 0.999849 -0.0166057 -1.20339 -0.868209 0.00386392 0.496183 87.0847 0 0 0 1
117 0.408192 0.0165355 0.912746 -1.40862 0.00231553 0.999814 -0.0191484 -1.2249 -0.912893 0.00992974 0.408078 87.3396 0 0 0 1
119 0.333443 0.00543386 0.942754 -0.671521 0.0223493 0.999657 -0.0136666 -1.23662 -0.942505 0.025627 0.333208 87.522 0 0 0 1
121 0.269054 0.0173163 0.962969 0.0638526 0.00961829 0.99974 -0.0206648 -1.26307 -0.963077 0.0148221 0.268818 87.7199 0 0 0 1
123 0.214897 0.0233915 0.976357 0.843046 0.00677025 0.999653 -0.0254398 -1.30009 -0.976613 0.0120771 0.214664 87.8763 0 0 0 1
125 0.171479 0.031054 0.984698 1.66216 0.0212619 0.999154 -0.0352125 -1.3179 -0.984958 0.0269747 0.170674 87.9743 0 0 0 1
127 0.134011 0.0386308 0.990227 2.52547 0.0207141 0.998912 -0.041773 -1.34147 -0.990763 0.0261097 0.133065 88.0809 0 0 0 1
129 0.10418 0.0310179 0.994075 3.44652 0.0195614 0.999256 -0.0332297 -1.39013 -0.994366 0.0229074 0.103496 88.1692 0 0 0 1
131 0.0794366 0.027788 0.996453 4.42556 0.0261822 0.999208 -0.0299521 -1.42776 -0.996496 0.0284686 0.0786462 88.2295 0 0 0 1
132 0.0693462 0.028443 0.997187 4.93885 0.0294969 0.999098 -0.0305488 -1.44582 -0.997156 0.0315324 0.0684447 88.2553 0 0 0 1
133 0.0615414 0.0290168 0.997683 5.46907 0.0316982 0.999016 -0.0310108 -1.46406 -0.997601 0.0335332 0.0605611 88.2814 0 0 0 1
134 0.0559347 0.029371 0.998002 6.0151 0.0334765 0.99895 -0.0312751 -1.48373 -0.997873 0.035159 0.0548927 88.307 0 0 0 1
135 0.0504312 0.0304374 0.998264 6.58025 0.0349281 0.99887 -0.0322204 -1.50267 -0.998117 0.0364923 0.0493112 88.3306 0 0 0 1
136 0.0445067 0.0311103 0.998525 7.16082 0.0355578 0.998832 -0.0327048 -1.52353 -0.998376 0.0369609 0.0433485 88.3531 0 0 0 1
137 0.040243 0.0311989 0.998703 7.76375 0.0381603 0.998735 -0.0327376 -1.54487 -0.998461 0.0394283 0.0390016 88.3716 0 0 0 1
138 0.0373982 0.0312027 0.998813 8.38568 0.0397152 0.998676 -0.0326855 -1.56772 -0.998511 0.0408905 0.0361095 88.3901 0 0 0 1
139 0.0343726 0.0307634 0.998936 9.02449 0.0406913 0.998654 -0.0321549 -1.59059 -0.99858 0.0417533 0.0330745 88.4092 0 0 0 1
140 0.0320861 0.0302694 0.999027 9.68038 0.0427798 0.998584 -0.03163 -1.61442 -0.998569 0.043753 0.0307457 88.4263 0 0 0 1
141 0.0316452 0.0299561 0.99905 10.3542 0.0473602 0.998383 -0.0314363 -1.63856 -0.998376 0.04831 0.0301753 88.4381 0 0 0 1
142 0.0327723 0.029714 0.999021 11.0457 0.0507142 0.998221 -0.0313539 -1.66282 -0.998175 0.0516921 0.031207 88.4556 0 0 0 1
143 0.0353027 0.0297602 0.998933 11.7546 0.0522842 0.998133 -0.0315841 -1.68678 -0.998008 0.0533435 0.0336808 88.4781 0 0 0 1
144 0.0392372 0.0297502 0.998787 12.4771 0.0547241 0.997993 -0.0318763 -1.71289 -0.99773 0.0559084 0.0375304 88.5062 0 0 0 1
145 0.0437096 0.0293188 0.998614 13.219 0.0550685 0.997979 -0.0317105 -1.73922 -0.997525 0.0563782 0.0420067 88.5387 0 0 0 1
146 0.0477725 0.0278103 0.998471 13.9764 0.0564652 0.997939 -0.0304971 -1.76499 -0.997261 0.0578358 0.0461037 88.5751 0 0 0 1
147 0.0518486 0.0263145 0.998308 14.7472 0.0562418 0.997989 -0.0292271 -1.79222 -0.99707 0.057662 0.0502644 88.6178 0 0 0 1
148 0.0560658 0.0242863 0.998132 15.5313 0.0531494 0.998214 -0.0272738 -1.82056 -0.997011 0.0545792 0.0546748 88.6693 0 0 0 1
149 0.0600218 0.0233355 0.997924 16.3271 0.0522059 0.998285 -0.0264839 -1.84733 -0.996831 0.0536871 0.0587006 88.7243 0 0 0 1
150 0.0641513 0.0243795 0.997642 17.1258 0.0492204 0.998408 -0.0275632 -1.87761 -0.996726 0.0508726 0.0628492 88.7821 0 0 0 1
151 0.0672583 0.028483 0.997329 17.929 0.0470717 0.998389 -0.0316877 -1.91204 -0.996625 0.0490772 0.0658092 88.842 0 0 0 1
152 0.0688453 0.0337446 0.997056 18.7357 0.0413971 0.99847 -0.0366509 -1.9468 -0.996768 0.0437985 0.067343 88.9041 0 0 0 1
153 0.0686545 0.0370247 0.996953 19.5482 0.0387033 0.99846 -0.0397459 -1.98038 -0.996889 0.0413142 0.0671158 88.9665 0 0 0 1

77
examples/Data/VO_camera_poses00s.txt Normal file
View File

@ -0,0 +1,77 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1

88781
examples/Data/VO_stereo_factors00.txt Normal file
View File

File diff suppressed because it is too large Load Diff

52544
examples/Data/VO_stereo_factors00s.txt Normal file
View File

File diff suppressed because it is too large Load Diff

3
examples/Data/pose3example-offdiagonal-rewritten.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000

3
examples/Data/pose3example-offdiagonal.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.000000

11
examples/Data/pose3example-rewritten.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446
VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592076 0.30338 -0.513225 0.542222 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000

11
examples/Data/pose3example.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.125250 -0.534379 0.769122 0.327419 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000

4
gtsam/base/types.h
View File

@ -299,6 +299,8 @@ namespace gtsam {
// Define some common g++ functions and macros we use that MSVC does not have
#if (_MSC_VER < 1800)
#include <boost/math/special_functions/fpclassify.hpp>
namespace std {
template<typename T> inline int isfinite(T a) {
@ -309,6 +311,8 @@ namespace std {
return (int)boost::math::isinf(a); }
}
#endif
#include <boost/math/constants/constants.hpp>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())

4
gtsam/geometry/tests/testUnit3.cpp
View File

@ -328,10 +328,6 @@ TEST(Unit3, localCoordinates_retract_expmap) {
//*******************************************************************************
TEST(Unit3, Random) {
boost::mt19937 rng(42);
// Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713);
Point3 actual = Unit3::Random(rng).point3();
EXPECT(assert_equal(expected,actual,1e-5));
// Check that means are all zero at least
Point3 expectedMean, actualMean;
for (size_t i = 0; i < 100; i++)

7
gtsam/linear/HessianFactor.cpp
View File

@ -635,10 +635,11 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
// Do dense elimination
GaussianConditional::shared_ptr conditional;
try {
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(keys.size());
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), keys.size(), Ab);
size_t numberOfKeysToEliminate = keys.size();
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate);
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), numberOfKeysToEliminate, Ab);
// Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size());
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate);
} catch(CholeskyFailed&) {
throw IndeterminantLinearSystemException(keys.front());
}

2
gtsam/linear/NoiseModel.h
View File

@ -19,6 +19,8 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp>
#include <gtsam/base/Matrix.h>

135
gtsam/slam/dataset.cpp
View File

@ -85,7 +85,6 @@ string createRewrittenFileName(const string& name) {
return newpath.string();
}
/* ************************************************************************* */
#endif
/* ************************************************************************* */
@ -250,7 +249,6 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph->push_back(factor);
}
// Parse measurements
double bearing, range, bearing_std, range_std;
@ -358,12 +356,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
}
/* ************************************************************************* */
GraphAndValues readG2o(const string& g2oFile,
GraphAndValues readG2o(const string& g2oFile, const bool is3D,
KernelFunctionType kernelFunctionType) {
// just call load2D
int maxID = 0;
bool addNoise = false;
bool smart = true;
if(is3D)
return load3D(g2oFile);
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
NoiseFormatG2O, kernelFunctionType);
}
@ -374,42 +376,89 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
fstream stream(filename.c_str(), fstream::out);
// save poses
// save 2D & 3D poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl;
const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value);
if(pose2D){
stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " "
<< pose2D->y() << " " << pose2D->theta() << endl;
}
const Pose3* pose3D = dynamic_cast<const Pose3*>(&key_value.value);
if(pose3D){
Point3 p = pose3D->translation();
Rot3 R = pose3D->rotation();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z()
<< " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
<< " " << R.toQuaternion().w() << endl;
}
}
// save edges
// save edges (2D or 3D)
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
continue;
if (factor){
SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
if (!gaussianModel){
model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!");
}
Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta();
for (int i = 0; i < 3; i++){
for (int j = i; j < 3; j++){
stream << " " << Info(i, j);
}
}
stream << endl;
}
SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw invalid_argument(
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(1) << " " << 0.0 << " "
<< diagonalModel->precision(2) << endl;
if (factor3D){
SharedNoiseModel model = factor3D->get_noiseModel();
boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
if (!gaussianModel){
model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!");
}
Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose3 pose3D = factor3D->measured();
Point3 p = pose3D.translation();
Rot3 R = pose3D.rotation();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
stream << " " << Info(i, j);
}
}
stream << endl;
}
}
stream.close();
}
/* ************************************************************************* */
bool load3D(const string& filename) {
GraphAndValues load3D(const string& filename) {
ifstream is(filename.c_str());
if (!is)
return false;
throw invalid_argument("load2D: can not find file " + filename);
Values::shared_ptr initial(new Values);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
while (is) {
char buf[LINESIZE];
@ -422,6 +471,17 @@ bool load3D(const string& filename) {
int id;
double x, y, z, roll, pitch, yaw;
ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
}
if (tag == "VERTEX_SE3:QUAT") {
int id;
double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
}
}
is.clear(); /* clears the end-of-file and error flags */
@ -438,13 +498,38 @@ bool load3D(const string& filename) {
int id1, id2;
double x, y, z, roll, pitch, yaw;
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
Matrix m = eye(6);
for (int i = 0; i < 6; i++)
for (int j = i; j < 6; j++)
ls >> m(i, j);
SharedNoiseModel model = noiseModel::Gaussian::Information(m);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
}
if (tag == "EDGE_SE3:QUAT") {
Matrix m = eye(6);
int id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
double mij;
ls >> mij;
m(i, j) = mij;
m(j, i) = mij;
}
}
SharedNoiseModel model = noiseModel::Gaussian::Information(m);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
}
}
return true;
return make_pair(graph, initial);
}
/* ************************************************************************* */

7
gtsam/slam/dataset.h
View File

@ -110,11 +110,12 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
/**
* @brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure
* @param filename The name of the g2o file
* @param filename The name of the g2o file\
* @param is3D indicates if the file describes a 2D or 3D problem
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile,
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/**
@ -130,7 +131,7 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
/**
* Load TORO 3D Graph
*/
GTSAM_EXPORT bool load3D(const std::string& filename);
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t, Point2> SfM_Measurement;

146
gtsam/slam/tests/testDataset.cpp
View File

@ -116,13 +116,116 @@ TEST( dataSet, readG2o)
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2o3D)
{
const string g2oFile = findExampleDataFile("pose3example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
expectedValues.insert(2, Pose3(R2, p2));
Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323);
Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
expectedValues.insert(3, Pose3(R3, p3));
Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933);
Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
expectedValues.insert(4, Pose3(R4, p4));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0));
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
expectedGraph.add(BetweenFactor<Pose3>(1, 2, Pose3(R12,p12), model));
Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
expectedGraph.add(BetweenFactor<Pose3>(2, 3, Pose3(R23,p23), model));
Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
expectedGraph.add(BetweenFactor<Pose3>(3, 4, Pose3(R34,p34), model));
Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
expectedGraph.add(BetweenFactor<Pose3>(1, 4, Pose3(R14,p14), model));
Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
expectedGraph.add(BetweenFactor<Pose3>(3, 0, Pose3(R30,p30), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2o3DNonDiagonalNoise)
{
const string g2oFile = findExampleDataFile("pose3example-offdiagonal.txt");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
Matrix Info = Matrix(6,6);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
if(i==j)
Info(i, j) = 10000;
else{
Info(i, j) = i+1; // arbitrary nonzero number
Info(j, i) = i+1;
}
}
}
noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse());
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
}
/* ************************************************************************* */
TEST( dataSet, readG2oHuber)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER);
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
@ -149,7 +252,8 @@ TEST( dataSet, readG2oTukey)
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY);
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
@ -188,6 +292,44 @@ TEST( dataSet, writeG2o)
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o3D)
{
const string g2oFile = findExampleDataFile("pose3example");
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
bool is3D = true;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o3DNonDiagonalNoise)
{
const string g2oFile = findExampleDataFile("pose3example-offdiagonal");
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
bool is3D = true;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
}
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{

155
gtsam_unstable/examples/ConcurrentCalibration.cpp Normal file
View File

@ -0,0 +1,155 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ConcurrentCalibration.cpp
* @brief First step towards estimating monocular calibration in concurrent
* filter/smoother framework. To start with, just batch LM.
* @date June 11, 2014
* @author Chris Beall
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <string>
#include <fstream>
#include <iostream>
#include <boost/lexical_cast.hpp>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
string calibration_loc = findExampleDataFile("VO_calibration00s.txt");
string pose_loc = findExampleDataFile("VO_camera_poses00s.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors00s.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0));
const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10));
initial_estimate.insert(Symbol('K', 0), *noisy_K);
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100));
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), *noisy_K, calNoise));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.push_back(PriorFactor<Pose3>(Symbol('x', pose_id), Pose3(m), poseNoise));
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
// graph.push_back( GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K));
graph.push_back(GeneralSFMFactor2<Cal3_S2>(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
cout << "Optimizing" << endl;
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate,params);
// Values result = optimizer.optimize();
string K_values_file = "K_values.txt";
ofstream stream_K(K_values_file.c_str());
double currentError;
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
// Iterative loop
do {
// Do next iteration
currentError = optimizer.error();
optimizer.iterate();
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl;
} while(optimizer.iterations() < params.maxIterations &&
!checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
params.errorTol, currentError, optimizer.error(), params.verbosity));
Values result = optimizer.values();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
Values(result.filter<Cal3_S2>()).print("Final K\n");
noisy_K->print("Initial noisy K\n");
K->print("Initial correct K\n");
return 0;
}

42
gtsam_unstable/gtsam_unstable.h
View File

@ -24,6 +24,7 @@ virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor;
virtual class gtsam::Cal3_S2;
virtual class gtsam::Cal3DS2;
class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph;
class gtsam::Ordering;
@ -745,4 +746,45 @@ virtual class OdometryFactorBase : gtsam::NoiseModelFactor {
void print(string s) const;
};
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor {
ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k);
ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
gtsam::Point2 measured() const;
CALIBRATION* calibration() const;
bool verboseCheirality() const;
bool throwCheirality() const;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCal3_S2;
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCal3DS2;
#include <gtsam_unstable/slam/ProjectionFactorPPPC.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor {
ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey);
ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey, bool throwCheirality, bool verboseCheirality);
gtsam::Point2 measured() const;
bool verboseCheirality() const;
bool throwCheirality() const;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCCal3_S2;
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;
} //\namespace gtsam

181
gtsam_unstable/slam/ProjectionFactorPPP.h Normal file
View File

@ -0,0 +1,181 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ProjectionFactorPPP.h
* @brief Derived from ProjectionFactor, but estimates body-camera transform
* in addition to body pose and 3D landmark
* @author Chris Beall
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
/// shorthand for this class
typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param transformKey is the index of the body-camera transform
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
*/
ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey,
const boost::shared_ptr<CALIBRATION>& K) :
Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey,
const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality) :
Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~ProjectionFactorPPP() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPP, z = ";
measured_.print();
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
try {
if(H1 || H2 || H3) {
gtsam::Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,6);
if (H3) *H3 = zeros(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
}
/** return the measurement */
const Point2& measured() const {
return measured_;
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

171
gtsam_unstable/slam/ProjectionFactorPPPC.h Normal file
View File

@ -0,0 +1,171 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ProjectionFactorPPPC.h
* @brief Derived from ProjectionFactor, but estimates body-camera transform
* and calibration in addition to body pose and 3D landmark
* @author Chris Beall
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. This factor
* estimates the body pose, body-camera transform, 3D landmark, and calibration.
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPPC: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
protected:
Point2 measured_; ///< 2D measurement
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
/// shorthand for this class
typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey,
bool throwCheirality, bool verboseCheirality) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~ProjectionFactorPPPC() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPPC, z = ";
measured_.print();
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
try {
if(H1 || H2 || H3 || H4) {
gtsam::Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,6);
if (H3) *H3 = zeros(2,3);
if (H4) *H4 = zeros(2,CALIBRATION::Dim());
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K.fx();
}
/** return the measurement */
const Point2& measured() const {
return measured_;
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

221
gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp Normal file
View File

@ -0,0 +1,221 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testProjectionFactor.cpp
* @brief Unit tests for ProjectionFactorPPP Class
* @author Chris Beall
* @date July 2014
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w=640,h=480;
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(2));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
using symbol_shorthand::T;
typedef ProjectionFactorPPP<Pose3, Point3> TestProjectionFactor;
/* ************************************************************************* */
TEST( ProjectionFactor, nonStandard ) {
ProjectionFactorPPP<Pose3, Point3, Cal3DS2> f;
}
/* ************************************************************************* */
TEST( ProjectionFactor, Constructor) {
Key poseKey(X(1));
Key transformKey(T(1));
Key pointKey(L(1));
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
}
/* ************************************************************************* */
TEST( ProjectionFactor, ConstructorWithTransform) {
Key poseKey(X(1));
Key transformKey(T(1));
Key pointKey(L(1));
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
}
/* ************************************************************************* */
TEST( ProjectionFactor, Equals ) {
// Create two identical factors and make sure they're equal
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
TEST( ProjectionFactor, EqualsWithTransform ) {
// Create two identical factors and make sure they're equal
Point2 measurement(323.0, 240.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
TEST( ProjectionFactor, Error ) {
// Create the factor with a measurement that is 3 pixels off in x
Key poseKey(X(1));
Key transformKey(T(1));
Key pointKey(L(1));
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
// Set the linearization point
Pose3 pose(Rot3(), Point3(0,0,-6));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, Pose3(), point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( ProjectionFactor, ErrorWithTransform ) {
// Create the factor with a measurement that is 3 pixels off in x
Key poseKey(X(1));
Key transformKey(T(1));
Key pointKey(L(1));
Point2 measurement(323.0, 240.0);
Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K);
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, transform, point));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( ProjectionFactor, Jacobian ) {
// Create the factor with a measurement that is 3 pixels off in x
Key poseKey(X(1));
Key transformKey(T(1));
Key pointKey(L(1));
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
// Set the linearization point
Pose3 pose(Rot3(), Point3(0,0,-6));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual);
// The expected Jacobians
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, Pose3(), point);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
}
/* ************************************************************************* */
TEST( ProjectionFactor, JacobianWithTransform ) {
// Create the factor with a measurement that is 3 pixels off in x
Key poseKey(X(1));
Key transformKey(T(1));
Key pointKey(L(1));
Point2 measurement(323.0, 240.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual;
factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual);
// The expected Jacobians
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

191
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp Normal file
View File

@ -0,0 +1,191 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testProjectionFactorPPPC.cpp
* @brief Unit tests for Pose+Transform+Calibration ProjectionFactor Class
* @author Chris Beall
* @date Jul 29, 2014
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam_unstable/slam/ProjectionFactorPPPC.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w=640,h=480;
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov,w,h));
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(2));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
using symbol_shorthand::T;
using symbol_shorthand::K;
typedef ProjectionFactorPPPC<Pose3, Point3, Cal3_S2> TestProjectionFactor;
/* ************************************************************************* */
TEST( ProjectionFactor, nonStandard ) {
ProjectionFactorPPPC<Pose3, Point3, Cal3DS2> f;
}
/* ************************************************************************* */
TEST( ProjectionFactor, Constructor) {
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
// TODO: Actually check something
}
/* ************************************************************************* */
TEST( ProjectionFactor, Equals ) {
// Create two identical factors and make sure they're equal
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K(1));
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K(1));
CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
TEST( ProjectionFactor, Error ) {
// Create the factor with a measurement that is 3 pixels off in x
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
// Set the linearization point
Pose3 pose(Rot3(), Point3(0,0,-6));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, Pose3(), point, *K1));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( ProjectionFactor, ErrorWithTransform ) {
// Create the factor with a measurement that is 3 pixels off in x
Point2 measurement(323.0, 240.0);
Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestProjectionFactor factor(measurement, model, X(1),T(1), L(1), K(1));
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose, transform, point, *K1));
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
Vector expectedError = (Vector(2) << -3.0, 0.0);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( ProjectionFactor, Jacobian ) {
// Create the factor with a measurement that is 3 pixels off in x
Point2 measurement(323.0, 240.0);
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
// Set the linearization point
Pose3 pose(Rot3(), Point3(0,0,-6));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual, H4Actual;
factor.evaluateError(pose, Pose3(), point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);
// The expected Jacobians
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), Pose3());
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
_1, boost::none, boost::none, boost::none, boost::none), *K1);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
}
/* ************************************************************************* */
TEST( ProjectionFactor, JacobianWithTransform ) {
// Create the factor with a measurement that is 3 pixels off in x
Point2 measurement(323.0, 240.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual, H4Actual;
factor.evaluateError(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);
// The expected Jacobians
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
_1, boost::none, boost::none, boost::none, boost::none), *K1);
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

304
matlab/unstable_examples/FlightCameraTransformIMU.m Normal file
View File

@ -0,0 +1,304 @@
clear all;
clf;
import gtsam.*;
write_video = true;
use_camera = true;
use_camera_transform_noise = true;
gps_noise = 0.5; % normally distributed (meters)
landmark_noise = 0.2; % normally distributed (meters)
nrLandmarks = 1000; % Number of randomly generated landmarks
% ground-truth IMU-camera transform
camera_transform = Pose3(Rot3.RzRyRx(-pi, 0, -pi/2),Point3());
% noise to compose onto the above for initialization
camera_transform_noise = Pose3(Rot3.RzRyRx(0.1,0.1,0.1),Point3(0,0.02,0));
if(write_video)
videoObj = VideoWriter('FlightCameraIMU_transform_GPS0_5_lm0_2_robust.avi');
videoObj.Quality = 100;
videoObj.FrameRate = 10;
open(videoObj);
end
% IMU parameters
IMU_metadata.AccelerometerSigma = 1e-2;
IMU_metadata.GyroscopeSigma = 1e-2;
IMU_metadata.AccelerometerBiasSigma = 1e-6;
IMU_metadata.GyroscopeBiasSigma = 1e-6;
IMU_metadata.IntegrationSigma = 1e-1;
transformKey = 1000;
calibrationKey = 2000;
fg = NonlinearFactorGraph;
initial = Values;
%% some noise models
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 0.1]);
GPS_trans_cov = noiseModel.Diagonal.Sigmas([3; 3; 4]);
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
l_cov = noiseModel.Diagonal.Sigmas([landmark_noise; landmark_noise; landmark_noise]);
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
% z_cov = noiseModel.Robust(noiseModel.mEstimator.Huber(1.0), noiseModel.Diagonal.Sigmas([1.0;1.0]));
%% calibration initialization
K = Cal3_S2(20,1280,960);
% initialize K incorrectly
K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py());
isamParams = gtsam.ISAM2Params;
isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!)
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];
%% generate trajectory and landmarks
trajectory = flight_trajectory();
landmarks = ground_landmarks(nrLandmarks);
figure(1);
% 3D map subplot
a1 = subplot(2,2,1);
grid on;
plot3DTrajectory(trajectory,'-b',true,5);
plot3DPoints(landmarks,'*g');
axis([-800 800 -800 800 0 1600]);
axis equal;
hold on;
view(-37,40);
% camera subplot
a2 = subplot(2,2,2);
if ~use_camera
title('Camera Off');
end
% IMU-cam transform subplot
a3 = subplot(2,2,3);
view(-37,40);
axis([-1 1 -1 1 -1 1]);
grid on;
xlabel('x');
ylabel('y');
zlabel('z');
title('Estimated vs. actual IMU-cam transform');
axis equal;
for i=1:size(trajectory)-1
xKey = symbol('x',i);
pose = trajectory.at(xKey); % GT pose
pose_t = pose.translation(); % GT pose-translation
if exist('h_cursor','var')
delete(h_cursor);
end
% current ground-truth position indicator
h_cursor = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'*');
camera_pose = pose.compose(camera_transform);
axes(a2);
if use_camera
% project (and plot 2D camera view inside)
measurements = project_landmarks(camera_pose,landmarks, K);
% plot red landmarks in 3D plot
plot_projected_landmarks(a1, landmarks, measurements);
else
measurements = Values;
end
%% ISAM stuff
currentVelKey = symbol('v',i);
currentBiasKey = symbol('b',i);
initial.insert(currentVelKey, currentVelocityGlobal);
initial.insert(currentBiasKey, currentBias);
% prior on translation, sort of like GPS with noise!
gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]);
fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov));
if i==1
% camera transform
if use_camera_transform_noise
camera_transform_init = camera_transform.compose(camera_transform_noise);
else
camera_transform_init = camera_transform;
end
initial.insert(transformKey,camera_transform_init);
fg.add(PriorFactorPose3(transformKey,camera_transform_init,trans_cov));
% calibration
initial.insert(2000, K_corrupt);
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
initial.insert(xKey, pose);
result = initial;
end
% priors on first two poses
if i < 3
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end
%% the 'normal' case
if i > 1
xKey_prev = symbol('x',i-1);
pose_prev = trajectory.at(xKey_prev);
step = pose_prev.between(pose);
% insert estimate for current pose with some normal noise on
% translation
initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
% visual measurements
if measurements.size > 0 && use_camera
measurementKeys = KeyVector(measurements.keys);
for zz = 0:measurementKeys.size-1
zKey = measurementKeys.at(zz);
lKey = symbol('l',symbolIndex(zKey));
fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ...
z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
% only add landmark to values if doesn't exist yet
if ~result.exists(lKey)
noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
initial.insert(lKey, noisy_landmark);
% and add a prior since its position is known
fg.add(PriorFactorPoint3(lKey, noisy_landmark,l_cov));
end
end
end % end landmark observations
%% IMU
deltaT = 1;
logmap = Pose3.Logmap(step);
omega = logmap(1:3);
velocity = logmap(4:6);
% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
% [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
% currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
accMeas = acc_omega(1:3)-g;
omegaMeas = acc_omega(4:6);
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
%% create IMU factor
fg.add(ImuFactor( ...
xKey_prev, currentVelKey-1, ...
xKey, currentVelKey, ...
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
% Bias evolution as given in the IMU metadata
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b)));
% ISAM update
isam.update(fg, initial);
result = isam.calculateEstimate();
%% reset
initial = Values;
fg = NonlinearFactorGraph;
currentVelocityGlobal = result.at(currentVelKey);
currentBias = result.at(currentBiasKey);
%% plot current pose result
isam_pose = result.at(xKey);
pose_t = isam_pose.translation();
if exist('h_result','var')
delete(h_result);
end
h_result = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'^b', 'MarkerSize', 10);
title(a1, sprintf('Step %d', i));
if exist('h_text1(1)', 'var')
delete(h_text1(1));
% delete(h_text2(1));
end
ty = result.at(transformKey).translation().y();
K_estimate = result.at(calibrationKey);
K_errors = K.localCoordinates(K_estimate);
camera_transform_estimate = result.at(transformKey);
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
% h_text1 = text(-600,0,0,sprintf('Y-Transform(0.0): %0.2f',ty));
text(0,1300,0,sprintf('Calibration and IMU-cam transform errors:'));
entries = [{' f_x', ' f_y', ' s', 'p_x', 'p_y'}; num2cell(K_errors')];
h_text1 = text(0,1750,0,sprintf('%s = %0.1f\n', entries{:}));
camera_transform_errors = camera_transform.localCoordinates(camera_transform_estimate);
entries1 = [{'ax', 'ay', 'az', 'tx', 'ty', 'tz'}; num2cell(camera_transform_errors')];
h_text2 = text(600,1700,0,sprintf('%s = %0.2f\n', entries1{:}));
% marginal is really huge
% marginal_camera_transform = isam.marginalCovariance(transformKey);
% plot transform
axes(a3);
cla;
plotPose3(camera_transform,[],1);
plotPose3(camera_transform_estimate,[],0.5);
end
drawnow;
if(write_video)
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame)
else
pause(0.00001);
end
end
% print out final camera transform
result.at(transformKey);
if(write_video)
close(videoObj);
end

189
matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m Normal file
View File

@ -0,0 +1,189 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
import gtsam.*
write_video = false;
if(write_video)
videoObj = VideoWriter('test.avi');
videoObj.Quality = 100;
videoObj.FrameRate = 2;
open(videoObj);
end
%% generate some landmarks
nrPoints = 8;
landmarks = {Point3([20 15 1]'),...
Point3([22 7 -1]'),...
Point3([20 20 6]'),...
Point3([24 19 -4]'),...
Point3([26 17 -2]'),...
Point3([12 15 4]'),...
Point3([25 11 -6]'),...
Point3([23 10 4]')};
curvature = 5.0;
transformKey = 1000;
calibrationKey = 2000;
fg = NonlinearFactorGraph;
initial = Values;
%% intial landmarks and camera trajectory shifted in + y-direction
y_shift = Point3(0,1,0);
% insert shifted points
for i=1:nrPoints
initial.insert(100+i,landmarks{i}.compose(y_shift));
end
figure(1);
cla
hold on;
%% initial pose priors
pose_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 0.1; 0.1; 0.1]);
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
initial.insert(transformKey,camera_transform);
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 20]);
fg.add(PriorFactorPose3(transformKey,camera_transform,trans_cov));
%% insert poses
initial.insert(1, Pose3());
move_forward = Pose3(Rot3(),Point3(1,0,0));
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
%% calibration initialization
K = Cal3_S2(900,900,0,640,480);
K_corrupt = Cal3_S2(910,890,0,650,470);
initial.insert(2000, K_corrupt);
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
cheirality_exception_count = 0;
isamParams = gtsam.ISAM2Params;
isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
result = initial
for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
% result
for j=1:nrPoints
% All landmarks seen in every frame
try
z = cam.project(landmarks{j});
fg.add(TransformCalProjectionFactorCal3_S2(z, z_cov, i, transformKey, 100+j, calibrationKey));
catch
cheirality_exception_count = cheirality_exception_count + 1;
end % end try/catch
end
if i > 2
disp('ISAM Update');
isam.update(fg, initial);
result = isam.calculateEstimate();
%% reset
initial = Values;
fg = NonlinearFactorGraph;
end
hold off;
clf;
hold on;
%% plot results
result_camera_transform = result.at(transformKey);
for j=1:i
gtsam.plotPose3(result.at(j),[],0.5);
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
ylabel('y (m)');
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
axis([0 20 0 20 -10 10]);
view(-37,40);
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
end
ty = result.at(transformKey).translation().y();
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
text(1,5,3,sprintf('fx(900): %.0f',fx));
text(1,5,1,sprintf('fy(900): %.0f',fy));
if(write_video)
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame)
else
pause(0.1);
end
end
if(write_video)
close(videoObj);
end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(transformKey)
disp('Calibration after optimization');
result.at(calibrationKey)

354
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m Normal file
View File

@ -0,0 +1,354 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Estimate trajectory, calibration, landmarks, body-camera offset,
% IMU
% @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
import gtsam.*
write_video = false;
if(write_video)
videoObj = VideoWriter('test.avi');
videoObj.Quality = 100;
videoObj.FrameRate = 2;
open(videoObj);
end
%% generate some landmarks
nrPoints = 8;
landmarks = {Point3([20 15 1]'),...
Point3([22 7 -1]'),...
Point3([20 20 6]'),...
Point3([24 19 -4]'),...
Point3([26 17 -2]'),...
Point3([12 15 4]'),...
Point3([25 11 -6]'),...
Point3([23 10 4]')};
IMU_metadata.AccelerometerSigma = 1e-2;
IMU_metadata.GyroscopeSigma = 1e-2;
IMU_metadata.AccelerometerBiasSigma = 1e-6;
IMU_metadata.GyroscopeBiasSigma = 1e-6;
IMU_metadata.IntegrationSigma = 1e-1;
curvature = 5.0;
transformKey = 1000;
calibrationKey = 2000;
steps = 50;
fg = NonlinearFactorGraph;
initial = Values;
%% intial landmarks and camera trajectory shifted in + y-direction
y_shift = Point3(0,0.5,0);
% insert shifted points
for i=1:nrPoints
initial.insert(100+i,landmarks{i}.compose(y_shift));
end
figure(1);
cla
hold on;
%% initial pose priors
pose_cov = noiseModel.Diagonal.Sigmas([0.1*pi/180; 0.1*pi/180; 0.1*pi/180; 1e-4; 1e-4; 1e-4]);
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
trans_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 20; 1e-6; 1e-6]);
move_forward = Pose3(Rot3(),Point3(1,0,0));
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
%% calibration initialization
K = Cal3_S2(900,900,0,640,480);
K_corrupt = Cal3_S2(910,890,0,650,470);
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
cheirality_exception_count = 0;
isamParams = gtsam.ISAM2Params;
isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
currentIMUPoseGlobal = Pose3();
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];
for i=1:steps
t = i-1;
currentVelKey = symbol('v',i);
currentBiasKey = symbol('b',i);
initial.insert(currentVelKey, currentVelocityGlobal);
initial.insert(currentBiasKey, currentBias);
if i==1
% Pose Priors
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
% insert first
initial.insert(1, Pose3());
% camera transform
initial.insert(transformKey,camera_transform);
fg.add(PriorFactorPose3(transformKey,camera_transform,trans_cov));
% calibration
initial.insert(2000, K_corrupt);
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
% velocity and bias evolution
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
result = initial;
end
if i == 2
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end
if i > 1
if i < 11
step = move_forward;
else
step = move_circle;
end
initial.insert(i,result.at(i-1).compose(step));
fg.add(BetweenFactorPose3(i-1,i, step, covariance));
deltaT = 1;
logmap = Pose3.Logmap(step);
omega = logmap(1:3);
velocity = logmap(4:6);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
[ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
accMeas = acc_omega(1:3)-g;
omegaMeas = acc_omega(4:6);
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
%% create IMU factor
fg.add(ImuFactor( ...
i-1, currentVelKey-1, ...
i, currentVelKey, ...
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
% Bias evolution as given in the IMU metadata
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
noiseModel.Diagonal.Sigmas(sqrt(steps) * sigma_between_b)));
end
% generate some camera measurements
cam_pose = currentIMUPoseGlobal.compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
% result
for j=1:nrPoints
% All landmarks seen in every frame
try
z = cam.project(landmarks{j});
fg.add(TransformCalProjectionFactorCal3_S2(z, z_cov, i, transformKey, 100+j, calibrationKey, false, true));
catch
cheirality_exception_count = cheirality_exception_count + 1;
end % end try/catch
end
if i > 1
disp('ISAM Update');
isam.update(fg, initial);
result = isam.calculateEstimate();
%% reset
initial = Values;
fg = NonlinearFactorGraph;
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);
%% Compute some marginals
marginal = isam.marginalCovariance(calibrationKey);
marginal_fx(i)=sqrt(marginal(1,1));
marginal_fy(i)=sqrt(marginal(2,2));
%% Compute condition number
isam_fg = isam.getFactorsUnsafe();
isam_values = isam.getLinearizationPoint();
gfg = isam_fg.linearize(isam_values);
mat = gfg.jacobian();
c(i) = cond(mat, 2);
mat = gfg.augmentedJacobian();
augmented_c(i)= cond(mat, 2);
for f=0:isam_fg.size()-1
nonlinear_factor = isam_fg.at(f);
if strcmp(class(nonlinear_factor),'gtsam.TransformCalProjectionFactorCal3_S2')
gaussian_factor = nonlinear_factor.linearize(isam_values);
A = gaussian_factor.getA();
b = gaussian_factor.getb();
% Column 17 (fy) in jacobian
A_col = A(:,17);
if A_col(2) == 0
% pause
disp('Cheirality Exception!');
end
end
end
end
hold off;
clf;
figure(1);
subplot(5,1,1:2);
hold on;
%% plot the integrated IMU frame (not from
gtsam.plotPose3(currentIMUPoseGlobal, [], 2);
%% plot results
result_camera_transform = result.at(transformKey);
for j=1:i
gtsam.plotPose3(result.at(j),[],0.5);
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
ylabel('y (m)');
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
axis([0 20 0 20 -10 10]);
view(-37,40);
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
end
ty = result.at(transformKey).translation().y();
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
px = result.at(calibrationKey).px();
py = result.at(calibrationKey).py();
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
text(1,5,3,sprintf('fx(900): %.0f',fx));
text(1,5,1,sprintf('fy(900): %.0f',fy));
fxs(i) = fx;
fys(i) = fy;
pxs(i) = px;
pys(i) = py;
subplot(5,1,3);
hold on;
plot(1:steps,repmat(K.fx,1,steps),'r--');
p(1) = plot(1:i,fxs,'r','LineWidth',2);
plot(1:steps,repmat(K.fy,1,steps),'g--');
p(2) = plot(1:i,fys,'g','LineWidth',2);
if i > 1
plot(2:i,fxs(2:i) + marginal_fx(2:i),'r-.');
plot(2:i,fxs(2:i) - marginal_fx(2:i),'r-.');
plot(2:i,fys(2:i) + marginal_fy(2:i),'g-.');
plot(2:i,fys(2:i) - marginal_fy(2:i),'g-.');
subplot(5,1,5);
hold on;
title('Condition Number');
plot(2:i,c(2:i),'b-');
plot(2:i,augmented_c(2:i),'r-');
axis([0 steps 0 max(c(2:i))*1.1]);
% figure(2);
% plotBayesTree(isam);
end
legend(p, 'f_x', 'f_y', 'Location', 'SouthWest');
% legend(p, 'f_x', 'f_x''', 'f_y', 'f_y''', 'Location', 'SouthWest');
%% plot principal points
subplot(5,1,4);
hold on;
plot(1:steps,repmat(K.px,1,steps),'r--');
pp(1) = plot(1:i,pxs,'r','LineWidth',2);
plot(1:steps,repmat(K.py,1,steps),'g--');
pp(2) = plot(1:i,pys,'g','LineWidth',2);
title('Principal Point');
legend(pp, 'p_x', 'p_y', 'Location', 'SouthWest');
if(write_video)
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame)
else
pause(0.1);
end
end
if(write_video)
close(videoObj);
end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(transformKey)
disp('Calibration after optimization');
result.at(calibrationKey)
disp('Bias after optimization');
currentBias

120
matlab/unstable_examples/TransformProjectionFactorExample.m Normal file
View File

@ -0,0 +1,120 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
import gtsam.*
%% generate some landmarks
nrPoints = 8;
landmarks = {Point3([20 15 1]'),...
Point3([22 7 1]'),...
Point3([20 20 6]'),...
Point3([24 19 4]'),...
Point3([26 17 2]'),...
Point3([12 15 4]'),...
Point3([25 11 6]'),...
Point3([23 10 4]')};
fg = NonlinearFactorGraph;
fg.add(NonlinearEqualityPose3(1, Pose3()));
initial = Values;
%% intial landmarks and camera trajectory shifted in + y-direction
y_shift = Point3(0,1,0);
% insert shifted points
for i=1:nrPoints
initial.insert(100+i,landmarks{i}.compose(y_shift));
end
figure(1);
cla
hold on;
plot3DPoints(initial);
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
initial.insert(1000,camera_transform);
%% insert poses
initial.insert(1, Pose3());
move_forward = Pose3(Rot3(),Point3(1,0,0));
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,0.2),Point3(1,0,0));
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
K = Cal3_S2(900,900,0,640,480);
cheirality_exception_count = 0;
for i=1:20
if i > 1
if i < 11
initial.insert(i,initial.at(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,initial.at(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
for j=1:nrPoints
% All landmarks seen in every frame
try
z = cam.project(landmarks{j});
fg.add(TransformProjectionFactorCal3_S2(z, z_cov, i, 1000, 100+j, K));
catch
cheirality_exception_count = cheirality_exception_count + 1;
end % end try/catch
end
end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
% plot3DTrajectory(initial, 'g-*');
%% camera plotting
for i=1:20
gtsam.plotPose3(initial.at(i).compose(camera_transform));
end
xlabel('x (m)');
ylabel('y (m)');
disp('Transform before optimization');
initial.at(1000)
params = LevenbergMarquardtParams;
params.setAbsoluteErrorTol(1e-15);
params.setRelativeErrorTol(1e-15);
params.setVerbosity('ERROR');
params.setVerbosityLM('VERBOSE');
optimizer = LevenbergMarquardtOptimizer(fg, initial, params);
result = optimizer.optimizeSafely();
disp('Transform after optimization');
result.at(1000)
axis([0 25 0 25 0 10]);
axis equal
view(-37,40)

174
matlab/unstable_examples/TransformProjectionFactorExampleISAM.m Normal file
View File

@ -0,0 +1,174 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
import gtsam.*
write_video = true;
if(write_video)
videoObj = VideoWriter('test.avi');
videoObj.Quality = 100;
videoObj.FrameRate = 2;
open(videoObj);
end
%% generate some landmarks
nrPoints = 8;
landmarks = {Point3([20 15 1]'),...
Point3([22 7 -1]'),...
Point3([20 20 6]'),...
Point3([24 19 -4]'),...
Point3([26 17 -2]'),...
Point3([12 15 4]'),...
Point3([25 11 -6]'),...
Point3([23 10 4]')};
fg = NonlinearFactorGraph;
pose_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 0.1; 0.1; 0.1]);
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
curvature = 0.5;
initial = Values;
%% intial landmarks and camera trajectory shifted in + y-direction
y_shift = Point3(0,1,0);
% insert shifted points
for i=1:nrPoints
initial.insert(100+i,landmarks{i}.compose(y_shift));
end
figure(1);
cla
hold on;
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
initial.insert(1000,camera_transform);
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 20]);
fg.add(PriorFactorPose3(1000,camera_transform,trans_cov));
%% insert poses
initial.insert(1, Pose3());
move_forward = Pose3(Rot3(),Point3(1,0,0));
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
K = Cal3_S2(900,900,0,640,480);
cheirality_exception_count = 0;
isamParams = gtsam.ISAM2Params;
isamParams.setFactorization('QR');
isam = ISAM2(isamParams);
result = initial
for i=1:20
if i > 1
if i < 11
initial.insert(i,result.at(i-1).compose(move_forward));
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
else
initial.insert(i,result.at(i-1).compose(move_circle));
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
end
end
% generate some camera measurements
cam_pose = initial.at(i).compose(actual_transform);
% gtsam.plotPose3(cam_pose);
cam = SimpleCamera(cam_pose,K);
i
% result
for j=1:nrPoints
% All landmarks seen in every frame
try
z = cam.project(landmarks{j});
fg.add(TransformProjectionFactorCal3_S2(z, z_cov, i, 1000, 100+j, K));
catch
cheirality_exception_count = cheirality_exception_count + 1;
end % end try/catch
end
if i > 2
disp('ISAM Update');
isam.update(fg, initial);
result = isam.calculateEstimate();
%% reset
initial = Values;
fg = NonlinearFactorGraph;
end
hold off;
clf;
hold on;
%% plot results
result_camera_transform = result.at(1000);
for j=1:i
gtsam.plotPose3(result.at(j));
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
ylabel('y (m)');
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
axis([0 20 0 20 -10 10]);
view(-37,40);
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
end
ty = result.at(1000).translation().y();
text(5,5,5,sprintf('Y-Transform: %0.2g',ty));
if(write_video)
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame)
else
pause(0.001);
end
end
if(write_video)
close(videoObj);
end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(1000)

56
matlab/unstable_examples/flight_trajectory.m Normal file
View File

@ -0,0 +1,56 @@
function [ values ] = flight_trajectory( input_args )
%UNTITLED2 Summary of this function goes here
% Detailed explanation goes here
import gtsam.*;
values = Values;
curvature = 2;
forward = Pose3(Rot3(),Point3(10,0,0));
left = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(10,0,0));
right = Pose3(Rot3.RzRyRx(0.0,0.0,-curvature*pi/180),Point3(10,0,0));
pose = Pose3(Rot3.RzRyRx(0,0,0),Point3(0,0,1000));
plan(1).direction = right;
plan(1).steps = 20;
plan(2).direction = forward;
plan(2).steps = 5;
plan(3).direction = left;
plan(3).steps = 100;
plan(4).direction = forward;
plan(4).steps = 50;
plan(5).direction = left;
plan(5).steps = 80;
plan(6).direction = forward;
plan(6).steps = 50;
plan(7).direction = right;
plan(7).steps = 100;
plan_steps = numel(plan);
values_i = 0;
for i=1:plan_steps
direction = plan(i).direction;
segment_steps = plan(i).steps;
for j=1:segment_steps
pose = pose.compose(direction);
values.insert(symbol('x',values_i), pose);
values_i = values_i + 1;
end
end
end

19
matlab/unstable_examples/ground_landmarks.m Normal file
View File

@ -0,0 +1,19 @@
function [ values ] = ground_landmarks( nrPoints )
%UNTITLED2 Summary of this function goes here
% Detailed explanation goes here
import gtsam.*;
values = Values;
x = -800+1600.*rand(nrPoints,1);
y = -800+1600.*rand(nrPoints,1);
z = 3 * rand(nrPoints,1);
for i=1:nrPoints
values.insert(symbol('l',i),gtsam.Point3(x(i),y(i),z(i)));
end
end

37
matlab/unstable_examples/plot_projected_landmarks.m Normal file
View File

@ -0,0 +1,37 @@
function [] = plot_projected_landmarks( a, landmarks, measurements )
%UNTITLED4 Summary of this function goes here
% Detailed explanation goes here
persistent h;
if ishghandle(h)
delete(h);
end
measurement_keys = gtsam.KeyVector(measurements.keys);
nrMeasurements = measurement_keys.size;
if nrMeasurements == 0
return;
end
x = zeros(1,nrMeasurements);
y = zeros(1,nrMeasurements);
z = zeros(1,nrMeasurements);
% Plot points and covariance matrices
for i = 0:measurement_keys.size-1
key = measurement_keys.at(i);
key_index = gtsam.symbolIndex(key);
p = landmarks.at(gtsam.symbol('l',key_index));
x(i+1) = p.x;
y(i+1) = p.y;
z(i+1) = p.z;
end
h = plot3(a, x,y,z,'rd', 'LineWidth',3);
end

51
matlab/unstable_examples/project_landmarks.m Normal file
View File

@ -0,0 +1,51 @@
function [ measurements ] = project_landmarks( pose, landmarks, K )
%UNTITLED3 Summary of this function goes here
% Detailed explanation goes here
import gtsam.*;
camera = SimpleCamera(pose,K);
measurements = Values;
for i=1:size(landmarks)-1
z = camera.project(landmarks.at(symbol('l',i)));
% check bounding box
if z.x < 0 || z.x > 1280
continue
elseif z.y < 0 || z.y > 960
continue
end
measurements.insert(symbol('z',i),z);
end
% persistent h;
%
% if isempty(h)
% h = figure();
% else
% figure(h);
% end
% clf;
if measurements.size == 0
return
end
cla;
plot2DPoints(measurements,'*g');
text(1120, 1000, sprintf('# = %d', measurements.size));
axis equal;
axis([0 1280 0 960]);
set(gca, 'YDir', 'reverse');
xlabel('u (pixels)');
ylabel('v (pixels)');
set(gca, 'XAxisLocation', 'top');
end

4
tests/testNonlinearISAM.cpp
View File

@ -212,11 +212,11 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
// verify values - all but the last one should be very close
Values actualChol = isamChol.estimate();
for (size_t i=0; i<nrPoses; ++i)
EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), 1e-4));
Values actualQR = isamQR.estimate();
for (size_t i=0; i<nrPoses; ++i)
EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), 1e-4));
// Check landmarks
EXPECT(assert_equal(expected.at<Point2>(lm1), actualChol.at<Point2>(lm1), tol));