From 0dc1eac55cd34b18695f25916892324e1a962e12 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 21 Dec 2013 20:12:12 +0000 Subject: [PATCH] Merged changes from the trunk back into navigation and slam. Needed some data files for tests, as well. git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/branches/2.4@20423 898a188c-9671-0410-8e00-e3fd810bbb7f --- examples/Data/5pointExample1.txt | 53 + examples/Data/Balbianello.out | 1659 +++++++++++++++++ examples/Data/dubrovnik-3-7-pre-rewritten.txt | 80 + examples/Data/dubrovnik-3-7-pre.txt | 80 + gtsam/navigation/ImuBias.h | 26 +- .../tests/testCombinedImuFactor.cpp | 26 +- gtsam/navigation/tests/testImuBias.cpp | 6 +- gtsam/slam/BoundingConstraint.h | 8 +- gtsam/slam/EssentialMatrixFactor.h | 54 + gtsam/slam/RotateFactor.h | 99 + gtsam/slam/dataset.cpp | 334 +++- gtsam/slam/dataset.h | 187 +- gtsam/slam/tests/testAntiFactor.cpp | 2 +- gtsam/slam/tests/testBetweenFactor.cpp | 4 +- gtsam/slam/tests/testDataset.cpp | 183 +- .../slam/tests/testEssentialMatrixFactor.cpp | 147 ++ gtsam/slam/tests/testGeneralSFMFactor.cpp | 6 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 6 +- gtsam/slam/tests/testPoseRotationPrior.cpp | 10 +- gtsam/slam/tests/testPoseTranslationPrior.cpp | 12 +- gtsam/slam/tests/testProjectionFactor.cpp | 12 +- gtsam/slam/tests/testRangeFactor.cpp | 8 +- gtsam/slam/tests/testRotateFactor.cpp | 185 ++ gtsam/slam/tests/testStereoFactor.cpp | 14 +- 24 files changed, 3072 insertions(+), 129 deletions(-) create mode 100644 examples/Data/5pointExample1.txt create mode 100644 examples/Data/Balbianello.out create mode 100644 examples/Data/dubrovnik-3-7-pre-rewritten.txt create mode 100644 examples/Data/dubrovnik-3-7-pre.txt create mode 100644 gtsam/slam/EssentialMatrixFactor.h create mode 100644 gtsam/slam/RotateFactor.h create mode 100644 gtsam/slam/tests/testEssentialMatrixFactor.cpp create mode 100644 gtsam/slam/tests/testRotateFactor.cpp diff --git a/examples/Data/5pointExample1.txt b/examples/Data/5pointExample1.txt new file mode 100644 index 000000000..212f75aec --- /dev/null +++ b/examples/Data/5pointExample1.txt @@ -0,0 +1,53 @@ +2 5 10 + +0 0 0 -0 +1 0 -6.123233995736766344e-18 -0.10000000000000000555 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 0.10000000000000000555 -0 +1 2 0 -0 +0 3 0 -1 +1 3 1 -0.20000000000000006661 +0 4 0 1 +1 4 -1 -0.19999999999999995559 + +3.141592653589793116 +0 +0 +-0 +0 +0 +1 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 +0 +-6.123233995736766344e-18 +-0.10000000000000000555 +0 +1 +0 +0 + +0 +0 +1 + +-0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0 +1 + +0 +0.5 +0.5 + +0 +-0.5 +0.5 + diff --git a/examples/Data/Balbianello.out b/examples/Data/Balbianello.out new file mode 100644 index 000000000..9ce24e9eb --- /dev/null +++ b/examples/Data/Balbianello.out @@ -0,0 +1,1659 @@ +# Bundle file v0.3 +5 544 +5.1869203975e+02 -1.1457014134e-01 -3.4479818947e-02 +9.9972739831e-01 5.9754666132e-03 2.2570397996e-02 +-6.3019161555e-03 9.9987616292e-01 1.4420286863e-02 +-2.2481435001e-02 -1.4558592624e-02 9.9964125188e-01 +7.1074927420e-02 4.4169219329e-02 5.6191022645e-01 +5.2076287822e+02 -1.2694794766e-01 2.3581020948e-02 +9.9090026638e-01 -1.9447047306e-02 -1.3318586426e-01 +2.5225522118e-02 9.9880593962e-01 4.1837399630e-02 +1.3221321841e-01 -4.4816373403e-02 9.9020763356e-01 +-2.3400802602e-01 3.8566104615e-02 4.5891189236e-01 +5.2078687110e+02 -1.3845031911e-01 8.8164199219e-02 +9.6414182620e-01 -2.8616950758e-02 -2.6384012035e-01 +8.9847670516e-03 9.9711908434e-01 -7.5318029711e-02 +2.6523539157e-01 7.0246720691e-02 9.6162133155e-01 +-4.6692292637e-01 -2.0478223609e-02 3.3422623587e-01 +5.1785173861e+02 -1.1983917773e-01 3.8806660874e-02 +9.4340686617e-01 -3.3565979665e-02 -3.2993455393e-01 +1.7067356686e-02 9.9846047986e-01 -5.2776656676e-02 +3.3119811323e-01 4.4158749566e-02 9.4252735484e-01 +-7.6428294145e-01 -2.4602640352e-02 2.0347992508e-01 +5.2005740007e+02 -1.0900307866e-01 -4.2992346969e-02 +8.2691803240e-01 -1.0052089965e-01 -5.5326496042e-01 +8.2265309276e-02 9.9493231048e-01 -5.7811040902e-02 +5.5627240322e-01 2.2904791136e-03 8.3099685145e-01 +-1.2112342076e+00 -1.0358901179e-01 -1.7024807421e-01 +1.0348687869e-01 -1.2489429393e-01 -2.0153888320e+00 +70 74 54 +3 0 27 45.2700 -38.3700 3 20 0.5500 -13.8100 1 17 48.3800 -57.5500 +-2.2635283095e-01 -9.9920725523e-02 -1.9536947458e+00 +51 67 44 +4 0 47 -74.7700 -30.4100 3 86 -99.1000 -7.5300 1 69 -67.7000 -51.5100 2 112 -53.7900 7.7000 +-4.2371314554e-01 -8.9101655483e-02 -1.9790749524e+00 +219 212 209 +4 0 48 -145.3700 -26.1400 3 50 -144.6000 -5.8700 1 42 -129.5000 -46.5900 2 70 -106.9200 11.1400 +-3.6419344622e-01 -1.6116890917e-01 -1.8833671644e+00 +219 209 190 +4 0 74 -131.9800 -57.3600 3 216 -145.2000 -26.3700 1 96 -121.4900 -75.2400 2 158 -103.5000 -12.9700 +-3.2546116378e-01 -1.6010922165e-01 -1.9075488641e+00 +121 112 91 +5 0 111 -115.0500 -54.2100 3 215 -131.1100 -26.8100 1 137 -105.4400 -73.9700 2 157 -88.7400 -12.7000 4 246 -109.1400 -48.2700 +4.1824218699e-02 -1.5300487566e-01 -1.9365008511e+00 +217 204 181 +5 0 112 25.5300 -51.2000 3 80 -25.6700 -23.9900 1 97 24.3400 -69.6500 2 108 30.7500 -8.9900 4 61 -26.8900 -42.3500 +-2.8018621479e-01 -8.6303814527e-02 -1.9601358308e+00 +172 187 193 +4 0 119 -94.4400 -24.7100 3 344 -112.2400 -4.5500 1 146 -85.1200 -46.5300 2 252 -69.0200 12.0000 +-4.1428798295e-01 -2.1521889736e-01 -1.8199013326e+00 +35 43 29 +4 0 162 -158.4400 -79.8300 3 330 -168.7300 -46.2400 1 219 -146.8800 -97.3100 2 239 -127.3100 -34.2200 +1.6788060226e-01 -8.5072469979e-02 -2.0429262038e+00 +180 183 189 +4 0 181 66.8700 -25.2100 3 138 21.6500 0.8100 1 150 69.7200 -43.2900 2 166 77.5100 16.4200 +1.5416247751e-02 -8.1713024631e-02 -1.8873380162e+00 +206 206 206 +4 0 182 17.0100 -25.4500 3 87 -40.8800 -1.9400 1 100 12.6500 -45.2300 2 115 16.9700 14.2800 +-3.1345187701e-01 -8.2575288182e-02 -1.9585057765e+00 +58 71 51 +3 0 185 -106.6500 -23.2500 3 223 -120.7500 -3.3100 1 149 -96.4000 -45.9400 +-6.0080024848e-02 6.8137242263e-03 -1.9034614416e+00 +220 203 184 +4 0 190 -12.4000 9.8400 3 358 -62.0500 25.7300 1 260 -14.6300 -13.1300 2 275 -7.6800 44.3800 +-4.5534333983e-01 1.9815868815e-01 -2.0417019107e+00 +254 254 254 +4 0 211 -149.2300 74.8700 3 271 -144.5300 74.8900 1 186 -131.8600 44.5600 2 193 -111.0100 96.9300 +-3.8725019873e-01 -2.5674320739e-01 -1.8007421117e+00 +131 130 146 +4 0 239 -149.3200 -98.3800 3 318 -164.7100 -60.1200 1 314 -140.3000 -113.9000 2 336 -121.7200 -49.2500 +-2.0330763195e-01 -2.0105337546e-01 -1.8384775562e+00 +206 189 167 +4 0 246 -71.1600 -73.9100 3 334 -109.8700 -42.4500 1 221 -70.1000 -91.6400 2 352 -59.5200 -29.2400 +-5.3161321425e-01 -1.5283503871e-01 -1.9016364768e+00 +149 151 155 +4 0 254 -193.6500 -50.9700 3 478 -182.2500 -24.6100 1 331 -173.8200 -71.2900 2 364 -148.1700 -10.9100 +-5.3023357897e-01 -1.5261860832e-01 -1.8987342953e+00 +149 151 155 +5 0 255 -193.6500 -50.9700 3 479 -182.2500 -24.6100 1 332 -173.8200 -71.2900 2 365 -148.1700 -10.9100 4 516 -147.3100 -47.7800 +3.8779392880e-01 -7.3681586595e-02 -2.0192289822e+00 +180 184 193 +4 0 267 144.9600 -21.5400 3 348 88.6300 5.0500 1 240 145.6100 -38.0700 2 258 150.6900 21.0300 +2.2739999793e-01 -6.7003076973e-02 -2.0591800114e+00 +64 79 66 +4 0 270 87.0300 -18.7900 3 352 41.7600 6.6500 2 373 95.8800 22.8200 4 254 36.3100 -9.5700 +-1.6136379489e-01 2.6168208831e-02 -1.9505552154e+00 +207 206 202 +4 0 284 -50.0700 15.4100 3 709 -83.7800 31.8000 1 367 -46.9800 -7.3900 2 542 -35.6700 49.9400 +-1.0165801523e-01 4.7902361007e-02 -2.2721717644e+00 +215 198 168 +4 0 285 -26.7300 18.3500 3 368 -27.6800 35.2700 1 268 -2.6900 -2.0300 2 392 2.6200 55.1700 +-4.1259052076e-01 1.4107854521e-01 -2.0253659157e+00 +221 216 215 +2 0 299 -136.3200 55.9200 3 387 -136.8600 60.4300 +-3.1219183918e-02 1.8877766391e-01 -2.3280599623e+00 +225 203 175 +4 0 300 -3.6400 58.4800 3 391 -8.1600 74.1400 2 414 30.8500 93.8100 4 434 7.6300 52.6800 +4.8372524472e-03 1.9172234269e-01 -2.3267263233e+00 +60 47 47 +4 0 301 7.1000 59.2600 3 272 0.2300 75.6500 1 277 21.6200 37.2400 2 294 40.3200 94.9500 +5.3445351764e-03 1.9105868605e-01 -2.3211920947e+00 +60 47 47 +5 0 302 7.1000 59.2600 3 273 0.2300 75.6500 1 278 21.6200 37.2400 2 295 40.3200 94.9500 4 305 14.1300 54.7300 +-6.2408844848e-01 -2.8925695436e-01 -1.8608705074e+00 +81 71 69 +4 0 340 -233.9800 -104.9700 3 845 -209.0200 -64.6500 1 435 -208.7300 -120.0300 2 647 -177.5400 -54.3900 +3.2470954578e-01 -2.3330893027e-01 -1.6273458954e+00 +45 44 42 +2 0 341 170.3700 -102.1600 3 446 36.3500 -69.6900 +-5.4257666827e-01 -1.3281459995e-01 -1.8982085264e+00 +47 65 31 +4 0 359 -198.3700 -42.9500 3 655 -185.5000 -19.0300 2 503 -151.9600 -4.6600 4 520 -149.9300 -43.4900 +-1.7586329353e-01 -8.6646269088e-02 -1.9443832670e+00 +39 47 57 +4 0 368 -56.2100 -25.7600 3 667 -87.3600 -3.7900 1 472 -51.6000 -46.8100 2 698 -39.3900 12.3500 +7.0190826516e-01 -7.5096209881e-02 -2.2595273008e+00 +187 175 162 +4 0 373 214.0000 -20.1100 3 234 193.9800 9.6400 1 346 230.3700 -34.3600 2 261 246.5100 24.8000 +1.9418317140e-01 -6.6971222707e-02 -2.0634286157e+00 +52 69 40 +4 0 378 75.1300 -18.7000 3 506 31.4000 6.5600 1 345 78.0900 -36.7000 2 374 86.3500 22.7600 +-2.0809221393e-01 -6.4519619308e-02 -1.9453915281e+00 +68 83 41 +3 0 380 -68.1100 -17.7700 3 501 -95.9300 3.0700 1 344 -62.6300 -39.1000 +-1.9091093823e-01 -5.4168294330e-02 -1.9521716392e+00 +192 194 199 +4 0 381 -61.4400 -13.6800 3 678 -90.5500 6.3300 1 486 -56.2900 -35.5200 2 520 -43.6300 23.0700 +3.0934815796e-01 -4.3212083279e-02 -2.0491698349e+00 +200 185 162 +3 0 386 115.2700 -10.5900 3 514 65.0700 14.4500 2 381 123.9000 31.1300 +-1.7720865568e-01 -3.8836756197e-02 -1.9529384884e+00 +43 44 45 +4 0 387 -56.3200 -7.2100 3 928 -87.0000 10.0000 1 494 -51.7400 -29.6600 2 712 -39.2900 27.8300 +-2.8261679356e-01 1.2899723260e-02 -1.9840419662e+00 +48 49 54 +4 0 402 -93.5600 10.7200 3 702 -110.4000 26.3200 1 362 -84.0800 -13.2400 2 538 -67.9700 44.0100 +-1.4656912921e-01 1.5208210408e-01 -2.3585568184e+00 +228 205 178 +4 0 422 -36.6900 47.1800 3 551 -32.3900 62.3900 1 525 -18.9900 24.0400 2 566 2.6900 81.3700 +6.6082423412e-03 1.5984936071e-01 -2.3149305669e+00 +223 203 168 +5 0 426 7.5400 50.4300 3 554 0.2500 67.3900 1 389 21.9400 28.7600 2 413 40.5800 86.6200 4 432 14.4100 47.0200 +1.5074095667e+00 1.9657948722e+00 -8.7451927287e+00 +255 253 255 +4 0 441 86.7300 116.8800 3 291 233.2200 162.8300 1 409 151.3100 106.0600 2 309 215.9900 174.1100 +-5.4087935965e-01 -2.3698879879e-01 -1.8620374852e+00 +147 148 143 +4 0 500 -202.6400 -85.2400 3 623 -190.7400 -50.4900 1 609 -182.4100 -102.0400 2 652 -155.9300 -38.8000 +-5.5428288652e-01 -1.5378413034e-01 -1.8992889864e+00 +53 67 36 +5 0 528 -202.4200 -51.6400 3 652 -187.6700 -24.5600 1 455 -181.3600 -71.8000 2 680 -154.7600 -11.2400 4 1003 -151.0200 -47.8600 +9.1969451459e-01 -9.9211661386e-02 -2.4155346913e+00 +199 172 155 +5 0 544 250.6100 -25.7500 3 349 256.1600 5.7200 1 481 278.1000 -38.7500 2 517 303.2300 20.1500 4 262 269.6500 4.2100 +-1.9448909337e-01 -7.7757068130e-02 -1.9499324969e+00 +211 208 199 +5 0 546 -63.0000 -22.6800 3 669 -91.3500 -0.6400 1 478 -57.5600 -43.6400 2 700 -44.6700 15.3900 4 1045 -78.1100 -23.2500 +2.8777692939e-01 -6.1422728396e-02 -2.0455370299e+00 +74 102 46 +3 0 554 108.1700 -16.9200 3 680 58.2300 8.5500 2 521 116.7900 24.9000 +-3.3910297156e-01 -9.6901696034e-04 -1.9890413773e+00 +196 205 211 +4 0 581 -113.7300 5.8800 3 950 -123.4400 21.4700 1 682 -101.6500 -18.0700 2 727 -83.5400 38.9500 +-2.3295325652e-01 3.0736880914e-02 -1.9576739856e+00 +192 194 192 +4 0 590 -76.7500 17.7000 3 707 -101.9500 31.5200 1 700 -70.1700 -6.2000 2 744 -56.6300 50.4000 +4.3565438384e-01 1.1858510885e-01 -2.1316917042e+00 +86 97 64 +4 0 602 149.0700 42.2100 3 738 107.4800 66.1300 1 526 154.2700 25.4200 2 569 164.7400 84.2900 +5.8596710361e-02 1.6897908638e-01 -2.3142764976e+00 +226 200 167 +4 0 609 23.2000 52.8600 3 741 12.9500 70.8800 1 529 36.5200 31.7300 2 571 54.3100 89.7500 +8.7097256325e-02 1.9602917723e-01 -2.3010167995e+00 +230 199 170 +4 0 614 31.8500 61.3400 3 744 18.8000 78.7200 1 533 44.3300 39.9200 2 768 61.5100 98.2800 +2.6668718423e-01 -2.2732343844e-01 -1.6318127165e+00 +121 121 124 +4 0 685 142.9300 -99.3300 3 842 13.6500 -66.2100 1 596 116.4800 -115.5900 2 648 99.7800 -53.5800 +3.1192297619e-01 -2.2732998416e-01 -1.6293737816e+00 +139 140 146 +4 0 686 164.2400 -99.2400 3 841 31.3200 -66.9500 1 829 137.7400 -115.5200 2 879 119.7800 -54.0200 +3.2819986348e-01 -2.2105045606e-01 -1.6303417250e+00 +108 112 122 +4 0 690 171.6000 -96.0800 3 843 37.9600 -64.4200 1 599 145.3400 -112.4800 2 882 126.9700 -51.2700 +-2.3330589679e-01 -2.2643035629e-01 -1.7955587351e+00 +64 77 50 +4 0 699 -85.8100 -86.8900 3 859 -125.1700 -52.0600 1 608 -85.4700 -103.5300 2 651 -74.7900 -40.0500 +-3.0164610151e-01 -2.2616856742e-01 -1.7895742275e+00 +70 72 57 +4 0 700 -114.9200 -87.0100 3 1197 -144.9300 -51.6200 1 840 -111.3200 -103.5100 2 903 -97.5900 -39.8800 +-3.0164610151e-01 -2.2616856742e-01 -1.7895742275e+00 +70 72 57 +4 0 701 -114.9200 -87.0100 3 1198 -144.9300 -51.6200 1 841 -111.3200 -103.5100 2 904 -97.5900 -39.8800 +-3.1396420107e-01 -2.1700020305e-01 -1.7963962752e+00 +134 149 78 +4 0 705 -119.5100 -82.4800 3 1207 -147.1500 -48.5000 1 847 -114.9800 -99.5700 2 912 -100.4700 -36.3900 +-5.5635539663e-01 -2.2739448785e-01 -1.8798937992e+00 +53 64 46 +4 0 710 -205.8700 -80.7400 3 870 -190.9100 -46.3100 1 615 -184.6600 -97.9400 2 918 -157.3200 -34.7800 +-1.9122770672e-01 -9.7046671099e-02 -1.9469149319e+00 +220 218 209 +5 0 748 -61.8900 -29.9500 3 915 -90.8500 -6.4400 1 644 -56.5100 -50.5300 2 974 -43.9000 9.2100 4 1038 -77.3200 -28.6200 +-3.3275975849e-01 -1.3803253860e-02 -1.9804107524e+00 +121 129 110 +4 0 781 -112.0100 1.6000 3 944 -123.0200 17.3000 1 940 -100.7000 -22.1400 2 1039 -82.6200 34.8400 +-3.1966567179e-01 9.1739802822e-03 -1.9931933317e+00 +118 117 124 +4 0 791 -106.3500 9.4200 3 1365 -118.0900 24.5900 1 953 -95.1400 -14.5900 2 1049 -77.5700 42.3900 +-3.8321106320e-01 1.2267935172e-01 -2.2013349976e+00 +214 206 203 +4 0 827 -113.9800 43.2900 3 990 -104.3000 53.7500 1 718 -94.3000 17.3900 2 1089 -71.4600 72.9000 +-1.4112153127e-01 1.8541537551e-01 -2.3658703044e+00 +177 182 178 +4 0 832 -35.2000 56.3200 3 1008 -30.9600 70.9200 1 726 -16.9900 32.7600 2 1113 4.4500 90.2800 +6.1801792563e-01 1.7830648177e+00 -8.9456379475e+00 +119 132 145 +4 0 860 30.3300 104.1600 3 760 172.6700 142.5000 1 739 94.5000 90.0200 2 787 155.7700 155.1700 +-3.4352937611e-01 -2.4671906469e-01 -1.7970499492e+00 +54 57 37 +3 0 946 -131.6900 -94.8800 3 1188 -154.2300 -57.5000 2 1305 -109.2600 -46.2600 +-1.9904649921e-01 -2.4825604019e-01 -1.8226878497e+00 +92 104 61 +4 0 947 -70.1700 -94.3400 3 848 -110.6000 -58.0900 1 603 -69.7900 -110.2500 2 891 -59.5900 -46.2700 +-1.9517929642e-01 -2.3720423566e-01 -1.8296955124e+00 +173 185 187 +4 0 952 -68.3600 -89.2500 3 855 -108.5800 -54.2100 1 607 -67.5900 -105.8200 2 898 -57.5800 -42.1000 +-1.8543495382e-01 -2.2791568353e-01 -1.8321660775e+00 +67 76 57 +2 0 959 -64.2100 -85.5900 3 1199 -105.5500 -51.0200 +-6.2646225849e-01 -2.4103551563e-01 -1.8730062058e+00 +54 65 54 +4 0 960 -233.1300 -85.7600 3 1204 -207.9000 -50.1200 1 1158 -207.6200 -102.5700 2 1324 -177.0200 -38.9500 +-1.7671752158e-01 -1.8485813586e-01 -1.8561771075e+00 +212 198 187 +4 0 985 -59.7400 -66.8500 3 1242 -99.9100 -36.4200 1 1188 -58.8200 -84.8400 2 1372 -48.8300 -22.9000 +4.2942919715e-01 -1.7619052077e-01 -1.8567904105e+00 +67 66 69 +3 0 995 179.4600 -63.3600 3 889 94.2800 -34.7600 4 1002 66.7000 -47.8100 +3.9062083070e-01 -5.4827686012e-02 -2.0273880764e+00 +101 109 81 +4 0 1075 144.8800 -14.8100 3 1318 89.7400 11.2000 1 925 146.3700 -31.4000 2 1021 151.4900 27.5400 +3.9062083070e-01 -5.4827686012e-02 -2.0273880764e+00 +101 109 81 +4 0 1076 144.8800 -14.8100 3 1319 89.7400 11.2000 1 926 146.3700 -31.4000 2 1022 151.4900 27.5400 +2.7980838281e-01 -4.8615300201e-02 -2.0478764341e+00 +198 181 163 +4 0 1084 105.2300 -12.3300 3 935 55.8200 12.3900 1 931 107.3800 -29.9300 2 1027 114.0400 29.0800 +-2.0056536091e-01 -3.7016827469e-02 -1.9543581882e+00 +84 95 58 +3 0 1093 -64.9200 -7.2500 3 1322 -92.9400 11.3800 1 1324 -59.3900 -29.5100 +-3.7982646250e-01 2.3269988412e-02 -2.0244313536e+00 +176 177 170 +2 0 1140 -125.5000 14.3100 3 1372 -127.9600 27.9200 +-2.8609419979e-01 2.9678517714e-02 -1.9756687583e+00 +188 189 196 +3 0 1143 -95.1100 17.3300 3 1378 -112.4000 30.7400 2 1060 -70.6900 49.0800 +-3.1728368905e-01 3.6086862493e-02 -1.9954755557e+00 +67 55 54 +4 0 1145 -105.2200 19.3800 3 1383 -117.3900 32.1600 1 962 -94.2800 -5.6500 2 1062 -76.9400 50.7200 +-8.2624972412e-01 1.8398169778e-01 -2.7855640717e+00 +119 123 132 +3 0 1175 -188.6100 44.6200 3 1433 -120.5900 55.8400 1 1414 -146.2200 18.1000 +-8.2624972412e-01 1.8398169778e-01 -2.7855640717e+00 +119 123 132 +3 0 1176 -188.6100 44.6200 3 1434 -120.5900 55.8400 1 1415 -146.2200 18.1000 +-1.2602609533e-01 1.7520581349e-01 -2.3575704081e+00 +78 75 79 +4 0 1181 -30.9500 53.8600 3 1465 -28.0800 68.4500 1 1431 -13.2800 30.6400 2 1594 7.7900 87.7500 +7.2518679942e-02 1.8457223191e-01 -2.3024084755e+00 +80 83 85 +4 0 1190 27.4600 57.9900 3 1012 15.2700 75.3800 1 1000 40.2700 36.6500 2 1118 57.6500 94.5800 +-1.2658541158e-01 1.9632473563e-01 -2.3615373040e+00 +78 74 71 +4 0 1193 -31.0200 59.6200 3 1472 -28.0500 73.8700 1 999 -13.3500 36.1900 2 1602 7.7600 93.3200 +-4.0089728491e-01 1.6200615970e-01 -2.0279317369e+00 +185 181 182 +4 0 1199 -131.8800 63.2900 3 1461 -133.9600 66.3600 1 997 -117.2400 33.9600 2 1108 -97.2700 87.4000 +7.0926912912e-02 -7.5306522396e-02 -1.9983632989e+00 +121 115 113 +4 0 1499 34.7600 -21.2600 3 671 -11.1200 1.4700 1 910 35.8000 -40.6900 2 704 43.4000 18.2300 +-2.0074472390e-01 -6.8182028485e-02 -1.9495977445e+00 +110 118 72 +3 0 1507 -65.2100 -19.0800 3 1293 -93.3100 2.1500 1 1276 -59.7400 -40.4200 +4.1257037448e-01 -5.3531806872e-02 -2.0185640623e+00 +84 94 62 +5 0 1540 153.4100 -14.4000 3 1321 96.4000 11.6400 1 1320 154.2700 -30.8100 2 1480 159.3000 28.0900 4 1079 80.8400 -2.0300 +9.8173382198e-02 1.6763443974e-02 -2.0836549831e+00 +72 79 52 +4 0 1626 41.6000 10.5200 3 1377 4.5600 31.1300 1 1374 45.9000 -9.4900 4 1118 5.9900 11.2900 +-3.9222659459e-01 3.1881466555e-02 -2.0307519004e+00 +88 88 87 +4 0 1639 -129.2600 17.3900 3 1376 -129.9800 30.1300 1 1381 -113.8700 -7.9300 2 1532 -93.3500 48.3700 +-1.5584353864e-01 1.9471227315e-01 -2.3710536132e+00 +121 115 118 +2 0 1725 -39.2500 58.9000 3 1470 -33.7400 72.7400 +1.6210760211e-02 2.1116192111e-01 -2.3726110408e+00 +93 96 94 +4 0 1736 11.9800 63.2800 3 1481 3.4700 79.0700 2 1122 43.9800 98.5900 4 1191 25.5600 60.8000 +-1.5389545828e-01 4.1201237094e-01 -2.9974639467e+00 +137 143 139 +4 0 1773 -31.4400 87.7400 3 1030 12.5900 105.2200 1 1472 0.8700 65.5400 2 1631 33.3500 123.9900 +2.1852083632e+00 1.9368113824e+00 -8.9840620716e+00 +137 155 165 +4 0 1786 124.5500 110.8700 3 1040 278.8000 161.0600 1 1491 190.9800 102.0500 2 1147 260.3500 171.4000 +1.6715703133e+00 1.9402022134e+00 -8.5417292355e+00 +251 255 255 +3 0 1792 99.5200 118.2000 3 1511 246.5200 165.2900 1 1498 164.0700 107.9600 +-3.7408438128e-02 -1.2318385685e-01 -2.0022433341e+00 +222 200 174 +2 0 1 -4.4300 -38.4000 2 0 9.3400 1.8400 +8.8039026620e-03 -1.5974736129e-01 -1.8816993336e+00 +219 204 181 +3 0 13 14.2900 -57.0500 1 9 10.7200 -73.5300 2 9 14.8400 -13.0300 +-2.2103531918e-01 -4.4809287725e-02 -1.9489575242e+00 +80 101 63 +3 0 31 -72.6500 -11.6800 1 23 -66.9300 -32.0600 2 45 -53.4200 27.1900 +1.8416697044e-01 -2.0696271765e-01 -1.9682684520e+00 +17 22 21 +2 0 42 76.8800 -69.9200 2 38 80.3100 -26.9000 +-8.0795384508e-01 -1.6840937503e-01 -2.0501713512e+00 +63 86 50 +2 0 44 -266.9600 -50.7600 1 39 -229.9000 -71.3500 +-5.8966032057e-02 -3.5794897814e-02 -1.8979653556e+00 +216 201 186 +3 0 49 -11.9500 -7.3800 1 73 -14.3900 -28.5000 2 74 -7.4900 29.9200 +-9.4440213015e-02 -8.3178254020e-02 -1.9040135014e+00 +24 23 19 +3 0 80 -25.8900 -25.5600 1 101 -26.2100 -45.8400 2 114 -18.1600 13.3700 +2.8410448702e+00 1.7056927416e+00 -8.2764944064e+00 +255 255 255 +2 0 91 178.2400 104.9900 1 85 246.9700 99.3900 +-1.9122833136e-01 -1.4561553674e-01 -1.9316223057e+00 +215 213 206 +3 0 113 -62.4600 -47.8800 1 139 -57.6400 -68.1300 2 162 -45.0800 -7.7300 +-2.8109667713e-01 -8.5652960131e-02 -1.9638221918e+00 +172 187 193 +3 0 118 -94.4400 -24.7100 1 145 -85.1200 -46.5300 2 251 -69.0200 12.0000 +-9.5542788413e-02 -3.5906186673e-02 -1.9173046044e+00 +213 204 189 +2 0 122 -26.0400 -7.4900 2 167 -17.6800 29.8700 +-9.4935860251e-02 2.7079030808e-03 -1.9172785681e+00 +54 64 73 +3 0 127 -25.6800 7.4900 1 154 -26.1700 -14.8500 2 171 -17.8200 42.8000 +-2.1239558474e-02 8.0117980287e-03 -1.8867254867e+00 +35 32 27 +3 0 129 2.8800 9.7300 1 109 -1.4700 -12.2300 2 172 3.5600 45.5600 +-2.1239558474e-02 8.0117980287e-03 -1.8867254867e+00 +35 32 27 +3 0 130 2.8800 9.7300 1 110 -1.4700 -12.2300 2 173 3.5600 45.5600 +-6.0912502939e-02 2.3049124460e-02 -1.9057283071e+00 +217 203 176 +3 0 131 -12.6200 15.1300 1 157 -14.8300 -7.2500 2 177 -8.1400 50.5600 +-6.0912502939e-02 2.3049124460e-02 -1.9057283071e+00 +217 203 176 +3 0 132 -12.6200 15.1300 1 158 -14.8300 -7.2500 2 178 -8.1400 50.5600 +5.8687306909e-01 6.2462038980e-02 -2.1382364798e+00 +25 24 30 +2 0 135 195.4600 22.9300 1 78 203.4500 8.6500 +-5.5139180536e-01 -2.8295998578e-01 -1.8278036705e+00 +34 30 36 +3 0 151 -211.4400 -105.6500 1 210 -191.4200 -120.4000 2 335 -164.0400 -55.0300 +-1.8647955894e-01 -2.4458670739e-01 -1.8299596503e+00 +190 192 210 +3 0 152 -64.7200 -92.4400 1 213 -64.4700 -108.4300 2 233 -54.5800 -44.6700 +-1.8647955894e-01 -2.4458670739e-01 -1.8299596503e+00 +190 192 210 +3 0 153 -64.7200 -92.4400 1 215 -64.4700 -108.4300 2 234 -54.5800 -44.6700 +-6.2850422171e-01 -2.6487584038e-01 -1.8799675860e+00 +175 183 200 +2 0 154 -232.6500 -94.6100 1 211 -206.9000 -109.9900 +-1.7930984824e-01 -2.0032662696e-01 -1.8634051727e+00 +192 169 154 +3 0 167 -60.4200 -72.7200 1 130 -59.1400 -90.2700 2 242 -48.6600 -27.7100 +-1.5705894938e-01 -1.5041614469e-01 -1.9194065641e+00 +88 106 55 +2 0 175 -49.8500 -51.2900 1 230 -46.7500 -69.4600 +1.4292299650e-01 -1.1072614259e-01 -2.0464228297e+00 +56 62 53 +2 0 178 58.2300 -33.5200 1 142 61.3400 -52.0900 +-5.9216569579e-02 -8.1513064136e-02 -1.8943926411e+00 +221 201 174 +2 0 183 -12.2300 -25.2100 1 147 -14.2100 -45.1000 +-5.9216569579e-02 -8.1513064136e-02 -1.8943926411e+00 +221 201 174 +2 0 184 -12.2300 -25.2100 1 148 -14.2100 -45.1000 +-3.2127550794e-01 -2.6509794255e-02 -1.9876353608e+00 +58 69 49 +2 0 187 -107.4400 -3.5600 1 251 -96.0100 -26.1500 +3.2261116218e-01 1.8849345111e-01 -2.7023978369e+00 +229 204 168 +2 0 201 80.2700 45.6100 1 180 104.7900 29.3900 +-1.0306385935e-01 1.5274821609e-01 -2.3503398192e+00 +222 207 179 +3 0 202 -24.3700 47.4300 1 179 -7.1800 24.7200 2 186 13.2000 82.4100 +1.6221776023e+00 1.8386740686e+00 -8.5598008248e+00 +96 113 118 +3 0 215 96.3700 111.3100 1 192 160.7200 101.2000 2 202 226.2700 169.4300 +-1.5037077278e-01 -2.0917781690e-01 -1.8436552987e+00 +205 196 190 +2 0 245 -49.5100 -77.3600 1 220 -50.1700 -94.4300 +-3.6033221217e-01 -1.3434106800e-01 -1.9098775639e+00 +89 101 48 +2 0 256 -128.1400 -44.4800 1 233 -117.0500 -64.6000 +-2.8197718395e-01 -1.1850557827e-01 -1.9475285831e+00 +84 106 68 +2 0 257 -95.8100 -37.5800 1 234 -86.8500 -58.2700 +4.2122442648e-01 -7.1897912544e-02 -2.0175018923e+00 +182 183 189 +2 0 268 156.4100 -20.9000 1 242 157.6000 -37.1400 +-1.6085974419e-01 2.4817878761e-02 -1.9469337458e+00 +207 206 202 +2 0 283 -50.0700 15.4100 1 366 -46.9800 -7.3900 +3.3550753808e+00 1.5217725976e+00 -8.7615423249e+00 +98 121 121 +2 0 307 197.9700 86.8100 1 286 270.1400 81.5100 +-4.6621459383e-01 -2.2591546972e-01 -1.8374673013e+00 +38 44 41 +2 0 344 -176.9100 -83.0000 1 443 -161.9800 -99.8800 +-1.7350697280e-01 -2.2029525532e-01 -1.8421493458e+00 +54 70 37 +3 0 345 -58.8900 -81.8400 1 320 -58.8600 -98.6800 2 475 -49.1000 -35.7100 +-3.0213844790e-01 -1.6202642298e-01 -1.8938173778e+00 +183 162 148 +2 0 349 -107.0800 -55.8900 1 328 -99.1900 -74.9100 +-2.7816799569e-01 -1.5590592449e-01 -1.8981233166e+00 +84 105 55 +3 0 354 -97.5300 -53.3700 1 329 -90.4700 -72.7100 2 362 -75.7900 -11.6300 +-4.1925409047e-01 -1.5676888927e-01 -1.9097647644e+00 +44 48 27 +3 0 355 -150.5300 -52.8100 1 454 -136.4100 -72.4600 2 498 -115.4800 -11.7200 +-4.6565311900e-01 -1.4417498531e-01 -1.9203739091e+00 +47 62 42 +2 0 356 -166.8300 -47.6100 1 459 -149.9600 -67.6200 +-2.0498487443e-01 -1.0696854695e-01 -1.9495712421e+00 +86 97 66 +2 0 364 -66.8900 -33.5600 1 470 -60.9500 -53.8900 +1.8620178084e-01 -4.7014132750e-02 -2.0690012555e+00 +204 184 158 +2 0 385 72.0600 -11.5500 1 350 75.3900 -30.0300 +-2.0827953473e-01 -3.3990970332e-02 -1.9504954114e+00 +69 84 52 +3 0 388 -67.9000 -6.2500 1 355 -62.4000 -28.5600 2 380 -49.4700 29.5500 +-1.9776522810e-01 -1.2548912918e-02 -1.9678553825e+00 +209 205 203 +3 0 394 -63.2100 1.8600 1 357 -57.6700 -21.0600 2 532 -44.4200 36.4100 +-3.0333560296e-01 -5.0217890899e-03 -1.9792098736e+00 +190 193 193 +2 0 395 -101.4200 4.5900 1 502 -91.1800 -19.1600 +-3.0333560296e-01 -5.0217890899e-03 -1.9792098736e+00 +190 193 193 +2 0 396 -101.4200 4.5900 1 503 -91.1800 -19.1600 +-1.7986086460e-01 1.2485288375e-02 -1.9687216455e+00 +43 40 40 +3 0 400 -56.4400 10.2600 1 505 -51.7800 -12.3700 2 735 -39.2100 45.6200 +-2.8279103570e-01 1.2122042657e-02 -1.9845383627e+00 +48 49 54 +2 0 401 -93.5600 10.7200 1 361 -84.0800 -13.2400 +5.9422971686e-03 1.6116020143e-01 -2.3249115162e+00 +223 203 168 +3 0 425 7.5400 50.4300 1 388 21.9400 28.7600 2 412 40.5800 86.6200 +-1.0766878127e-01 1.8652125290e-01 -2.3576892676e+00 +228 201 179 +3 0 428 -25.3300 57.0900 1 531 -8.9000 33.5800 2 572 12.5900 91.1600 +-1.0687689332e-01 1.8725289002e-01 -2.3599694591e+00 +228 201 179 +2 0 429 -25.3300 57.0900 2 573 12.5900 91.1600 +1.0518119988e+00 1.8732211980e+00 -8.6314277499e+00 +253 251 255 +3 0 440 59.5000 113.1400 1 407 123.2600 100.8400 2 425 185.4100 168.0600 +-6.3185039516e-01 -3.0889732915e-01 -1.8790008681e+00 +124 130 140 +3 0 484 -233.9100 -111.2900 1 433 -207.4500 -125.7200 2 646 -176.2800 -58.9500 +-1.5096716072e-01 -2.6093534857e-01 -1.8175296857e+00 +177 173 172 +3 0 489 -50.6900 -99.9100 1 597 -52.0800 -115.4600 2 883 -44.4200 -51.6000 +-5.1507728455e-01 -2.6240523129e-01 -1.8409346633e+00 +160 170 179 +2 0 490 -195.5200 -97.0600 1 600 -177.3800 -112.4500 +-5.6602409556e-01 -2.6310534392e-01 -1.8609694115e+00 +71 70 59 +3 0 491 -212.2100 -95.1100 1 602 -190.5500 -111.3300 2 889 -162.5800 -47.0600 +-4.8279098692e-01 -2.5220487699e-01 -1.8302851423e+00 +44 46 39 +2 0 492 -184.3300 -93.8900 1 604 -168.4800 -109.7100 +-4.8285359792e-01 -2.5228578510e-01 -1.8304485611e+00 +44 46 39 +3 0 493 -184.3300 -93.8900 1 606 -168.4800 -109.7100 2 893 -144.8800 -45.4200 +-2.3036297810e-01 -1.9711734774e-01 -1.8613352307e+00 +72 90 58 +2 0 511 -80.9300 -71.3300 1 624 -77.4800 -89.0200 +-2.7608433150e-01 -1.7073202522e-01 -1.8798548242e+00 +182 166 129 +3 0 521 -97.8600 -59.9500 1 451 -91.8000 -78.6800 2 495 -77.3100 -17.0200 +-4.3244656392e-01 -1.4154879783e-01 -1.9162887044e+00 +197 179 182 +3 0 530 -154.7900 -46.7500 1 458 -139.9700 -66.9800 2 685 -118.4100 -6.7000 +-2.1937476038e-01 -1.3029209823e-01 -1.9501620720e+00 +68 77 44 +2 0 536 -72.3100 -41.8600 1 639 -65.5900 -62.4300 +3.9823523906e-01 -1.2011956286e-01 -1.8895634871e+00 +129 162 75 +2 0 537 163.4300 -40.4300 1 469 157.3500 -57.2200 +1.9225296149e-01 -9.6006628980e-02 -2.0205882239e+00 +48 69 33 +2 0 541 76.8600 -29.1900 2 507 84.3300 12.6100 +2.5573637106e-01 -7.3119749000e-02 -2.0588003955e+00 +174 181 182 +2 0 548 96.3100 -20.8400 1 484 99.0600 -38.2200 +-2.5843144226e-01 -6.8517433581e-02 -1.9512032181e+00 +86 106 54 +2 0 551 -86.6800 -19.2500 1 653 -78.9600 -40.4600 +2.8819838679e-01 -6.1509428773e-02 -2.0482242005e+00 +74 102 46 +2 0 555 108.1700 -16.9200 2 522 116.7900 24.9000 +-2.7883815550e-01 -4.3357931548e-02 -1.9617920036e+00 +69 69 71 +2 0 563 -93.6500 -8.7500 1 661 -84.7100 -32.9200 +-3.2828744208e-01 2.1984182041e-02 -1.9844093833e+00 +192 191 197 +2 0 587 -110.0500 14.4600 1 506 -98.7700 -10.3100 +4.4098584084e-01 1.0160514348e-01 -2.1180918441e+00 +167 174 115 +2 0 599 152.1600 36.8800 1 523 156.8900 20.2500 +-1.5536580134e-01 1.2789007398e-01 -2.3545385550e+00 +93 91 94 +2 0 601 -39.5200 40.0900 1 519 -21.0900 17.6700 +-7.0937957659e-02 1.3096967100e-01 -2.3366200752e+00 +85 98 115 +3 0 605 -15.1700 42.4200 1 386 1.1800 19.0900 2 407 21.0400 76.4000 +8.6999541792e-02 1.9570240672e-01 -2.3009425793e+00 +230 199 170 +2 0 615 31.8500 61.3400 1 534 44.3300 39.9200 +4.1658721703e-01 2.7791103742e-01 -2.6865889988e+00 +215 198 157 +2 0 617 103.3200 67.6700 2 579 153.4800 111.7300 +3.5950707655e+00 1.5681861718e+00 -8.9703439120e+00 +254 255 251 +2 0 625 206.6300 86.8800 1 546 279.9300 82.2800 +2.2658734282e+00 1.7205396132e+00 -8.4096061722e+00 +113 136 140 +2 0 630 139.2900 105.2400 1 403 205.8200 97.1200 +2.5964497658e+00 1.8088831909e+00 -8.5627227943e+00 +104 120 116 +2 0 631 156.7900 108.0600 1 405 224.5300 101.2600 +2.6598918278e+00 1.8539591632e+00 -8.7600954446e+00 +104 120 116 +3 0 632 156.7900 108.0600 1 406 224.5300 101.2600 2 426 298.4100 172.0000 +-5.0093090704e-01 -2.5751560605e-01 -1.8129843553e+00 +153 157 168 +2 0 689 -193.8700 -97.1400 1 601 -177.3800 -112.4500 +-5.2862377544e-01 -2.5901636727e-01 -1.8451446616e+00 +108 117 87 +3 0 691 -200.1900 -94.9000 1 834 -181.1500 -110.9400 2 1304 -155.1400 -46.9400 +-6.1511021812e-01 -2.3384366513e-01 -1.8734393562e+00 +168 175 190 +2 0 706 -228.8400 -82.7200 2 1333 -174.0500 -36.9600 +-2.3534043564e-01 -2.1538456574e-01 -1.8002629031e+00 +134 150 77 +2 0 708 -86.4300 -82.1700 1 848 -85.7300 -98.8700 +-4.8735837919e-01 -2.2119748592e-01 -1.8432343719e+00 +103 135 56 +3 0 711 -184.4300 -80.5200 1 616 -168.3000 -97.7100 2 916 -144.4300 -34.8900 +-3.1081618315e-01 -2.0823480051e-01 -1.8031618337e+00 +32 43 28 +3 0 716 -117.6500 -78.4300 1 851 -112.9600 -95.8800 2 922 -98.6000 -33.0800 +-2.8131200676e-01 -1.8153888681e-01 -1.8523205693e+00 +66 89 34 +2 0 723 -101.8200 -65.5300 1 626 -96.6100 -83.4100 +-3.8831293510e-01 -1.8491916018e-01 -1.8983478184e+00 +108 106 111 +3 0 725 -139.9200 -63.8000 1 629 -127.5200 -82.7200 2 938 -107.9200 -21.6300 +3.7099421425e-02 -1.7944568347e-01 -1.8936005573e+00 +94 86 81 +3 0 728 25.2500 -62.0600 1 324 21.1300 -81.9300 2 356 26.0400 -20.2300 +-5.5251084153e-01 -1.6743381646e-01 -1.8678694320e+00 +111 140 66 +3 0 733 -206.2700 -57.6200 1 631 -185.7200 -77.3600 2 945 -159.3200 -16.5900 +2.9230116791e-01 -7.5249191663e-02 -2.0431837798e+00 +166 180 180 +2 0 757 109.9500 -21.6900 2 708 118.1600 20.2800 +8.2054411311e-01 -6.6551348434e-02 -2.3480808549e+00 +65 73 63 +2 0 760 234.7300 -17.5700 1 491 256.6200 -29.9400 +-3.8752826164e-02 -3.3086690522e-03 -1.8924522474e+00 +71 75 86 +2 0 788 -4.0600 5.4100 1 687 -7.4400 -16.4800 +-2.3875497631e-01 1.0729565085e-03 -1.9560720678e+00 +103 121 54 +2 0 790 -78.9100 6.8700 1 949 -72.2900 -16.5500 +-3.9277389806e-01 4.6713752641e-02 -2.0445484490e+00 +219 217 214 +3 0 806 -128.2800 22.1800 1 701 -112.6200 -3.3800 2 747 -91.8200 53.0400 +-1.4180510847e-01 1.8589906680e-01 -2.3716696769e+00 +177 182 178 +3 0 831 -35.2000 56.3200 1 725 -16.9900 32.7600 2 1112 4.4500 90.2800 +-3.6978195128e-01 1.9894746162e-01 -2.2295880486e+00 +230 228 228 +2 0 838 -107.7800 65.9200 2 763 -65.8400 93.7800 +-3.6978195128e-01 1.9894746162e-01 -2.2295880486e+00 +230 228 228 +2 0 839 -107.7800 65.9200 2 764 -65.8400 93.7800 +-4.4964515668e-01 1.9036766427e-01 -2.0402854231e+00 +254 254 255 +3 0 843 -147.4600 72.6600 1 1006 -130.5800 41.5200 2 1119 -109.1300 94.9100 +2.1576033102e+00 1.9503852621e+00 -8.8083221295e+00 +251 255 254 +2 0 862 125.6000 113.9300 1 549 191.7600 105.6700 +-1.6216580956e-01 -2.5884432299e-01 -1.8316958976e+00 +131 126 120 +3 0 942 -54.7400 -98.5000 1 831 -55.2300 -113.7300 2 1303 -46.2500 -49.3300 +3.4795763045e-01 -2.2252554564e-01 -1.6327328352e+00 +89 92 107 +4 0 943 180.4300 -96.4400 1 832 154.2700 -113.0700 2 881 135.7600 -51.7200 4 960 0.9900 -78.5300 +-3.4760513484e-01 -2.3767182220e-01 -1.8025082654e+00 +137 152 75 +2 0 948 -132.9100 -90.6700 1 1147 -126.0400 -106.8600 +-5.5463659727e-01 -2.4573958609e-01 -1.8590432251e+00 +183 181 184 +2 0 955 -208.2300 -88.8300 1 1151 -187.3600 -105.3800 +-9.1194507764e-03 -2.3213411906e-01 -1.8433003405e+00 +107 102 100 +2 0 956 7.6800 -87.0300 1 1156 2.6600 -103.1000 +-3.3525888339e-01 -2.2899322047e-01 -1.8015782865e+00 +40 48 21 +3 0 957 -127.9100 -87.1300 1 842 -121.8000 -103.6700 2 1318 -106.1300 -40.0900 +-1.5122482387e-03 -2.2556133810e-01 -1.8508688377e+00 +212 204 200 +3 0 962 10.6200 -83.8500 1 843 5.9000 -100.4200 2 1329 9.3700 -37.5600 +6.9306462117e-02 -2.2371850427e-01 -1.9300232371e+00 +145 145 144 +2 0 965 36.1400 -78.7000 2 1344 39.5500 -33.3600 +-6.2174812140e-01 -2.2229528906e-01 -1.8718196170e+00 +74 80 64 +2 0 966 -231.6000 -78.3600 1 852 -206.5600 -96.1600 +-2.1078505694e-01 -1.9931659804e-01 -1.8379764604e+00 +206 186 161 +3 0 974 -74.2500 -73.4100 1 1176 -72.8000 -91.0000 2 1358 -62.0600 -28.6000 +-1.3391924250e-01 -1.8453736237e-01 -1.9067662783e+00 +101 112 77 +2 0 993 -41.3400 -64.5100 1 868 -39.3600 -82.5800 +-3.5495644098e-01 -1.5355205616e-01 -1.8795696648e+00 +205 194 180 +2 0 1007 -128.7300 -52.8600 1 1212 -118.9300 -72.3700 +-4.0791825264e-01 -1.4536243191e-01 -1.9128155248e+00 +217 198 169 +2 0 1013 -145.9500 -48.3800 1 1218 -132.3000 -68.4400 +-1.4800810284e-01 -1.4094240434e-01 -1.9152803122e+00 +113 117 88 +2 0 1015 -46.4800 -47.4500 1 883 -43.9600 -66.5700 +-1.4839855269e-01 -1.4094959626e-01 -1.9188293908e+00 +113 117 88 +3 0 1016 -46.4800 -47.4500 1 884 -43.9600 -66.5700 2 1396 -33.1800 -5.7200 +-2.0784992302e-01 -1.2168887187e-01 -1.9500574024e+00 +184 186 172 +3 0 1023 -67.9700 -39.1200 1 888 -61.7900 -59.1000 2 1407 -48.4100 1.2100 +-2.0094110973e-01 -1.1078988640e-01 -1.9437628798e+00 +87 99 57 +3 0 1033 -65.6200 -35.3100 1 892 -60.0200 -55.4000 2 968 -47.1800 4.7400 +-2.7742359624e-01 -1.0746364355e-01 -1.9562612744e+00 +105 118 62 +3 0 1036 -93.5200 -33.5400 1 1242 -84.6400 -54.0000 2 1415 -68.6000 5.5000 +2.0726897416e-01 -7.9982721232e-02 -2.0585823773e+00 +147 163 155 +3 0 1053 79.7600 -23.1500 1 907 82.8500 -40.9600 2 993 90.3800 18.5600 +-3.8885808268e-02 -5.7833764245e-02 -1.8856590612e+00 +72 80 75 +2 0 1071 -4.2000 -15.9700 1 1286 -7.4800 -36.5100 +-2.7174250498e-01 -5.6969454163e-02 -1.9648887781e+00 +186 195 202 +3 0 1077 -90.8600 -14.5700 1 1287 -82.0300 -36.7400 2 1453 -66.5400 21.8000 +-2.7191803294e-01 -5.7064368549e-02 -1.9658261743e+00 +186 195 202 +2 0 1078 -90.8600 -14.5700 1 1288 -82.0300 -36.7400 +-1.8201428400e-01 -5.4103778219e-02 -1.9574999642e+00 +189 209 217 +3 0 1079 -57.9100 -13.8100 1 918 -52.8100 -35.3100 2 1460 -40.4600 23.2300 +-1.8212224940e-01 -5.4153512699e-02 -1.9583478925e+00 +189 209 217 +2 0 1080 -57.9100 -13.8100 1 919 -52.8100 -35.3100 +1.5911179739e-01 -5.1702344385e-02 -2.0732156386e+00 +194 177 161 +2 0 1081 62.6300 -13.2800 2 1023 75.2400 27.5400 +-2.9595131566e-01 -4.3702896346e-02 -1.9791141323e+00 +166 175 171 +3 0 1088 -98.8400 -9.4800 1 923 -88.6700 -32.1200 2 1470 -71.9500 25.7300 +-3.6185689265e-01 -3.5005433401e-03 -2.0141387693e+00 +61 60 50 +2 0 1114 -120.0400 4.9100 1 1350 -106.0800 -19.0200 +-3.6185689265e-01 -3.5005433401e-03 -2.0141387693e+00 +61 60 50 +2 0 1115 -120.0400 4.9100 1 1351 -106.0800 -19.0200 +-2.6856478371e-01 8.7121154691e-03 -1.9858923656e+00 +193 191 178 +2 0 1124 -88.3200 9.3600 1 1366 -79.3300 -14.2200 +1.7552485798e-01 3.1084545459e-02 -2.3032251741e+00 +111 106 107 +2 0 1130 57.7900 12.4100 1 698 71.0500 -6.7100 +1.7552485798e-01 3.1084545459e-02 -2.3032251741e+00 +111 106 107 +2 0 1131 57.7900 12.4100 1 699 71.0500 -6.7100 +-8.1168562793e-02 1.6796908237e-02 -1.9186463761e+00 +104 100 98 +2 0 1135 -20.3900 12.8500 1 1376 -21.2400 -9.5700 +-1.2616055567e-01 1.3564646970e-01 -2.3524172462e+00 +73 66 64 +2 0 1174 -31.0900 42.5200 1 986 -13.2900 19.8800 +2.0512684504e-02 1.4236617779e-01 -2.3155752318e+00 +72 65 59 +3 0 1177 11.8900 45.1200 1 990 25.9800 23.8600 2 1103 44.1500 81.8800 +-8.6914057457e-02 1.7843140511e-01 -2.3517484063e+00 +82 88 100 +3 0 1182 -19.5900 54.7200 1 996 -2.9400 31.9200 2 1110 17.3400 89.4700 +3.8882904940e-01 2.2495613196e-01 -2.6928499026e+00 +86 85 83 +2 0 1183 96.4300 54.8600 1 1003 120.6200 38.2800 +3.8882904940e-01 2.2495613196e-01 -2.6928499026e+00 +86 85 83 +2 0 1184 96.4300 54.8600 1 1004 120.6200 38.2800 +1.0041236337e-01 1.8517174852e-01 -2.2958852678e+00 +62 76 93 +3 0 1191 35.9800 58.3200 1 1447 48.1200 37.2900 2 1604 65.0100 95.3200 +-8.6890983754e-02 1.9825560511e-01 -2.3523828713e+00 +84 87 89 +2 0 1194 -19.5800 60.4700 1 1002 -2.9300 37.4000 +4.2602487500e-02 2.0502278445e-01 -2.3157213376e+00 +79 77 86 +2 0 1198 18.4600 63.4400 1 1454 31.8700 41.8200 +7.2475088579e-02 2.0628041326e-01 -2.3075865870e+00 +89 87 87 +3 0 1200 27.4400 64.1400 1 1008 40.1400 42.7000 2 1125 57.5700 100.4600 +4.3049349979e-01 3.0412674340e-01 -2.6848697266e+00 +118 109 110 +3 0 1203 106.6600 74.0700 1 1015 130.2200 57.7600 2 776 156.7800 118.0500 +3.0319700492e-01 3.1467903377e-01 -2.7527186122e+00 +214 203 197 +2 0 1205 73.6200 74.4300 1 1014 98.6200 56.6300 +4.2505427609e-01 3.3006568518e-01 -2.6953039077e+00 +111 127 108 +3 0 1208 104.6500 79.9700 1 1471 128.7100 63.1600 2 1135 155.0500 124.0700 +3.2664939894e+00 1.5365846826e+00 -8.5675026104e+00 +118 142 136 +2 0 1214 197.4600 89.9000 1 737 269.0200 84.9000 +2.7628382891e+00 1.6568565426e+00 -8.3995263947e+00 +127 150 163 +2 0 1221 170.6300 100.3600 1 1027 239.2900 94.1900 +4.0163670245e-01 -2.2958952750e-01 -1.6231874094e+00 +120 119 125 +2 0 1329 206.1100 -100.3500 1 1140 180.5700 -116.5500 +3.3741668520e-01 -2.2625518765e-01 -1.6273967043e+00 +168 171 178 +2 0 1333 176.2500 -98.8100 1 1141 149.9300 -115.2100 +-3.7053675568e-01 -2.4917896248e-01 -1.8121048519e+00 +67 72 53 +2 0 1341 -141.3600 -94.6600 1 1145 -132.7200 -110.4400 +-2.7929422964e-01 -2.3283749538e-01 -1.7905874318e+00 +58 62 39 +3 0 1348 -105.4800 -89.6700 1 1149 -103.0000 -106.0800 2 1309 -90.1000 -42.3200 +-1.7859682100e-01 -2.3252375978e-01 -1.8391057751e+00 +160 172 172 +3 0 1352 -61.1500 -86.9800 1 1155 -60.7500 -103.4100 2 1317 -51.0700 -40.0300 +-5.0597583877e-02 -2.0751296720e-01 -1.6859251885e+00 +105 105 107 +3 0 1355 -8.7000 -86.5400 1 1157 -23.7900 -103.1000 2 1316 -25.7200 -39.9300 +-1.9366741390e-01 -2.2889165669e-01 -1.8322607424e+00 +57 78 51 +2 0 1357 -67.5700 -85.9400 2 1323 -56.8700 -38.9300 +-1.6185102773e-01 -2.2910144331e-01 -1.8422801450e+00 +166 177 186 +3 0 1358 -54.2100 -85.3400 1 1159 -54.4200 -101.9700 2 1322 -45.2400 -38.9100 +-1.7511799499e-02 -2.2662895809e-01 -1.8708903438e+00 +215 206 207 +3 0 1361 3.7600 -84.0900 1 319 1.3300 -99.0300 2 474 5.4000 -36.4500 +3.9491741692e-01 -1.9070877442e-01 -1.6410361167e+00 +176 181 190 +2 0 1365 200.0700 -80.8500 1 850 175.4400 -97.2600 +5.0933848413e-01 -1.9974982509e-01 -1.8128926350e+00 +171 180 198 +2 0 1377 216.2300 -74.3400 1 1177 208.0900 -90.2600 +5.0933848413e-01 -1.9974982509e-01 -1.8128926350e+00 +171 180 198 +2 0 1378 216.2300 -74.3400 1 1178 208.0900 -90.2600 +5.1376079396e-01 -1.8475254403e-01 -1.8183015257e+00 +81 96 72 +3 0 1394 217.1000 -68.0000 1 1192 209.1200 -83.9100 2 1366 203.0300 -25.0500 +-3.0152234341e-01 -1.6466233273e-01 -1.8747496170e+00 +195 193 147 +2 0 1412 -108.2200 -57.5900 1 1201 -101.1600 -76.5900 +-3.0320411706e-01 -8.0809613282e-02 -1.9710333737e+00 +94 102 61 +2 0 1489 -102.1100 -23.3100 1 1259 -91.6200 -44.7800 +6.4468842282e-01 -7.3548815057e-02 -2.2105491943e+00 +198 190 167 +2 0 1503 203.6700 -20.0000 1 1297 217.2000 -34.4400 +3.3388586602e-01 -6.5943788975e-02 -2.0377098731e+00 +167 174 187 +3 0 1510 124.7000 -18.4600 1 1290 126.0800 -35.4900 2 1005 132.2100 23.3700 +7.4859211978e-01 -6.7508165826e-02 -2.2898663056e+00 +190 179 168 +2 0 1515 222.9700 -17.9500 1 1319 241.4200 -31.1700 +4.1359384706e-01 -6.1878745650e-02 -2.0192715432e+00 +172 181 186 +4 0 1521 153.9000 -17.0600 1 921 154.6300 -33.9500 2 1009 159.3400 24.9800 4 1071 81.6000 -4.4400 +1.5459109304e-01 -6.1431704381e-02 -2.0743575123e+00 +83 83 71 +2 0 1522 61.0200 -16.5600 2 1466 73.9100 24.3700 +-8.9186022107e-02 -7.1038539985e-02 -2.5233505293e+00 +112 108 100 +2 0 1524 -19.9900 -16.6100 1 1291 3.8300 -35.6200 +-2.9714375102e-01 -6.2076509390e-02 -1.9774972684e+00 +172 174 177 +2 0 1526 -99.4400 -16.3300 1 1283 -89.0800 -38.3900 +2.7388890521e-01 -5.7945920400e-02 -2.0386508532e+00 +99 115 73 +2 0 1530 104.0300 -15.6800 1 1307 105.5400 -33.2200 +-1.9960461699e-01 -5.3148798976e-02 -1.9483763788e+00 +111 117 91 +2 0 1545 -64.7900 -13.4500 1 1295 -59.5500 -35.1200 +2.9333530185e-01 -5.0281789621e-02 -2.0495376381e+00 +200 185 176 +2 0 1548 109.8300 -13.0200 2 1482 118.4700 28.6400 +6.4137058496e-01 -4.3820576154e-02 -2.2268892819e+00 +159 157 155 +2 0 1557 200.7200 -10.6200 1 1333 214.5300 -25.4600 +4.5189081773e-01 -3.2103638937e-02 -2.0199354467e+00 +158 145 125 +2 0 1565 166.5600 -6.8700 1 1335 167.8000 -23.2000 +4.6425067130e-01 -1.7906355072e-02 -2.0262904029e+00 +218 202 201 +4 0 1587 170.0400 -1.9800 1 1354 171.5400 -18.0900 2 1516 177.0100 40.7600 4 775 96.0000 10.8400 +4.6401497534e-01 -1.7799781296e-02 -2.0250750993e+00 +218 202 201 +2 0 1588 170.0400 -1.9800 1 1355 171.5400 -18.0900 +-1.1244760931e-01 1.4962022794e-02 -1.9249062398e+00 +87 84 83 +2 0 1630 -32.2900 12.2800 1 1370 -31.7900 -10.6800 +-1.1244760931e-01 1.4962022794e-02 -1.9249062398e+00 +87 84 83 +2 0 1631 -32.2900 12.2800 1 1371 -31.7900 -10.6800 +-4.0790867612e-02 6.5902196683e-02 -1.9161494043e+00 +243 198 180 +2 0 1665 -4.8100 31.6700 1 1404 -7.3800 8.3500 +-1.5629867677e-01 1.7445390228e-01 -2.3737988813e+00 +119 114 119 +2 0 1714 -39.3900 53.0200 2 1593 0.9700 86.7900 +3.5056500710e-01 2.2275671616e-01 -2.7113182490e+00 +86 82 81 +3 0 1718 86.4900 53.8600 1 1442 111.0800 37.0500 2 1607 137.6400 97.0500 +3.4382659658e+00 1.2608935933e+00 -8.4309855705e+00 +114 131 114 +2 0 1753 211.5300 73.7700 1 1475 284.7600 68.9100 +-3.6545013236e-02 4.3147166703e-01 -3.0249317851e+00 +251 255 255 +2 0 1775 -6.5200 90.4800 1 1476 25.2000 69.4400 +2.9656712890e+00 1.7175185691e+00 -8.8090245979e+00 +244 255 255 +2 0 1782 173.9200 98.5700 1 1026 243.7200 92.6900 +2.8003840297e+00 1.8162586690e+00 -8.7126297462e+00 +246 255 255 +3 0 1783 166.1300 105.9100 1 1488 234.6500 99.9000 2 1146 309.7700 171.0800 +2.7272162527e+00 1.8256905236e+00 -8.6852305675e+00 +144 167 172 +3 0 1784 162.3600 107.1200 1 1489 230.3200 100.7200 2 1640 304.9700 171.7400 +2.0214024246e+00 1.9591551936e+00 -8.8957122689e+00 +245 253 253 +3 0 1790 116.0700 113.8600 1 1495 181.8600 104.5400 2 1643 250.2800 173.4800 +-5.3795359839e-02 -2.5498810336e-01 -1.8347700340e+00 +93 93 94 +2 3 6 -66.7400 -61.3100 2 7 -9.7500 -48.2800 +1.2081163536e-02 -2.8107078607e-01 -1.8939132291e+00 +71 71 71 +2 3 19 -38.5200 -66.6700 2 19 18.1400 -54.5800 +-3.4481530111e-01 -1.0147829901e-01 -1.9863895590e+00 +68 82 64 +3 3 34 -123.9600 -8.1500 1 19 -103.3500 -50.2100 2 41 -84.3100 6.5100 +1.1916302537e-01 -7.3121180931e-03 -2.0708596270e+00 +70 96 40 +2 3 35 9.5500 24.5900 2 46 62.1600 40.7800 +-4.2372197496e-01 1.5595621142e-01 -2.0379312862e+00 +218 207 196 +2 3 68 -137.6500 64.5100 2 49 -102.2600 84.7200 +9.6443405404e-01 1.9028347652e+00 -8.5630720375e+00 +255 255 253 +3 3 72 196.1500 158.2000 1 86 117.8000 103.5800 2 85 179.7100 170.4400 +1.5535700020e-03 -2.0962115046e-01 -1.8588033456e+00 +210 201 188 +4 3 77 -47.3600 -45.0900 1 58 6.9400 -93.8300 2 100 11.4400 -31.4800 4 95 -48.6200 -62.3000 +8.4730569915e-02 -5.9025662846e-02 -2.0430654415e+00 +216 201 166 +3 3 89 -2.6700 7.6700 2 118 50.2300 24.6500 4 65 -1.3100 -10.8400 +6.8165647223e-02 6.4742406410e-03 -1.9910810647e+00 +226 203 170 +2 3 96 -13.5500 27.3000 2 122 41.2300 45.7600 +-4.6266872600e-01 4.6846460296e-02 -1.9801465695e+00 +108 121 65 +2 3 99 -154.9300 32.8400 2 124 -119.8300 52.9700 +3.3371652581e-01 -2.1650430210e-01 -1.6323912686e+00 +139 141 138 +2 3 124 40.3500 -62.6600 2 98 129.4700 -48.7000 +6.3909177384e-02 -1.0669189022e-01 -1.9516556930e+00 +216 193 152 +4 3 135 -18.1400 -8.4200 1 340 32.4300 -53.2600 2 250 38.4100 7.0000 4 104 -20.1200 -26.6500 +1.1969865762e-01 9.3493167575e-02 -2.2751785004e+00 +81 109 102 +2 3 151 26.2100 52.0100 2 183 70.2500 70.0000 +-2.9931040573e-02 1.6419020470e-01 -2.3369834993e+00 +233 196 151 +3 3 153 -7.0600 67.6500 1 182 12.2700 28.7500 2 187 31.7300 86.9200 +5.0877600166e-01 -2.0252429093e-01 -1.8168751963e+00 +178 175 161 +3 3 200 122.6200 -46.4900 1 128 207.4900 -91.2300 2 240 200.9600 -32.0500 +5.0877600166e-01 -2.0252429093e-01 -1.8168751963e+00 +178 175 161 +3 3 201 122.6200 -46.4900 1 129 207.4900 -91.2300 2 241 200.9600 -32.0500 +-3.2570200098e-01 -1.6021965753e-01 -1.9080918432e+00 +179 154 127 +3 3 214 -131.1100 -26.8100 1 136 -105.4400 -73.9700 2 156 -88.7400 -12.7000 +4.5794476859e-02 -1.0684592990e-01 -1.9097032570e+00 +101 111 107 +4 3 219 -28.8900 -8.9100 1 339 24.8500 -55.1100 2 249 29.4300 5.4400 4 170 -31.7700 -27.2600 +-1.3943628508e-01 -7.8433071008e-02 -1.9270030409e+00 +213 201 176 +3 3 224 -79.9900 -0.9500 1 103 -40.5100 -44.0900 2 165 -30.1700 15.0100 +8.4649807286e-01 -8.7013703527e-02 -2.3524943200e+00 +209 192 160 +4 3 232 236.4500 7.8900 1 244 263.8400 -36.0600 2 259 285.6200 22.8100 4 187 244.2300 4.7900 +5.9126336028e-01 -6.1248272912e-02 -2.1758359372e+00 +188 161 128 +2 3 236 158.9300 12.1900 2 377 215.3300 27.7300 +3.4185208974e-01 -3.6784771159e-02 -2.0410772240e+00 +206 186 155 +3 3 243 74.7300 16.8700 1 249 128.5700 -25.6300 2 270 134.7700 33.3500 +3.9702548528e-02 3.5482317373e-03 -1.9304336040e+00 +71 75 74 +3 3 250 -29.0800 26.2900 1 258 22.4200 -13.6100 2 273 27.7500 44.4200 +3.9702548528e-02 3.5482317373e-03 -1.9304336040e+00 +71 75 74 +3 3 252 -29.0800 26.2900 1 259 22.4200 -13.6100 2 274 27.7500 44.4200 +-4.3874900998e-01 1.6168208698e-01 -2.0279901573e+00 +168 165 154 +2 3 269 -142.7700 65.7700 2 189 -107.7400 86.6100 +2.6347045165e-02 1.6301951933e-01 -2.3202497853e+00 +225 196 151 +3 3 270 5.1900 68.6700 2 292 45.9200 87.5500 4 211 19.5400 48.4100 +6.2969426721e-02 1.9350779684e-01 -2.3001035662e+00 +150 144 147 +4 3 274 12.7100 77.4700 1 279 37.5000 39.3100 2 297 54.5700 97.0900 4 308 23.8500 57.6700 +1.1997091293e-01 1.9702508987e-01 -2.2918643003e+00 +79 83 82 +2 3 277 26.6300 79.8700 2 299 70.2300 99.0700 +4.9014807460e-01 -2.0796051188e-01 -1.8216137287e+00 +42 52 28 +3 3 327 115.6900 -48.4800 1 321 199.4800 -93.0200 2 346 193.3100 -33.9500 +-1.5659414468e-01 -1.4910946556e-01 -1.9172669476e+00 +118 119 74 +2 3 341 -85.2100 -23.2800 1 231 -46.7500 -69.4600 +-3.5769471632e-01 -1.3308105925e-01 -1.9015964474e+00 +108 119 51 +3 3 342 -140.6400 -18.8900 1 232 -117.0500 -64.6000 2 246 -99.5500 -4.1700 +4.2077081536e-01 -7.1196907740e-02 -2.0151333946e+00 +182 183 171 +2 3 350 99.1500 5.8800 2 372 162.2100 22.0200 +-1.9980816581e-01 1.6514975067e-02 -1.9589631273e+00 +212 197 181 +2 3 360 -92.6800 27.8000 2 277 -46.4600 45.9300 +9.1264482795e-02 9.7553507057e-03 -2.0597760443e+00 +219 203 157 +2 3 362 0.3100 29.2200 2 539 52.6300 46.2400 +9.1264482795e-02 9.7553507057e-03 -2.0597760443e+00 +219 203 157 +2 3 363 0.3100 29.2200 2 540 52.6300 46.2400 +4.6111177364e-02 3.0664282809e-02 -1.9301572093e+00 +225 197 159 +3 3 369 -27.6800 35.2700 2 391 29.7300 53.6200 4 277 -32.1200 12.6100 +1.5648853481e-01 9.0396065416e-02 -2.2740677777e+00 +215 200 195 +4 3 380 36.0500 51.6900 1 374 64.7700 11.0000 2 399 80.9000 69.6100 4 288 43.8800 34.3200 +2.1285098804e-01 1.4142103070e-01 -2.3860223715e+00 +95 92 82 +3 3 390 57.4800 64.5000 2 567 99.1700 82.4000 4 301 68.9600 49.1200 +1.5572151023e-01 1.9470838504e-01 -2.2984488431e+00 +221 209 192 +3 3 394 36.4300 79.5000 1 392 63.9700 40.6900 2 417 80.7000 98.7600 +1.1731437077e-01 2.2800503675e-01 -2.2913883741e+00 +216 189 153 +2 3 400 25.6300 88.1600 2 303 69.2300 107.8800 +-5.1821283612e-01 -2.8146532961e-01 -1.8299745965e+00 +36 36 33 +3 3 447 -190.7200 -64.8800 1 209 -179.9300 -120.1700 2 334 -154.7500 -54.5100 +7.6527452742e-01 -2.6053241349e-01 -1.9696999625e+00 +22 23 25 +2 3 450 220.9900 -59.8500 2 466 290.9600 -45.8700 +5.6019338338e-01 -1.7392909147e-01 -1.8309832130e+00 +129 155 80 +2 3 463 142.7300 -34.7800 2 492 221.3100 -20.4300 +-2.8980600757e-01 -1.7966274740e-01 -1.8655932034e+00 +86 119 34 +4 3 467 -129.3900 -33.7000 1 448 -97.8200 -83.0000 2 490 -83.4200 -20.8900 4 510 -109.7000 -53.9700 +5.5102747173e-01 -1.5582497619e-01 -1.8266371519e+00 +42 45 34 +2 3 476 138.9400 -28.2900 2 361 217.8400 -13.3200 +-5.1638825524e-02 -1.4048438430e-01 -1.8753013994e+00 +176 180 185 +2 3 482 -61.7200 -21.8200 2 500 -6.2500 -6.8300 +-5.2680324661e-02 -1.4080025228e-01 -1.8777497559e+00 +176 180 185 +3 3 483 -61.7200 -21.8200 1 334 -12.4400 -67.5400 2 501 -6.2500 -6.8300 +-5.2680324661e-02 -1.4080025228e-01 -1.8777497559e+00 +176 180 185 +3 3 484 -61.7200 -21.8200 1 335 -12.4400 -67.5400 2 502 -6.2500 -6.8300 +-5.2787416434e-01 -1.3404005596e-01 -1.9114985818e+00 +102 120 66 +2 3 485 -179.9200 -19.0600 2 367 -145.7700 -4.6800 +-3.5745992971e-01 -1.1761061670e-01 -1.9448543130e+00 +162 156 145 +3 3 492 -133.5000 -12.8300 2 505 -93.3900 1.8700 4 705 -109.1400 -36.5500 +2.9914270069e-01 -7.9329138510e-02 -2.0318610370e+00 +172 172 162 +3 3 500 61.0700 2.5700 2 371 120.3400 19.0800 4 385 51.6700 -12.5800 +6.1447850762e-01 -6.4067152133e-02 -2.1964793188e+00 +190 172 139 +2 3 509 166.4700 11.6500 2 523 221.9800 27.1600 +4.8007761610e-01 -1.5080818637e-02 -2.0192824137e+00 +45 44 37 +2 3 521 118.6200 24.8400 1 358 177.5000 -16.7600 +1.2668209261e-01 6.3405866600e-02 -2.2627315368e+00 +228 197 159 +2 3 540 27.3600 43.8900 2 555 71.9700 61.7000 +-4.3560148815e-01 1.0556824021e-01 -2.0157446012e+00 +187 175 155 +2 3 543 -143.4000 50.0700 2 559 -108.0200 70.2900 +8.4510959520e-02 9.1409635087e-02 -2.2801700841e+00 +233 201 155 +4 3 544 17.6200 50.9200 1 375 43.8300 10.5400 2 398 60.7000 69.0700 4 564 28.4700 32.4200 +5.6229721036e-01 7.7552089035e-02 -2.0955091897e+00 +32 34 28 +2 3 548 147.2900 56.1300 2 565 208.6500 73.1400 +-6.9465761806e-02 1.5686047948e-01 -2.3337310259e+00 +229 201 161 +4 3 552 -16.5800 65.2200 1 527 1.2200 26.6900 2 570 21.0400 84.3500 4 574 1.5400 43.7900 +-3.7037787769e-01 1.9337328164e-01 -2.2281104774e+00 +231 224 214 +2 3 555 -98.5400 71.7600 1 390 -88.4700 36.9300 +-3.7037787769e-01 1.9337328164e-01 -2.2281104774e+00 +231 224 214 +2 3 556 -98.5400 71.7600 1 391 -88.4700 36.9300 +-3.0033184561e-01 -1.8336041609e-01 -1.8430547263e+00 +44 52 28 +2 3 641 -135.5500 -36.0000 2 488 -89.7600 -22.8900 +-5.7654098631e-01 -1.1738298259e-01 -1.9160339203e+00 +76 81 46 +2 3 659 -190.4100 -14.0900 2 690 -158.3300 0.2900 +7.2062060802e-02 -7.6410096956e-02 -1.9947302179e+00 +83 85 84 +2 3 670 -11.1200 1.4700 2 703 43.4000 18.2300 +-1.7630622424e-01 -6.6540791549e-02 -1.9461817197e+00 +78 75 61 +4 3 674 -87.2400 2.9200 1 480 -51.8500 -40.3000 2 515 -39.6800 18.9600 4 1047 -75.3900 -19.2200 +4.0719069276e-02 -6.3435298683e-02 -1.9202459177e+00 +62 67 61 +3 3 676 -29.2800 4.1200 1 916 22.9000 -37.4800 2 1001 28.0200 21.3000 +9.3071129121e-01 -7.5708297774e-02 -2.4384315510e+00 +79 77 70 +4 3 684 259.1900 12.1600 1 489 278.3400 -31.4900 2 375 303.6100 27.6600 4 408 273.0800 11.1800 +3.4231739886e-01 -2.8761951890e-02 -2.0439540876e+00 +207 195 169 +3 3 697 75.0100 19.5400 1 497 128.5900 -22.8900 2 722 134.8300 35.9800 +-3.3255713599e-01 2.2322478606e-02 -1.9945429514e+00 +196 192 184 +2 3 703 -121.1700 28.0000 2 541 -81.3100 46.2700 +4.6683241784e-01 2.8477841678e-03 -2.0301574028e+00 +217 207 186 +4 3 706 114.3800 30.8800 1 694 171.9700 -10.9300 2 740 177.8700 47.8200 4 550 96.5600 17.8300 +2.1435675461e-02 4.8068948866e-02 -1.8831293717e+00 +205 188 175 +4 3 713 -41.4100 41.1800 1 703 13.7200 2.8000 2 552 17.6300 59.5500 4 782 -47.1500 16.9900 +5.9840955335e-02 9.9734338548e-02 -2.2897961642e+00 +126 119 111 +4 3 724 11.9700 52.8200 1 514 36.8500 12.5500 2 560 54.1400 70.5400 4 801 24.0900 34.2300 +5.9840955335e-02 9.9734338548e-02 -2.2897961642e+00 +126 119 111 +4 3 725 11.9700 52.8200 1 515 36.8500 12.5500 2 561 54.1400 70.5400 4 802 24.0900 34.2300 +1.2397717978e-01 1.7112957997e-01 -2.2917975383e+00 +227 195 155 +3 3 742 27.9300 72.9200 1 530 55.0700 33.6600 2 574 71.4700 91.8100 +-1.6138051457e-01 2.6877497277e-01 -2.4062988730e+00 +249 255 251 +2 3 749 -32.2500 90.2700 2 771 1.2200 110.2400 +-1.1975508628e-01 2.7008899438e-01 -2.3781824460e+00 +255 251 250 +2 3 750 -25.3700 91.8600 2 772 9.8200 112.5300 +5.1143977031e-01 3.0581060267e-01 -2.8772168582e+00 +224 198 170 +2 3 751 144.4300 96.6400 2 1129 174.0600 113.6900 +-1.3214732001e-01 4.0939651163e-01 -2.9848083083e+00 +114 130 120 +2 3 755 16.0400 105.5500 2 780 36.8700 124.1000 +6.1029091209e-01 1.7219348187e+00 -8.6606988962e+00 +110 116 121 +4 3 761 172.6700 142.5000 1 740 94.5000 90.0200 2 788 155.7700 155.1700 4 615 274.5600 151.9000 +-5.3723882524e-01 -2.6611569821e-01 -1.8405965566e+00 +214 206 207 +3 3 846 -193.5000 -59.6500 1 598 -184.6700 -113.7000 2 887 -158.2000 -49.0700 +-2.1434145467e-01 -2.4197464021e-01 -1.8048558006e+00 +38 49 19 +3 3 850 -118.0400 -56.5600 1 835 -77.2000 -109.1600 2 894 -67.1000 -45.0000 +6.2019946285e-03 -2.2437378970e-01 -1.8483951174e+00 +213 199 187 +2 3 862 -47.1300 -50.5300 1 611 8.6400 -100.0100 +-1.7608255675e-02 -2.2505205303e-01 -1.8594595025e+00 +216 201 182 +2 3 863 -52.7700 -49.9000 2 909 4.6400 -37.1600 +-2.2387371782e-01 -1.9429724308e-01 -1.8438612551e+00 +104 97 57 +2 3 881 -114.7200 -39.8300 2 667 -65.5300 -26.5500 +-5.2489013067e-01 -1.7108898861e-01 -1.8921912881e+00 +75 77 63 +2 3 894 -182.2700 -30.1900 2 677 -147.5700 -16.8400 +5.3300200966e-01 -1.5738063947e-01 -1.8422929406e+00 +175 177 166 +2 3 902 132.4900 -28.4200 2 951 209.6900 -13.2000 +-4.7005847410e-01 -1.4118144335e-01 -1.9284380834e+00 +87 82 46 +2 3 904 -163.2800 -20.2500 2 686 -127.1300 -6.7200 +6.9270879687e-02 -1.1012774840e-01 -1.9897382733e+00 +201 187 153 +3 3 911 -12.0900 -8.8600 2 971 42.4300 6.7800 4 711 -12.2500 -27.0300 +8.9589073591e-01 -1.0014471533e-01 -2.3857884921e+00 +205 184 158 +2 3 920 250.4200 4.8800 2 996 298.1200 19.4200 +7.4784497181e-01 -8.4017852766e-02 -2.2833171081e+00 +217 198 174 +2 3 923 207.8400 7.4000 2 1003 259.8000 22.5000 +2.4567390226e-01 -6.0846346691e-02 -2.0551967304e+00 +73 86 50 +2 3 925 46.1400 8.6300 1 659 95.6300 -34.3900 +8.4407669342e-01 -7.6671490315e-02 -2.3447775549e+00 +207 187 163 +2 3 930 235.9800 11.0500 2 1011 285.5200 25.5000 +5.5365229284e-01 -5.8667180366e-02 -2.1216617307e+00 +210 191 157 +2 3 933 146.0600 12.0800 2 1025 205.0500 28.0600 +2.5401846043e-01 -4.9689369314e-02 -2.0597701121e+00 +207 181 143 +3 3 934 48.8500 12.0200 1 927 98.1700 -30.4200 2 1026 105.7800 28.6400 +6.0160567687e-01 -4.6372655393e-02 -2.1788208335e+00 +75 81 47 +2 3 942 162.1300 17.1300 2 1034 218.5800 32.3000 +-3.6988745537e-01 1.2667035115e-02 -2.0220659739e+00 +122 113 99 +3 3 956 -125.8000 24.6100 1 956 -107.7800 -13.7300 2 1052 -88.1500 42.9600 +-3.0459233624e-01 2.6601859047e-02 -1.9948124435e+00 +193 190 179 +2 3 960 -114.2700 29.7800 2 743 -73.3100 47.7000 +-2.2038799287e-01 4.5327314268e-02 -1.9720779169e+00 +96 82 71 +2 3 965 -96.4500 36.0300 2 1067 -51.5000 55.0700 +2.0321016732e-01 9.2815712537e-02 -2.3785231189e+00 +74 82 80 +2 3 986 54.9200 51.5200 2 1567 96.6800 69.3600 +2.0321016732e-01 9.2815712537e-02 -2.3785231189e+00 +74 82 80 +2 3 987 54.9200 51.5200 2 1568 96.6800 69.3600 +2.1962954088e-01 1.4048498340e-01 -2.3946093656e+00 +100 94 82 +2 3 998 59.6700 64.1200 2 1101 101.2400 82.1600 +2.1886955050e-01 1.4031070444e-01 -2.4006420170e+00 +100 94 82 +3 3 999 59.6700 64.1200 2 1102 101.2400 82.1600 4 819 72.1300 48.5700 +-4.1438583126e-01 1.6230403850e-01 -2.0108561074e+00 +162 160 154 +3 3 1003 -139.7900 66.4500 1 727 -123.1300 34.5100 2 761 -103.5500 87.7800 +-5.4483540540e-02 1.7831551775e-01 -2.3288482160e+00 +82 79 74 +2 3 1006 -13.7600 71.1200 2 1111 24.4400 90.2900 +1.7531187805e-01 1.8437553671e-01 -2.3174664282e+00 +93 103 112 +2 3 1015 43.0000 76.7700 2 1120 86.7700 95.5500 +2.0841245518e-01 1.9930386435e-01 -2.3981245879e+00 +118 116 124 +3 3 1018 56.6600 79.3800 2 1608 97.7800 98.2900 4 832 67.8400 62.8700 +-2.3905601796e-02 -2.2260943216e-01 -1.8497907531e+00 +60 51 36 +3 3 1205 -55.9900 -49.6600 1 1162 -2.5700 -99.9300 2 913 1.4300 -36.0000 +4.0263009288e-01 -1.7824090926e-01 -1.6499912276e+00 +202 206 207 +3 3 1211 69.8300 -45.7600 1 1175 178.0400 -90.8400 2 1355 160.5800 -31.3300 +2.9432498985e-01 -1.8372262640e-01 -1.7835216551e+00 +98 123 68 +2 3 1225 40.2300 -40.5900 2 1363 115.0400 -26.1900 +4.4191133485e-01 -1.8767022151e-01 -1.8531550887e+00 +74 69 53 +2 3 1233 99.1300 -39.0800 2 1368 173.1200 -24.4800 +4.4191133485e-01 -1.8767022151e-01 -1.8531550887e+00 +74 69 53 +2 3 1234 99.1300 -39.0800 2 1369 173.1200 -24.4800 +-5.5356734325e-01 -1.6773086832e-01 -1.8697506827e+00 +117 132 64 +3 3 1254 -193.2100 -29.9000 1 632 -185.7200 -77.3600 2 946 -159.3200 -16.5900 +-4.1133877460e-01 -1.2195052950e-01 -1.9648691957e+00 +228 209 194 +3 3 1272 -143.3500 -14.4400 1 641 -126.9200 -58.8900 2 965 -105.6300 0.6700 +4.6900354197e-02 -8.2678992456e-02 -1.9297235099e+00 +230 209 178 +3 3 1286 -26.0200 -1.4700 1 1262 25.6300 -44.4700 2 1438 30.8700 14.6300 +-2.5626139858e-01 -6.5228519029e-02 -1.9486082267e+00 +90 107 54 +2 3 1295 -108.0600 2.8500 2 994 -63.8100 18.7800 +-2.6636215025e-01 -5.6622927051e-02 -1.9518118695e+00 +176 186 164 +2 3 1301 -110.2600 5.1000 2 1454 -66.5400 21.8000 +3.8441252290e-01 -6.2407184059e-02 -2.0297936468e+00 +171 173 164 +2 3 1309 87.9600 8.6500 1 1302 144.0300 -34.0100 +4.3992512267e-01 -5.8611575466e-02 -1.9410370810e+00 +174 176 177 +3 3 1310 104.8500 9.6200 2 1476 168.5300 25.8800 4 1070 77.1200 -5.0600 +4.7966419122e-01 -5.9842928032e-02 -2.0102323812e+00 +170 171 161 +2 3 1312 118.5000 10.1300 2 1475 183.1100 26.1300 +9.4692749680e-01 -8.1651054290e-02 -2.4373436053e+00 +202 191 168 +3 3 1316 263.3300 11.0100 2 1468 309.3600 25.4300 4 773 278.4100 10.1900 +9.4692749680e-01 -8.1651054290e-02 -2.4373436053e+00 +202 191 168 +3 3 1317 263.3300 11.0100 2 1469 309.3600 25.4300 4 774 278.4100 10.1900 +-3.1125292919e-01 1.7715550982e-02 -1.9968058190e+00 +197 196 183 +2 3 1367 -115.5000 26.9200 2 1526 -74.8900 45.0500 +2.2861739842e-01 1.2895850179e-01 -2.4500941782e+00 +96 103 68 +3 3 1443 65.0700 60.4400 1 1417 84.4300 18.8800 2 1581 104.9200 78.0000 +2.2861739842e-01 1.2895850179e-01 -2.4500941782e+00 +96 103 68 +3 3 1444 65.0700 60.4400 1 1418 84.4300 18.8800 2 1582 104.9200 78.0000 +-3.4063949397e-01 1.5059850258e-01 -2.2308644444e+00 +226 222 214 +2 3 1446 -91.0900 60.9900 2 1586 -57.8500 80.8100 +-3.9881262872e-01 1.5342723424e-01 -2.2060113563e+00 +219 217 212 +3 3 1447 -107.5300 61.3200 1 1425 -98.4000 26.0200 2 1587 -74.9900 81.0100 +-4.0145740173e-01 1.6201443013e-01 -2.0290054834e+00 +185 175 166 +3 3 1462 -133.9600 66.3600 1 998 -117.2400 33.9600 2 1109 -97.2700 87.4000 +-8.4786800244e-02 1.9760584723e-01 -2.3451384171e+00 +108 103 93 +2 3 1473 -19.6600 75.1200 2 1603 17.3000 94.7500 +-4.0350416560e-02 2.6829173380e-01 -2.8320593004e+00 +72 72 66 +2 3 1484 25.6500 82.8300 1 1005 21.6300 40.4000 +2.1657958847e+00 1.9879242318e+00 -8.9880829272e+00 +245 255 252 +2 3 1509 277.1900 164.1400 2 1148 258.9200 174.7200 +-2.8014032010e-01 -4.6373841835e-02 -1.9811905548e+00 +112 114 112 +2 1 4 -83.2300 -33.8200 2 4 -67.1600 26.1200 +3.4879249194e-01 -6.4816765558e-02 -2.0467923182e+00 +166 180 184 +2 1 20 131.0300 -35.0900 2 21 137.1900 24.1600 +4.5687621803e-01 -5.6489740905e-02 -2.0432083276e+00 +100 109 112 +2 1 22 168.2800 -31.4600 2 23 174.2900 27.5700 +-1.1335759744e-01 -2.8023879892e-01 -1.8537468843e+00 +82 79 86 +2 1 36 -35.2300 -120.7000 2 35 -27.4400 -55.8500 +1.1458436371e-01 -9.2432735264e-03 -2.1019550322e+00 +47 56 50 +2 1 46 51.6800 -18.1800 2 47 62.1600 40.7800 +3.9568549947e-01 -1.7826343826e-01 -1.6646370455e+00 +143 150 170 +2 1 60 173.5600 -89.9300 2 67 157.1800 -30.6200 +-4.7810061201e-02 -1.9020519732e-01 -1.8799378513e+00 +35 31 32 +2 1 62 -10.1100 -86.4600 2 104 -4.0200 -23.1300 +-5.7223162113e-01 -1.2665554745e-01 -1.8778230552e+00 +40 53 34 +2 1 67 -190.5400 -62.8300 2 109 -163.6000 -3.3300 +8.3055524935e-02 -1.3481356346e-01 -1.9899724508e+00 +63 69 55 +2 1 98 40.2900 -61.2700 2 110 47.1700 -1.7700 +-7.8573865117e-01 -9.7507356900e-02 -1.9941131001e+00 +149 187 207 +2 1 99 -233.7100 -51.1600 2 113 -197.6400 7.5800 +-2.1666945272e-02 -7.6756215802e-02 -1.8815949509e+00 +46 53 68 +2 1 104 -1.2600 -43.3400 2 116 3.8000 15.4200 +7.9285975196e-02 -5.7532486674e-02 -2.0758005326e+00 +215 199 193 +2 1 106 40.1000 -33.7000 2 119 50.2300 24.6500 +-1.7179476962e-01 -2.7696960004e-01 -1.8219350362e+00 +49 48 45 +2 1 122 -59.4900 -121.2100 2 151 -50.4100 -56.5400 +-3.1988389545e-02 2.1211685192e-01 -2.3342184056e+00 +219 198 160 +2 1 184 11.4500 42.3300 2 195 30.4700 99.7500 +5.2238669010e-01 -2.1405946529e-01 -1.7859406810e+00 +32 28 39 +2 1 216 216.4500 -97.4100 2 235 208.0200 -38.4200 +4.9308239751e-02 -2.2625020457e-01 -1.9133999409e+00 +189 182 176 +2 1 218 27.0600 -97.4600 2 237 31.7000 -34.7600 +9.4542494937e-01 -9.5749721057e-02 -2.4306363201e+00 +153 141 127 +2 1 241 283.0600 -37.2100 2 257 309.3300 21.5600 +-6.1116911579e-02 6.6142048787e-03 -1.9089043571e+00 +218 198 177 +2 1 261 -14.6300 -13.1300 2 276 -7.6800 44.3800 +-2.5248501679e-01 1.8587465639e-02 -1.9787054763e+00 +197 190 182 +2 1 265 -74.8000 -10.8400 2 278 -60.0000 46.0200 +-2.5149475558e-02 3.6082554431e-02 -1.8941029351e+00 +215 195 172 +2 1 269 -2.6900 -2.0300 2 393 2.6200 55.1700 +-6.6255423601e-01 8.2581670209e-02 -1.9745115917e+00 +241 250 255 +2 1 271 -202.3900 7.0800 2 280 -173.4000 60.3400 +2.5404802787e-02 2.2488825036e-01 -2.3421551677e+00 +217 191 160 +2 1 281 27.5000 46.2200 2 301 46.1600 103.8100 +-4.9599222272e-01 -2.6211488628e-01 -1.8180294970e+00 +143 151 149 +2 1 315 -174.8400 -114.0200 2 464 -150.6100 -49.0900 +-4.4814158816e-01 -2.5364732544e-01 -1.8000567059e+00 +128 134 128 +2 1 316 -161.5600 -112.2500 2 465 -140.0300 -47.7700 +6.4814202559e-01 -6.7670648231e-02 -2.2193962088e+00 +172 159 147 +2 1 347 217.4600 -32.5900 2 264 231.7500 26.4800 +1.8635123854e-01 -4.7134090561e-02 -2.0836338207e+00 +200 177 155 +2 1 349 75.3900 -30.0300 2 378 84.3000 29.2100 +4.5234314671e-01 -7.7251868557e-03 -2.0584798802e+00 +21 21 28 +2 1 360 165.2800 -14.7900 2 386 172.2500 43.9500 +-3.5631911023e-01 8.9141425850e-02 -2.1788029780e+00 +220 210 209 +2 1 373 -88.5400 7.9700 2 396 -66.8000 64.4800 +-3.6300196439e-01 1.1583748811e-01 -2.2056173215e+00 +216 213 204 +2 1 380 -88.1400 15.1200 2 403 -65.9100 71.6000 +-3.6300196439e-01 1.1583748811e-01 -2.2056173215e+00 +216 213 204 +2 1 381 -88.1400 15.1200 2 404 -65.9100 71.6000 +-1.7607676508e-01 2.4782678627e-01 -2.3988979415e+00 +120 104 89 +2 1 396 -25.0100 48.3800 2 419 -2.8000 104.9100 +-3.5619943890e-01 -2.0260739811e-01 -1.8307185288e+00 +50 55 34 +2 1 446 -125.4600 -92.4600 2 664 -108.4000 -29.3600 +6.9503193108e-02 -1.2678464942e-01 -1.9742681058e+00 +79 87 55 +2 1 466 35.1800 -59.3500 2 691 41.7000 0.9100 +-2.5717942094e-01 -8.6836317139e-02 -1.9495079181e+00 +84 106 61 +2 1 473 -78.6000 -46.7800 2 980 -63.7200 11.7600 +2.8799886187e-01 -9.3078646511e-02 -2.0516081985e+00 +88 115 73 +2 1 477 110.3100 -44.2400 2 370 116.9600 13.7200 +3.4236587301e-01 -2.8985533362e-02 -2.0444382470e+00 +204 184 168 +2 1 498 128.5900 -22.8900 2 530 134.8400 36.0000 +4.1830595929e-01 -2.0231288654e-02 -2.0360347026e+00 +42 39 32 +2 1 501 155.0500 -19.5100 2 534 160.8800 39.7300 +-1.9847374478e-01 1.3821415193e-01 -2.3951007581e+00 +93 100 79 +2 1 522 -30.6100 19.2200 2 758 -7.8100 76.2600 +2.1148941245e-01 1.4255596768e-01 -2.4021878375e+00 +94 102 91 +2 1 524 80.0500 23.4100 2 568 99.1700 82.4000 +-1.7191085655e-01 -2.6417830967e-01 -1.8261156717e+00 +88 83 79 +2 1 595 -59.2500 -116.3100 2 884 -50.0900 -51.6100 +-4.8302147864e-01 -2.5230622779e-01 -1.8307537943e+00 +39 47 37 +2 1 605 -168.4800 -109.7100 2 892 -144.8800 -45.4200 +-3.2439869778e-01 -2.1055295104e-01 -1.8103360097e+00 +34 43 28 +2 1 619 -116.9000 -96.2900 2 921 -101.6800 -33.3600 +-3.9195076334e-01 -1.3147617883e-01 -1.9171651689e+00 +119 130 61 +2 1 638 -126.5800 -63.5500 2 688 -106.9800 -3.3600 +-5.9732482404e-01 -9.6877894021e-02 -1.9037071880e+00 +208 191 163 +2 1 643 -194.1700 -51.9700 2 693 -166.2600 6.3900 +3.2289794266e-01 -9.1617697231e-02 -2.0321713197e+00 +170 167 178 +2 1 648 122.8900 -44.5700 2 988 128.4400 14.8500 +1.3457222361e-01 -7.3895509584e-02 -2.0757267230e+00 +159 164 165 +2 1 656 58.4500 -39.1100 2 707 67.7000 20.1500 +-9.5887274458e-01 -5.9093538626e-02 -2.1763779632e+00 +113 108 104 +2 1 658 -247.9000 -37.5300 2 995 -206.2400 19.0500 +4.6689378336e-01 2.8728198552e-03 -2.0302813499e+00 +216 211 214 +2 1 695 171.9700 -10.9300 2 741 177.8700 47.8200 +-4.0158574381e-01 1.0188397490e-01 -2.0431137015e+00 +181 174 170 +2 1 716 -115.4600 14.5600 2 1086 -95.0300 68.8700 +-4.3499510452e-01 1.3382569826e-01 -2.0240735744e+00 +138 124 114 +2 1 720 -127.8200 24.5600 2 1097 -107.0000 78.6000 +-4.1116932191e-01 1.4951498826e-01 -2.0637528093e+00 +214 209 211 +2 1 722 -116.1500 28.8400 2 759 -95.4300 82.6300 +-5.8887589359e-02 1.9885055929e-01 -2.3451683695e+00 +77 69 66 +2 1 728 4.4600 38.1000 2 765 24.0900 95.3800 +-4.1384664539e-01 -2.3169034066e-01 -1.8075529172e+00 +121 142 69 +2 1 836 -148.6800 -103.9500 2 1319 -128.9800 -40.1900 +-5.1805367944e-01 -2.4164176654e-01 -1.8569564213e+00 +125 129 114 +2 1 837 -175.8900 -104.1000 2 1313 -150.4600 -40.6000 +-5.1805367944e-01 -2.4164176654e-01 -1.8569564213e+00 +125 129 114 +2 1 838 -175.8900 -104.1000 2 1314 -150.4600 -40.6000 +-4.6617326017e-01 -2.1182088621e-01 -1.8321828208e+00 +106 133 55 +2 1 853 -162.9000 -94.9400 2 1350 -140.5000 -32.3900 +-4.6617326017e-01 -2.1182088621e-01 -1.8321828208e+00 +106 133 55 +2 1 854 -162.9000 -94.9400 2 1351 -140.5000 -32.3900 +-1.4893989401e-01 -2.0918457098e-01 -1.8433466217e+00 +209 189 163 +2 1 855 -49.6700 -94.5700 2 1353 -41.1800 -31.8600 +-3.4409013176e-01 -1.8146011250e-01 -1.8651477474e+00 +59 85 36 +2 1 866 -116.8800 -83.0800 2 1380 -99.8700 -21.1300 +-4.4332912337e-01 -1.5437621857e-01 -1.9085781503e+00 +125 153 56 +2 1 880 -144.3600 -71.7500 2 959 -122.5500 -10.9300 +-3.5141182685e-01 -1.1826265549e-01 -1.9597029727e+00 +59 77 44 +2 1 889 -108.2900 -57.9500 2 1408 -89.5900 1.9900 +4.8899615262e-02 -1.1333237202e-01 -1.9352105319e+00 +77 73 70 +2 1 891 26.7500 -55.5500 2 967 32.1700 4.4000 +-5.9797857656e-01 -1.0002268101e-01 -1.8772875725e+00 +76 96 32 +2 1 894 -198.8500 -53.5500 2 1413 -171.0300 4.8700 +7.0530191409e-02 -9.6744618289e-02 -1.9996121331e+00 +50 54 62 +2 1 902 35.9000 -48.3900 2 979 43.3600 11.4500 +-9.9983964763e-01 -6.0984635786e-02 -2.1974915795e+00 +130 178 226 +2 1 914 -254.3000 -37.8800 2 1447 -211.3200 18.4700 +-9.6371779222e-01 -4.8792750092e-02 -2.1457041226e+00 +97 115 103 +2 1 920 -254.1400 -34.9600 2 1002 -212.3500 21.4400 +4.4102570899e-01 -2.0064392495e-02 -2.0240194885e+00 +13 20 28 +2 1 944 163.6200 -19.0100 2 1514 169.0000 39.7200 +-1.1287378587e-01 -8.0911079677e-03 -1.9244454451e+00 +62 72 78 +2 1 946 -31.8300 -18.9300 2 1043 -22.8000 38.9100 +2.0824959893e-02 1.2071437736e-01 -2.3166526699e+00 +87 77 77 +2 1 985 26.2900 17.8000 2 1579 44.4200 75.7500 +-3.5957645475e-01 1.4042923856e-01 -2.1974816966e+00 +177 162 155 +2 1 989 -87.9900 22.8300 2 1583 -66.0800 78.2700 +-3.9604037555e-01 1.2940117145e-01 -2.0270901113e+00 +146 136 128 +3 1 991 -115.7800 23.8500 2 1094 -95.7300 77.9500 4 557 -107.2600 26.8000 +-4.0078900894e-01 1.3123695107e-01 -2.0385678322e+00 +146 136 128 +2 1 992 -115.7800 23.8500 2 1095 -95.7300 77.9500 +3.1610980402e-01 3.2716646428e-01 -2.7580045180e+00 +78 79 80 +2 1 1016 101.4300 59.6300 2 1623 129.0400 119.5900 +-3.9648657068e-01 -2.3962292110e-01 -1.8114731566e+00 +38 53 38 +2 1 1148 -141.9800 -106.7400 2 1308 -123.0100 -42.7800 +4.0282494800e-01 -1.7839685007e-01 -1.6513918936e+00 +195 204 216 +2 1 1174 178.0400 -90.8400 2 1354 160.5800 -31.3300 +-1.7728077057e-01 -1.6509851449e-01 -1.9067750636e+00 +128 129 100 +2 1 1204 -54.7300 -75.5000 2 1386 -43.4200 -14.4600 +5.2965566281e-01 -1.6355811553e-01 -1.8299287824e+00 +88 107 79 +2 1 1206 214.1500 -74.7500 2 1381 208.9400 -16.2400 +6.7395321946e-01 -7.6480747730e-02 -2.2428170239e+00 +181 163 159 +2 1 1293 223.4300 -34.9800 2 1006 238.7800 24.1300 +-2.5572848880e-01 4.1270279338e-02 -2.7562279236e+00 +76 76 79 +2 1 1373 -27.3100 -9.9800 2 1536 2.7000 49.8800 +1.3119051082e-01 1.1011585065e-01 -2.2764485524e+00 +96 94 86 +2 1 1410 57.4600 16.4900 2 1576 73.4500 74.8800 +2.1884972687e-01 1.3254438273e-01 -2.4397054924e+00 +215 197 181 +2 1 1419 81.9100 20.0000 2 1099 101.9900 78.9600 +1.9719970334e-01 1.5596585851e-01 -2.3885511110e+00 +205 192 184 +2 1 1427 76.0800 27.1300 2 1107 94.8700 86.3400 +3.5057611108e-01 2.2303975801e-01 -2.7123892334e+00 +82 72 71 +2 1 1441 111.0800 37.0500 2 1606 137.6400 97.0500 +-3.9748392625e-01 1.7943495584e-01 -2.0330735248e+00 +222 218 214 +2 1 1450 -115.5000 39.5600 2 1599 -96.0100 92.4900 +-5.2746807023e-01 -2.2047864647e-01 -1.9949122989e+00 +87 111 42 +2 3 0 -163.8300 -41.6700 4 0 -126.7800 -60.0100 +8.4316154947e-02 -1.3662015620e-01 -1.9685507243e+00 +71 84 47 +2 3 83 -9.7400 -17.1900 4 100 -11.3900 -35.6200 +-6.9070921853e-02 -3.3605860403e-02 -1.9190347936e+00 +218 200 177 +2 3 92 -61.9000 12.8700 4 108 -58.5600 -8.6700 +1.7409865996e-01 -7.6585120535e-03 -2.1066559272e+00 +86 106 57 +2 3 94 28.6000 23.8100 4 67 28.1600 7.9000 +-3.9407026846e-01 -2.1697645899e-01 -1.8183324356e+00 +57 64 39 +2 3 126 -163.8000 -47.1200 4 156 -137.9300 -66.7900 +3.2815473487e-02 -1.8913705712e-01 -1.8916308820e+00 +48 42 36 +2 3 129 -33.7400 -36.7700 4 98 -35.6700 -54.4500 +4.4701162683e-02 -2.2481929447e-01 -1.9258345915e+00 +192 179 162 +2 3 199 -25.4800 -47.0200 4 159 -25.6100 -63.3000 +-4.5633091589e-02 -1.7979185902e-01 -1.8698766061e+00 +42 35 30 +2 3 209 -60.2200 -34.9600 4 245 -58.2200 -52.5100 +4.2586633201e-02 -1.0362520401e-01 -1.9148661495e+00 +101 111 107 +2 3 218 -28.8900 -8.9100 4 169 -31.7700 -27.2600 +8.4639591348e-01 -8.6973963561e-02 -2.3542147244e+00 +209 192 160 +2 3 231 236.4500 7.8900 4 186 244.2300 4.7900 +4.6635003203e-01 -3.2140111407e-02 -2.0251071342e+00 +208 186 156 +2 3 244 114.3800 19.3100 4 191 96.8800 6.4300 +8.5333762891e-02 -1.4979379648e-02 -2.0450152954e+00 +71 74 76 +2 3 247 -2.6100 21.2700 4 178 -2.2100 1.9700 +9.8185218038e-03 1.3030661070e-01 -2.3072875911e+00 +121 128 118 +2 3 262 0.4900 59.9400 4 296 15.0600 39.9900 +9.8185218038e-03 1.3030661070e-01 -2.3072875911e+00 +121 128 118 +2 3 263 0.4900 59.9400 4 297 15.0600 39.9900 +-2.7197516534e-02 2.1165786800e-01 -2.3193470029e+00 +224 197 152 +2 3 278 -8.2900 80.5100 4 218 6.9500 58.6100 +-9.5463283915e-02 2.5037760072e-01 -2.3617151826e+00 +133 107 84 +2 3 286 -21.1000 88.0900 4 313 -2.0500 65.0300 +2.5888209842e-01 -7.3812593657e-02 -2.0504391614e+00 +187 181 172 +2 3 347 49.9300 4.3200 4 387 43.4400 -10.9800 +4.9164178772e-01 -4.1213753975e-02 -2.0504709227e+00 +173 159 126 +2 3 357 123.7800 16.6300 4 269 107.9500 4.7000 +1.7995976259e-01 5.1506517980e-02 -2.2892494055e+00 +215 196 150 +2 3 375 43.4600 41.2000 4 284 52.3900 24.9400 +2.1340553692e-01 1.4130882167e-01 -2.3846417214e+00 +95 92 82 +2 3 389 57.4800 64.5000 4 300 68.9600 49.1200 +-5.3736018580e-01 -2.3900168837e-01 -1.8536083644e+00 +170 165 147 +2 3 452 -191.4500 -51.1400 4 502 -154.8300 -71.3100 +1.0295688715e-01 -1.7072056767e-01 -1.9594550071e+00 +125 122 114 +2 3 474 -4.7800 -28.7600 4 517 -7.4100 -45.2800 +-2.2143098591e-01 -1.5204440880e-01 -1.9366642233e+00 +63 65 39 +2 3 481 -99.6700 -23.6500 4 519 -83.7400 -43.8300 +8.0645228896e-02 -8.9790278831e-02 -2.0146784388e+00 +220 189 158 +2 3 495 -6.3400 -1.8800 4 380 -6.0200 -20.3300 +1.6126457228e-01 1.8669775992e-02 -2.2607030704e+00 +219 197 174 +2 3 526 36.7600 32.2600 4 414 45.3700 15.4300 +1.9529198690e-01 8.2128783117e-02 -2.3320018366e+00 +223 197 161 +2 3 542 50.0600 49.2500 4 287 60.2800 33.5500 +9.5260810244e-03 1.5867994622e-01 -2.3082488155e+00 +229 199 164 +2 3 553 0.2500 67.3900 4 431 14.4100 47.0200 +3.3331948720e-01 3.1611800692e-01 -2.6945451195e+00 +255 255 255 +2 3 562 99.4300 102.2500 4 608 121.2900 90.8800 +-4.3214435648e-01 -1.8174428617e-01 -1.8659265486e+00 +32 42 22 +2 3 644 -164.8900 -34.1400 4 683 -136.0500 -56.0700 +9.3117266285e-01 -7.6548376986e-02 -2.4292173304e+00 +79 77 70 +2 3 683 259.1900 12.1600 4 407 273.0800 11.1800 +2.1963681735e-01 -3.0941451925e-02 -2.0690900110e+00 +209 190 162 +2 3 693 39.1100 17.6800 4 741 34.7700 1.0400 +-1.9711398783e-01 -1.4979132759e-02 -1.9601170168e+00 +214 199 177 +2 3 695 -91.4200 18.4900 4 732 -78.3200 -6.0700 +6.4220726214e-01 -2.9984159264e-02 -2.2205942273e+00 +154 156 134 +2 3 700 175.0700 22.4600 4 413 170.7300 14.7400 +2.0193623643e-02 1.8028873071e-01 -2.3077599200e+00 +77 71 68 +2 3 743 2.6700 73.3800 4 822 16.0600 52.6300 +6.9434069661e-01 1.6390577908e+00 -8.3994106305e+00 +254 255 255 +2 3 759 178.0500 141.1300 4 612 280.1700 150.8600 +3.2589349196e-01 -2.2272223650e-01 -1.6388209875e+00 +117 116 111 +2 3 844 37.9600 -64.4200 4 961 -4.8600 -78.1300 +9.9555545655e-03 -2.3419778789e-01 -1.8598420178e+00 +111 104 91 +2 3 856 -44.2900 -53.3800 4 968 -45.6200 -68.9500 +6.4405034562e-01 -5.3181779361e-02 -2.2093233654e+00 +189 181 151 +2 3 939 175.7800 14.8900 4 768 171.0300 7.9200 +4.7396891265e-01 -1.7919404540e-02 -2.0322525058e+00 +43 35 20 +2 3 954 117.0700 24.3900 4 1117 99.6400 11.0800 +-3.8388793155e-01 1.6461939634e-01 -2.1903543937e+00 +252 254 253 +2 3 1002 -106.2600 64.4300 4 806 -78.4600 36.7100 +-3.8632536060e-01 1.8550607278e-01 -2.0504709629e+00 +254 254 252 +2 3 1011 -127.1900 72.9400 4 1156 -102.7500 41.3400 +7.5218191012e-01 1.5812373868e+00 -8.1771395561e+00 +118 117 113 +2 3 1037 182.3600 140.5500 4 1225 283.8000 150.8000 +4.3136575187e-01 -1.8393914735e-01 -1.8563821630e+00 +186 180 169 +2 3 1240 95.4300 -37.6400 4 998 67.2300 -50.5500 +5.9293858354e-02 -1.3185352760e-01 -1.9695156705e+00 +204 188 145 +2 3 1269 -17.0900 -16.4200 4 1031 -17.3800 -33.7600 +4.2965937492e-01 -5.3693665957e-02 -2.0157785952e+00 +101 107 64 +2 3 1324 101.9700 11.7800 4 1082 85.4400 -1.6800 +5.5923414564e-01 -3.5995770203e-02 -2.1241944144e+00 +67 64 42 +2 3 1346 147.7500 19.1900 4 1115 136.5800 9.5500 +7.2488868669e-01 -4.2190953838e-02 -2.8203332625e+00 +115 114 108 +2 3 1359 195.5500 21.9200 4 786 229.1000 18.3300 +-3.5722260442e-02 2.2498022016e-01 -2.4848317450e+00 +106 103 89 +2 3 1482 3.4700 79.0700 4 1192 25.5600 60.8000 +-1.6923051517e-01 2.8533181449e-01 -2.3816152926e+00 +208 203 198 +2 3 1488 -36.5300 94.7300 4 1202 -14.3900 70.3500 +-1.6923051517e-01 2.8533181449e-01 -2.3816152926e+00 +208 203 198 +2 3 1489 -36.5300 94.7300 4 1203 -14.3900 70.3500 +3.1977368160e-01 3.1849528198e-01 -2.7183582130e+00 +255 255 255 +2 3 1493 97.1200 101.6500 4 1216 120.2100 90.5700 +8.4926111776e-01 -9.6697271079e-02 -2.3524060662e+00 +211 191 164 +2 2 1450 286.4800 19.9100 4 1097 245.3300 1.8900 diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt new file mode 100644 index 000000000..12c9f4db4 --- /dev/null +++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt @@ -0,0 +1,80 @@ +3 7 19 + +0 0 -385.989990234375 387.1199951171875 +1 0 -38.439998626708984375 492.1199951171875 +2 0 -667.91998291015625 123.1100006103515625 +0 1 383.8800048828125 -15.299989700317382812 +1 1 559.75 -106.15000152587890625 +0 2 591.54998779296875 136.44000244140625 +1 2 863.8599853515625 -23.469970703125 +2 2 494.720001220703125 112.51999664306640625 +0 3 592.5 125.75 +1 3 861.08001708984375 -35.219970703125 +2 3 498.540008544921875 101.55999755859375 +0 4 348.720001220703125 558.3800048828125 +1 4 776.030029296875 483.529998779296875 +2 4 7.7800288200378417969 326.350006103515625 +0 5 14.010009765625 96.420013427734375 +1 5 207.1300048828125 118.3600006103515625 +0 6 202.7599945068359375 340.989990234375 +1 6 543.18011474609375 294.80999755859375 +2 6 -58.419979095458984375 110.8300018310546875 + +0.29656188120312942935 +-0.035318354384285870207 +0.31252101755032046793 +0.47230274932665988752 +-0.3572340863744113415 +-2.0517704282499575896 +1430.031982421875 +-7.5572756941255647689e-08 +3.2377570134516087119e-14 + +0.28532097381985194184 +-0.27699838370789808817 +0.048601169984112867206 +-1.2598695987143850861 +-0.049063798188844320869 +-1.9586867140445654023 +1432.137451171875 +-7.3171918302250560373e-08 +3.1759419042137054801e-14 + +0.057491325683772541433 +0.34853090049579965592 +0.47985129303736057116 +8.1963904289063389541 +6.5146840788718787252 +-3.8392804395897406344 +1572.047119140625 +-1.5962623223231275915e-08 +-1.6507904730136101212e-14 + +-11.317351620610928364 +3.3594874875767186673 +-42.755222607849105998 + +4.2648515634753199066 +-8.4629358700849355301 +-22.252086323427270997 + +10.996977688149536689 +-9.2123370180278048025 +-29.206739014051372294 + +10.935342607054865383 +-9.4338917557810741954 +-29.112263909175499776 + +15.714024935401759819 +1.3745079651566265433 +-59.286834979937104606 + +-1.3624227800805182031 +-4.1979357415396094666 +-21.034430148188398846 + +6.7690173115899296974 +-4.7352452433700786827 +-53.605307875695892506 + diff --git a/examples/Data/dubrovnik-3-7-pre.txt b/examples/Data/dubrovnik-3-7-pre.txt new file mode 100644 index 000000000..783e8f8d0 --- /dev/null +++ b/examples/Data/dubrovnik-3-7-pre.txt @@ -0,0 +1,80 @@ +3 7 19 + +0 0 -3.859900e+02 3.871200e+02 +1 0 -3.844000e+01 4.921200e+02 +2 0 -6.679200e+02 1.231100e+02 +0 1 3.838800e+02 -1.529999e+01 +1 1 5.597500e+02 -1.061500e+02 +0 2 5.915500e+02 1.364400e+02 +1 2 8.638600e+02 -2.346997e+01 +2 2 4.947200e+02 1.125200e+02 +0 3 5.925000e+02 1.257500e+02 +1 3 8.610800e+02 -3.521997e+01 +2 3 4.985400e+02 1.015600e+02 +0 4 3.487200e+02 5.583800e+02 +1 4 7.760300e+02 4.835300e+02 +2 4 7.780029e+00 3.263500e+02 +0 5 1.401001e+01 9.642001e+01 +1 5 2.071300e+02 1.183600e+02 +0 6 2.027600e+02 3.409900e+02 +1 6 5.431801e+02 2.948100e+02 +2 6 -5.841998e+01 1.108300e+02 + +-1.6943983532198115e-02 +1.1171804676513932e-02 +2.4643508831711991e-03 +7.3030995682610689e-01 +-2.6490818471043420e-01 +-1.7127892627337182e+00 +1.4300319432711681e+03 +-7.5572758535864072e-08 +3.2377569465570913e-14 + +1.5049725341485708e-02 +-1.8504564785154357e-01 +-2.9278402790141456e-01 +-1.0590476152349551e+00 +-3.6017862414345798e-02 +-1.5720340175803784e+00 +1.4321374541298685e+03 +-7.3171919892612292e-08 +3.1759419019880947e-14 + +-3.0793597986873011e-01 +3.2077907982952031e-01 +2.2253985096991455e-01 +8.5034483295909009e+00 +6.7499603629668741e+00 +-3.6383814384447088e+00 +1.5720470590375264e+03 +-1.5962623661947355e-08 +-1.6507904848058800e-14 + +-1.2055995050700867e+01 +1.2838775976205760e+01 +-4.1099369264082803e+01 + +6.4168905904672933e+00 +3.8897031177598462e-01 +-2.3586282709150449e+01 + +1.3051100355717297e+01 +3.8387587111611952e+00 +-2.9777932175344951e+01 + +1.3060946673472820e+01 +3.5910521225905803e+00 +-2.9759080795372942e+01 + +1.4265764475421857e+01 +2.4096216156436530e+01 +-5.4823971067225500e+01 + +-2.5292283211391348e-01 +2.2166082122808284e+00 +-2.1712127480255084e+01 + +7.6465738085189585e+00 +1.4185331909846619e+01 +-5.2070299568846060e+01 + diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 12e702b7c..b535f5179 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -26,12 +26,12 @@ /* * NOTES: * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * - * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. + * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. */ namespace gtsam { @@ -40,11 +40,11 @@ namespace gtsam { namespace imuBias { class ConstantBias : public DerivedValue { - private: + private: Vector3 biasAcc_; Vector3 biasGyro_; - public: + public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 6; @@ -144,17 +144,17 @@ namespace imuBias { /// return dimensionality of tangent space inline size_t dim() const { return dimension; } - /** Update the LieVector with a tangent space update */ - inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } + /** Update the LieVector with a tangent space update */ + inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } /// @} /// @name Group /// @{ - /** identity for group operation */ + /** identity for group operation */ static ConstantBias identity() { return ConstantBias(); } /** invert the object and yield a new one */ @@ -213,7 +213,7 @@ namespace imuBias { /// @} - }; // ConstantBias class + }; // ConstantBias class } // namespace ImuBias diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 66d62ef22..7330f6aa3 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -136,12 +136,12 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) // Actual preintegrated values ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero()); + Matrix3::Zero(), Matrix3::Zero()); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc @@ -193,13 +193,13 @@ TEST( CombinedImuFactor, ErrorWithBiases ) ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -224,14 +224,14 @@ TEST( CombinedImuFactor, ErrorWithBiases ) // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + Matrix H1a, H2a, H3a, H4a, H5a, H6a; + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); - EXPECT(assert_equal(H1e, H1a.topRows(9))); - EXPECT(assert_equal(H2e, H2a.topRows(9))); - EXPECT(assert_equal(H3e, H3a.topRows(9))); - EXPECT(assert_equal(H4e, H4a.topRows(9))); - EXPECT(assert_equal(H5e, H5a.topRows(9))); + EXPECT(assert_equal(H1e, H1a.topRows(9))); + EXPECT(assert_equal(H2e, H2a.topRows(9))); + EXPECT(assert_equal(H3e, H3a.topRows(9))); + EXPECT(assert_equal(H4e, H4a.topRows(9))); + EXPECT(assert_equal(H5e, H5a.topRows(9))); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 2fdd2fb4e..f5b5fd234 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -25,8 +25,8 @@ using namespace gtsam; /* ************************************************************************* */ TEST( ImuBias, Constructor) { - Vector bias_acc(Vector_(3,0.1,0.2,0.4)); - Vector bias_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector bias_acc((Vector(3) << 0.1,0.2,0.4)); + Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03)); // Default Constructor gtsam::imuBias::ConstantBias bias1; @@ -39,5 +39,5 @@ TEST( ImuBias, Constructor) } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 58a5deb34..78b8abd1f 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -74,9 +74,9 @@ struct BoundingConstraint1: public NoiseModelFactor1 { } if (isGreaterThan_) - return Vector_(1, error); + return (Vector(1) << error); else - return -1.0 * Vector_(1, error); + return -1.0 * (Vector(1) << error); } private: @@ -147,9 +147,9 @@ struct BoundingConstraint2: public NoiseModelFactor2 { } if (isGreaterThan_) - return Vector_(1, error); + return (Vector(1) << error); else - return -1.0 * Vector_(1, error); + return -1.0 * (Vector(1) << error); } private: diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h new file mode 100644 index 000000000..d3f560b32 --- /dev/null +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -0,0 +1,54 @@ +/* + * @file EssentialMatrixFactor.cpp + * @brief EssentialMatrixFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Factor that evaluates epipolar error p'Ep for given essential matrix + */ +class EssentialMatrixFactor: public NoiseModelFactor1 { + + Point2 pA_, pB_; ///< Measurements in image A and B + Vector vA_, vB_; ///< Homogeneous versions + + typedef NoiseModelFactor1 Base; + +public: + + /// Constructor + EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) : + Base(model, key), pA_(pA), pB_(pB), // + vA_(EssentialMatrix::Homogeneous(pA)), // + vB_(EssentialMatrix::Homogeneous(pB)) { + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + std::cout << " EssentialMatrixFactor with measurements\n (" + << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() + << ")'" << std::endl; + } + + /// vector of errors returns 1D vector + Vector evaluateError(const EssentialMatrix& E, boost::optional H = + boost::none) const { + return (Vector(1) << E.error(vA_, vB_, H)); + } + +}; + +} // gtsam + diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h new file mode 100644 index 000000000..f30392d62 --- /dev/null +++ b/gtsam/slam/RotateFactor.h @@ -0,0 +1,99 @@ +/* + * @file RotateFactor.cpp + * @brief RotateFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor on unknown rotation iRC that relates two incremental rotations + * c1Rc2 = iRc' * i1Ri2 * iRc + * Which we can write (see doc/math.lyx) + * e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p]) + * with z and p measured and predicted angular velocities, and hence + * p = iRc * z + */ +class RotateFactor: public NoiseModelFactor1 { + + Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z + + typedef NoiseModelFactor1 Base; + +public: + + /// Constructor + RotateFactor(Key key, const Rot3& P, const Rot3& Z, + const SharedNoiseModel& model) : + Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) { + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + std::cout << "RotateFactor:" << std::endl; + p_.print("p"); + z_.print("z"); + } + + /// vector of errors returns 2D vector + Vector evaluateError(const Rot3& R, + boost::optional H = boost::none) const { + // predict p_ as q = R*z_, derivative H will be filled if not none + Point3 q = R.rotate(z_,H); + // error is just difference, and note derivative of that wrpt q is I3 + return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z(); + } + +}; + +/** + * Factor on unknown rotation R that relates two directions p_i = iRc * z_c + * Directions provide less constraints than a full rotation + */ +class RotateDirectionsFactor: public NoiseModelFactor1 { + + Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z + + typedef NoiseModelFactor1 Base; + +public: + + /// Constructor + RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z, + const SharedNoiseModel& model) : + Base(model, key), p_(p), z_(z) { + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + std::cout << "RotateDirectionsFactor:" << std::endl; + p_.print("p"); + z_.print("z"); + } + + /// vector of errors returns 2D vector + Vector evaluateError(const Rot3& R, + boost::optional H = boost::none) const { + Sphere2 q = R * z_; + Vector e = p_.error(q, H); + if (H) { + Matrix DR; + R.rotate(z_, DR); + *H = (*H) * DR; + } + return e; + } + +}; +} // gtsam + diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index e22d0b42c..e84ff9671 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -12,7 +12,7 @@ /** * @file dataset.cpp * @date Jan 22, 2010 - * @author nikai + * @author nikai, Luca Carlone * @brief utility functions for loading datasets */ @@ -50,6 +50,7 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name); namesToSearch.push_back(name + ".graph"); namesToSearch.push_back(name + ".txt"); + namesToSearch.push_back(name + ".out"); // Find first name that exists BOOST_FOREACH(const fs::path& root, rootsToSearch) { @@ -61,10 +62,10 @@ string findExampleDataFile(const string& name) { // If we did not return already, then we did not find the file throw std::invalid_argument( - "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + - name + ", " + name + ".graph, or " + name + ".txt"); + "gtsam::findExampleDataFile could not find a matching file in\n" + SOURCE_TREE_DATASET_DIR " or\n" + INSTALLED_DATASET_DIR " named\n" + + name + ", " + name + ".graph, or " + name + ".txt"); } #endif @@ -150,7 +151,7 @@ pair load2D( // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); if (!model) { - Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2)); + Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2)); model = noiseModel::Diagonal::Variances(variances, smart); } @@ -178,7 +179,7 @@ pair load2D( continue; noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); graph->add(BearingRangeFactor(id1, id2, bearing, range, measurementNoise)); // Insert poses or points if they do not exist yet @@ -210,13 +211,13 @@ pair load2D( { double rangeVar = v1; double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, bearingVar, rangeVar)); + measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); } else { if(!haveLandmark) { cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" - "non-uniform covariance on LANDMARK measurements in this file." << endl; + "non-uniform covariance on LANDMARK measurements in this file." << endl; haveLandmark = true; } } @@ -228,7 +229,7 @@ pair load2D( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -385,7 +386,7 @@ pair load2D_robust( continue; noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); graph->add(BearingRangeFactor(id1, id2, bearing, range, measurementNoise)); // Insert poses or points if they do not exist yet @@ -402,9 +403,318 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } +/* ************************************************************************* */ +Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL + /* R = [ 1 0 0 + * 0 -1 0 + * 0 0 -1] + */ + Matrix3 R_mat = Matrix3::Zero(3,3); + R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0; + return Rot3(R_mat); +} + +/* ************************************************************************* */ +Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) +{ + Rot3 R90 = openGLFixedRotation(); + Rot3 wRc = ( R.inverse() ).compose(R90); + + // Our camera-to-world translation wTc = -R'*t + return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) +{ + Rot3 R90 = openGLFixedRotation(); + Rot3 cRw_openGL = R90.compose( R.inverse() ); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz)); + return Pose3(cRw_openGL, t_openGL); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Pose3& PoseGTSAM) +{ + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); +} + +/* ************************************************************************* */ +bool readBundler(const string& filename, SfM_data &data) +{ + // Load the data file + ifstream is(filename.c_str(),ifstream::in); + if(!is) + { + cout << "Error in readBundler: can not find the file!!" << endl; + return false; + } + + // Ignore the first line + char aux[500]; + is.getline(aux,500); + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints; + is >> nrPoses >> nrPoints; + + // Get the information for the camera poses + for( size_t i = 0; i < nrPoses; i++ ) + { + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + // Get the rotation matrix + float r11, r12, r13; + float r21, r22, r23; + float r31, r32, r33; + is >> r11 >> r12 >> r13 + >> r21 >> r22 >> r23 + >> r31 >> r32 >> r33; + + // Bundler-OpenGL rotation matrix + Rot3 R( + r11, r12, r13, + r21, r22, r23, + r31, r32, r33); + + // Check for all-zero R, in which case quit + if(r11==0 && r12==0 && r13==0) + { + cout << "Error in readBundler: zero rotation matrix for pose " << i << endl; + return false; + } + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R,tx,ty,tz); + + data.cameras.push_back(SfM_Camera(pose,K)); + } + + // Get the information for the 3D points + for( size_t j = 0; j < nrPoints; j++ ) + { + SfM_Track track; + + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + track.p = Point3(x,y,z); + + // Get the color information + float r, g, b; + is >> r >> g >> b; + track.r = r/255.f; + track.g = g/255.f; + track.b = b/255.f; + + // Now get the visibility information + size_t nvisible = 0; + is >> nvisible; + + for( size_t k = 0; k < nvisible; k++ ) + { + size_t cam_idx = 0, point_idx = 0; + float u, v; + is >> cam_idx >> point_idx >> u >> v; + track.measurements.push_back(make_pair(cam_idx,Point2(u,-v))); + } + + data.tracks.push_back(track); + } + + is.close(); + return true; +} + +/* ************************************************************************* */ +bool readBAL(const string& filename, SfM_data &data) +{ + // Load the data file + ifstream is(filename.c_str(),ifstream::in); + if(!is) + { + cout << "Error in readBAL: can not find the file!!" << endl; + return false; + } + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints, nrObservations; + is >> nrPoses >> nrPoints >> nrObservations; + + data.tracks.resize(nrPoints); + + // Get the information for the observations + for( size_t k = 0; k < nrObservations; k++ ) + { + size_t i = 0, j = 0; + float u, v; + is >> i >> j >> u >> v; + data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v))); + } + + // Get the information for the camera poses + for( size_t i = 0; i < nrPoses; i++ ) + { + // Get the rodriguez vector + float wx, wy, wz; + is >> wx >> wy >> wz; + Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R,tx,ty,tz); + + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + data.cameras.push_back(SfM_Camera(pose,K)); + } + + // Get the information for the 3D points + for( size_t j = 0; j < nrPoints; j++ ) + { + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + SfM_Track& track = data.tracks[j]; + track.p = Point3(x,y,z); + track.r = 0.4f; + track.g = 0.4f; + track.b = 0.4f; + } + + is.close(); + return true; +} + +/* ************************************************************************* */ +bool writeBAL(const string& filename, SfM_data &data) +{ + // Open the output file + ofstream os; + os.open(filename.c_str()); + os.precision(20); + if (!os.is_open()) { + cout << "Error in writeBAL: can not open the file!!" << endl; + return false; + } + + // Write the number of camera poses and 3D points + size_t nrObservations=0; + for (size_t j = 0; j < data.number_tracks(); j++){ + nrObservations += data.tracks[j].number_measurements(); + } + + // Write observations + os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; + os << endl; + + for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + SfM_Track track = data.tracks[j]; + + for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j + size_t i = track.measurements[k].first; // camera id + double u0 = data.cameras[i].calibration().u0(); + double v0 = data.cameras[i].calibration().v0(); + + if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;} + + double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin + double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin + Point2 pixelMeasurement(pixelBALx, pixelBALy); + os << i /*camera id*/ << " " << j /*point id*/ << " " + << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; + } + } + os << endl; + + // Write cameras + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Pose3 poseGTSAM = data.cameras[i].pose(); + Cal3Bundler cameraCalibration = data.cameras[i].calibration(); + Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().vector() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; + os << endl; + } + + // Write the points + for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + Point3 point = data.tracks[j].p; + os << point.x() << endl; + os << point.y() << endl; + os << point.z() << endl; + os << endl; + } + + os.close(); + return true; +} + +bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ + + // CHECKS + Values valuesPoses = values.filter(); + if( valuesPoses.size() != data.number_cameras()){ + cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() + <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl; + return false; + } + Values valuesPoints = values.filter(); + if( valuesPoints.size() != data.number_tracks()){ + cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() + <<") and values (#points " << valuesPoints.size() << ")!!" << endl; + } + if(valuesPoints.size() + valuesPoses.size() != values.size()){ + cout << "writeBALfromValues write only poses and points values!!" << endl; + return false; + } + if(valuesPoints.size()==0 || valuesPoses.size()==0){ + cout << "writeBALfromValues: No point or pose in values!!" << endl; + return false; + } + + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = values.at(poseKey); + Cal3Bundler K = data.cameras[i].calibration(); + PinholeCamera camera(pose, K); + data.cameras[i] = camera; + } + + for (size_t j = 0; j < data.number_tracks(); j++){ // for each point + Key pointKey = symbol('l',j); + if(values.exists(pointKey)){ + Point3 point = values.at(pointKey); + data.tracks[j].p = point; + }else{ + data.tracks[j].r = 1.0; + data.tracks[j].g = 0.0; + data.tracks[j].b = 0.0; + data.tracks[j].p = Point3(); + } + } + + return writeBAL(filename, data); +} + + } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 053da842a..bd5a28cdd 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -12,7 +12,7 @@ /** * @file dataset.h * @date Jan 22, 2010 - * @author nikai + * @author nikai, Luca Carlone * @brief utility functions for loading datasets */ @@ -20,62 +20,157 @@ #include #include +#include +#include +#include +#include +#include +#include // for pair #include namespace gtsam { #ifndef MATLAB_MEX_FILE - /** - * Find the full path to an example dataset distributed with gtsam. The name - * may be specified with or without a file extension - if no extension is - * give, this function first looks for the .graph extension, then .txt. We - * first check the gtsam source tree for the file, followed by the installed - * example dataset location. Both the source tree and installed locations - * are obtained from CMake during compilation. - * @return The full path and filename to the requested dataset. - * @throw std::invalid_argument if no matching file could be found using the - * search process described above. - */ - GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); +/** + * Find the full path to an example dataset distributed with gtsam. The name + * may be specified with or without a file extension - if no extension is + * give, this function first looks for the .graph extension, then .txt. We + * first check the gtsam source tree for the file, followed by the installed + * example dataset location. Both the source tree and installed locations + * are obtained from CMake during compilation. + * @return The full path and filename to the requested dataset. + * @throw std::invalid_argument if no matching file could be found using the + * search process described above. + */ +GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); #endif - /** - * Load TORO 2D Graph - * @param dataset/model pair as constructed by [dataset] - * @param maxID if non-zero cut out vertices >= maxID - * @param addNoise add noise to the edges - * @param smart try to reduce complexity of covariance to cheapest model - */ - GTSAM_EXPORT std::pair load2D( - std::pair > dataset, - int maxID = 0, bool addNoise = false, bool smart = true); +/** + * Load TORO 2D Graph + * @param dataset/model pair as constructed by [dataset] + * @param maxID if non-zero cut out vertices >= maxID + * @param addNoise add noise to the edges + * @param smart try to reduce complexity of covariance to cheapest model + */ +GTSAM_EXPORT std::pair load2D( + std::pair > dataset, + int maxID = 0, bool addNoise = false, bool smart = true); - /** - * Load TORO 2D Graph - * @param filename - * @param model optional noise model to use instead of one specified by file - * @param maxID if non-zero cut out vertices >= maxID - * @param addNoise add noise to the edges - * @param smart try to reduce complexity of covariance to cheapest model - */ - GTSAM_EXPORT std::pair load2D( - const std::string& filename, - boost::optional model = boost::optional< - noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, - bool smart = true); +/** + * Load TORO 2D Graph + * @param filename + * @param model optional noise model to use instead of one specified by file + * @param maxID if non-zero cut out vertices >= maxID + * @param addNoise add noise to the edges + * @param smart try to reduce complexity of covariance to cheapest model + */ +GTSAM_EXPORT std::pair load2D( + const std::string& filename, + boost::optional model = boost::optional< + noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, + bool smart = true); - GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, - gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); +GTSAM_EXPORT std::pair load2D_robust( + const std::string& filename, + gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); - /** save 2d graph */ - GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, - const noiseModel::Diagonal::shared_ptr model, const std::string& filename); +/** save 2d graph */ +GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const std::string& filename); - /** - * Load TORO 3D Graph - */ - GTSAM_EXPORT bool load3D(const std::string& filename); +/** + * Load TORO 3D Graph + */ +GTSAM_EXPORT bool load3D(const std::string& filename); + +/// A measurement with its camera index +typedef std::pair SfM_Measurement; + +/// Define the structure for the 3D points +struct SfM_Track +{ + gtsam::Point3 p; ///< 3D position of the point + float r,g,b; ///< RGB color of the 3D point + std::vector measurements; ///< The 2D image projections (id,(u,v)) + size_t number_measurements() const { return measurements.size();} +}; + +/// Define the structure for the camera poses +typedef gtsam::PinholeCamera SfM_Camera; + +/// Define the structure for SfM data +struct SfM_data +{ + std::vector cameras; ///< Set of cameras + std::vector tracks; ///< Sparse set of points + size_t number_cameras() const { return cameras.size();} ///< The number of camera poses + size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points +}; + +/** + * @brief This function parses a bundler output file and stores the data into a + * SfM_data structure + * @param filename The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); + +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a + * SfM_data structure + * @param filename The name of the BAL file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool readBAL(const std::string& filename, SfM_data &data); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a + * SfM_data structure + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a + * SfM_data structure and a value structure (measurements are the same as the SfM input data, + * while camera poses and values are read from Values) + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @param values structure where the graph values are stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); + +/** + * @brief This function converts an openGL camera pose to an GTSAM camera pose + * @param R rotation in openGL + * @param tx x component of the translation in openGL + * @param ty y component of the translation in openGL + * @param tz z component of the translation in openGL + * @return Pose3 in GTSAM format + */ +GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param R rotation in GTSAM + * @param tx x component of the translation in GTSAM + * @param ty y component of the translation in GTSAM + * @param tz z component of the translation in GTSAM + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param PoseGTSAM pose in GTSAM format + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); } // namespace gtsam diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index d918dbd99..1902bfa85 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -72,7 +72,7 @@ TEST( AntiFactor, NegativeHessian) size_t variable_count = originalFactor->size(); for(size_t i = 0; i < variable_count; ++i){ for(size_t j = i; j < variable_count; ++j){ - Matrix expected_G = -originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j); + Matrix expected_G = -Matrix(originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j)); Matrix actual_G = antiHessian->info(antiHessian->begin()+i, antiHessian->begin()+j); CHECK(assert_equal(expected_G, actual_G, 1e-5)); } diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index e35dcae7b..af6ab4bca 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,8 +1,8 @@ /** - * @file testBetweenFactor.cpp + * @file testBetweenFactor.cpp * @brief * @author Duy-Nguyen Ta - * @date Aug 2, 2013 + * @date Aug 2, 2013 */ #include diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index e830c74fd..f363fce11 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -12,7 +12,7 @@ /** * @file testDataset.cpp * @brief Unit test for dataset.cpp - * @author Richard Roberts + * @author Richard Roberts, Luca Carlone */ #include @@ -20,11 +20,13 @@ #include #include +#include #include using namespace std; using namespace gtsam; +/* ************************************************************************* */ TEST(dataSet, findExampleDataFile) { const string expected_end = "examples/Data/example.graph"; const string actual = findExampleDataFile("example"); @@ -33,6 +35,185 @@ TEST(dataSet, findExampleDataFile) { EXPECT(assert_equal(expected_end, actual_end)); } +/* ************************************************************************* */ +TEST( dataSet, Balbianello) +{ + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("Balbianello"); + SfM_data mydata; + CHECK(readBundler(filename, mydata)); + + // Check number of things + EXPECT_LONGS_EQUAL(5,mydata.number_cameras()); + EXPECT_LONGS_EQUAL(544,mydata.number_tracks()); + const SfM_Track& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + const SfM_Camera& camera0 = mydata.cameras[0]; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + EXPECT(assert_equal(expected,actual,1)); +} + +/* ************************************************************************* */ +TEST( dataSet, readBAL_Dubrovnik) +{ + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data mydata; + CHECK(readBAL(filename, mydata)); + + // Check number of things + EXPECT_LONGS_EQUAL(3,mydata.number_cameras()); + EXPECT_LONGS_EQUAL(7,mydata.number_tracks()); + const SfM_Track& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + const SfM_Camera& camera0 = mydata.cameras[0]; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + EXPECT(assert_equal(expected,actual,12)); +} + +/* ************************************************************************* */ +TEST( dataSet, openGL2gtsam) +{ + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(0.0,0.0,0.0); + Pose3 poseGTSAM = Pose3(R,t); + + Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns! + Rot3 cRw( + r1.x(), r2.x(), r3.x(), + -r1.y(), -r2.y(), -r3.y(), + -r1.z(), -r2.z(), -r3.z()); + Rot3 wRc = cRw.inverse(); + Pose3 actual = Pose3(wRc,t); + + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( dataSet, gtsam2openGL) +{ + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(1.0,20.0,10.0); + Pose3 actual = Pose3(R,t); + Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Pose3 expected = gtsam2openGL(poseGTSAM); + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( dataSet, writeBAL_Dubrovnik) +{ + ///< Read a file using the unit tested readBAL + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data readData; + readBAL(filenameToRead, readData); + + // Write readData to file filenameToWrite + const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + CHECK(writeBAL(filenameToWrite, readData)); + + // Read what we wrote + SfM_data writtenData; + CHECK(readBAL(filenameToWrite, writtenData)); + + // Check that what we read is the same as what we wrote + EXPECT(assert_equal(readData.number_cameras(),writtenData.number_cameras())); + EXPECT(assert_equal(readData.number_tracks(),writtenData.number_tracks())); + + for (size_t i = 0; i < readData.number_cameras(); i++){ + PinholeCamera expectedCamera = writtenData.cameras[i]; + PinholeCamera actualCamera = readData.cameras[i]; + EXPECT(assert_equal(expectedCamera,actualCamera)); + } + + for (size_t j = 0; j < readData.number_tracks(); j++){ + // check point + SfM_Track expectedTrack = writtenData.tracks[j]; + SfM_Track actualTrack = readData.tracks[j]; + Point3 expectedPoint = expectedTrack.p; + Point3 actualPoint = actualTrack.p; + EXPECT(assert_equal(expectedPoint,actualPoint)); + + // check rgb + Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b ); + Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b); + EXPECT(assert_equal(expectedRGB,actualRGB)); + + // check measurements + for (size_t k = 0; k < actualTrack.number_measurements(); k++){ + EXPECT(assert_equal(expectedTrack.measurements[k].first,actualTrack.measurements[k].first)); + EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); + } + } +} + + +/* ************************************************************************* */ +TEST( dataSet, writeBALfromValues_Dubrovnik){ + + ///< Read a file using the unit tested readBAL + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data readData; + readBAL(filenameToRead, readData); + + Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); + + Values value; + for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = poseChange.compose(readData.cameras[i].pose()); + value.insert(poseKey, pose); + } + for(size_t j=0; j < readData.number_tracks(); j++){ // for each point + Key pointKey = symbol('l',j); + Point3 point = poseChange.transform_from( readData.tracks[j].p ); + value.insert(pointKey, point); + } + + // Write values and readData to a file + const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + writeBALfromValues(filenameToWrite, readData, value); + + // Read the file we wrote + SfM_data writtenData; + readBAL(filenameToWrite, writtenData); + + // Check that the reprojection errors are the same and the poses are correct + // Check number of things + EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); + EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); + const SfM_Track& track0 = writtenData.tracks[0]; + EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + const SfM_Camera& camera0 = writtenData.cameras[0]; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + EXPECT(assert_equal(expected,actual,12)); + + Pose3 expectedPose = camera0.pose(); + Key poseKey = symbol('x',0); + Pose3 actualPose = value.at(poseKey); + EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); + + Point3 expectedPoint = track0.p; + Key pointKey = symbol('l',0); + Point3 actualPoint = value.at(pointKey); + EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp new file mode 100644 index 000000000..34a26adbe --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -0,0 +1,147 @@ +/* + * @file testEssentialMatrixFactor.cpp + * @brief Test EssentialMatrixFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +const string filename = findExampleDataFile("5pointExample1.txt"); +SfM_data data; +bool readOK = readBAL(filename, data); +Rot3 aRb = data.cameras[1].pose().rotation(); +Point3 aTb = data.cameras[1].pose().translation(); + +Point2 pA(size_t i) { + return data.tracks[i].measurements[0].second; +} +Point2 pB(size_t i) { + return data.tracks[i].measurements[1].second; +} +Vector vA(size_t i) { + return EssentialMatrix::Homogeneous(pA(i)); +} +Vector vB(size_t i) { + return EssentialMatrix::Homogeneous(pB(i)); +} + +//************************************************************************* +TEST (EssentialMatrixFactor, testData) { + CHECK(readOK); + + // Check E matrix + Matrix expected(3, 3); + expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; + Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix(); + EXPECT(assert_equal(expected, aEb_matrix,1e-8)); + + // Check some projections + EXPECT(assert_equal(Point2(0,0),pA(0),1e-8)); + EXPECT(assert_equal(Point2(0,0.1),pB(0),1e-8)); + EXPECT(assert_equal(Point2(0,-1),pA(4),1e-8)); + EXPECT(assert_equal(Point2(-1,0.2),pB(4),1e-8)); + + // Check homogeneous version + EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB(4),1e-8)); + + // Check epipolar constraint + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8); + + // Check epipolar constraint + EssentialMatrix trueE(aRb, aTb); + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i),vB(i)), 1e-7); +} + +//************************************************************************* +TEST (EssentialMatrixFactor, factor) { + EssentialMatrix trueE(aRb, aTb); + noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); + + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor factor(1, pA(i), pB(i), model); + + // Check evaluation + Vector expected(1); + expected << 0; + Matrix HActual; + Vector actual = factor.evaluateError(trueE, HActual); + EXPECT(assert_equal(expected, actual, 1e-7)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix HExpected; + HExpected = numericalDerivative11( + boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, + boost::none), trueE); + + // Verify the Jacobian is correct + EXPECT(assert_equal(HExpected, HActual, 1e-8)); + } +} + +//************************************************************************* +TEST (EssentialMatrixFactor, fromConstraints) { + // Here we want to optimize directly on essential matrix constraints + // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, + // but GTSAM does the equivalent anyway, provided we give the right + // factors. In this case, the factors are the constraints. + + // We start with a factor graph and add constraints to it + // Noise sigma is 1cm, assuming metric measurements + NonlinearFactorGraph graph; + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, + 0.01); + for (size_t i = 0; i < 5; i++) + graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model)); + + // Check error at ground truth + Values truth; + EssentialMatrix trueE(aRb, aTb); + truth.insert(1, trueE); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + EssentialMatrix initialE = trueE.retract( + (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); + initial.insert(1, initialE); + EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2); + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(1); + EXPECT(assert_equal(trueE, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i),vB(i)), 1e-6); + +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 0aeadfa6a..fb38d7035 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector_(2,323.,240.); + Vector z = (Vector(2) << 323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -309,7 +309,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { } else { - Vector delta = Vector_(11, + Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, focal_noise, // f_x, f_y diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 6d8f36634..9d15736d0 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -87,7 +87,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector_(2,323.,240.); + Vector z = (Vector(2) << 323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -112,7 +112,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); } @@ -309,7 +309,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { } else { - Vector delta = Vector_(9, + Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 9f5c3f426..b40da2e2a 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -18,8 +18,8 @@ using namespace gtsam; -const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); typedef PoseRotationPrior Pose2RotationPrior; typedef PoseRotationPrior Pose3RotationPrior; @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector_(3, 0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3) << 0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); @@ -62,7 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -84,7 +84,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; - EXPECT(assert_equal(Vector_(1,-M_PI_2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 724e0e7e8..36d94c9a3 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -15,8 +15,8 @@ using namespace gtsam; -const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3)); typedef PoseTranslationPrior Pose2TranslationPrior; typedef PoseTranslationPrior Pose3TranslationPrior; @@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -81,7 +81,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -103,7 +103,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); @@ -125,7 +125,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; - EXPECT(assert_equal(Vector_(2,-3.0,-4.0), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative11( boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index b53fde536..1066f3cd2 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -107,7 +107,7 @@ TEST( ProjectionFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = Vector_(2, -3.0, 0.0); + Vector expectedError = (Vector(2) << -3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -130,7 +130,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = Vector_(2, -3.0, 0.0); + Vector expectedError = (Vector(2) << -3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -153,8 +153,8 @@ TEST( ProjectionFactor, Jacobian ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // 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 H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); + Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); + Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); @@ -179,8 +179,8 @@ TEST( ProjectionFactor, JacobianWithTransform ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // 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 H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376); + Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); + Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 75fe40aa8..a50e1549b 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -117,7 +117,7 @@ TEST( RangeFactor, Error2D ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -142,7 +142,7 @@ TEST( RangeFactor, Error2DWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -164,7 +164,7 @@ TEST( RangeFactor, Error3D ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -189,7 +189,7 @@ TEST( RangeFactor, Error3DWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance - Vector expectedError = Vector_(1, 0.295630141); + Vector expectedError = (Vector(1) << 0.295630141); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp new file mode 100644 index 000000000..715b72b74 --- /dev/null +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -0,0 +1,185 @@ +/* + * @file testRotateFactor.cpp + * @brief Test RotateFactor class + * @author Frank Dellaert + * @date December 17, 2013 + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +//************************************************************************* +// Create some test data +// Let's assume IMU is aligned with aero (X-forward,Z down) +// And camera is looking forward. +Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); +Rot3 iRc(cameraX, cameraY, cameraZ); + +// Now, let's create some rotations around IMU frame +Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); +Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), // +i2Ri3 = Rot3::rodriguez(p2, 1), // +i3Ri4 = Rot3::rodriguez(p3, 1); + +// The corresponding rotations in the camera frame +Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // +c2Zc3 = iRc.inverse() * i2Ri3 * iRc, // +c3Zc4 = iRc.inverse() * i3Ri4 * iRc; + +// The corresponding rotated directions in the camera frame +Sphere2 z1 = iRc.inverse() * p1, // +z2 = iRc.inverse() * p2, // +z3 = iRc.inverse() * p3; + +typedef noiseModel::Isotropic::shared_ptr Model; + +//************************************************************************* +TEST (RotateFactor, checkMath) { + EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1))); + EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1))); + EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1))); +} + +//************************************************************************* +TEST (RotateFactor, test) { + Model model = noiseModel::Isotropic::Sigma(3, 0.01); + RotateFactor f(1, i1Ri2, c1Zc2, model); + EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); + + Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); + f.evaluateError(iRc, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative11( + boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); + f.evaluateError(R, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + +//************************************************************************* +TEST (RotateFactor, minimization) { + // Let's try to recover the correct iRc by minimizing + NonlinearFactorGraph graph; + Model model = noiseModel::Isotropic::Sigma(3, 0.01); + graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); + graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); + graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); + + // Check error at ground truth + Values truth; + truth.insert(1, iRc); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + double degree = M_PI / 180; + Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); + initial.insert(1, initialE); + EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1); + + // Optimize + LevenbergMarquardtParams parameters; + //parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + Rot3 actual = result.at(1); + EXPECT(assert_equal(iRc, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +} + +//************************************************************************* +TEST (RotateDirectionsFactor, test) { + Model model = noiseModel::Isotropic::Sigma(2, 0.01); + RotateDirectionsFactor f(1, p1, z1, model); + EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); + + Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + Vector expectedE = (Vector(2) << -0.08867, -0.20197); + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + boost::none), iRc); + f.evaluateError(iRc, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative11( + boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + boost::none), R); + f.evaluateError(R, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + +//************************************************************************* +TEST (RotateDirectionsFactor, minimization) { + // Let's try to recover the correct iRc by minimizing + NonlinearFactorGraph graph; + Model model = noiseModel::Isotropic::Sigma(2, 0.01); + graph.add(RotateDirectionsFactor(1, p1, z1, model)); + graph.add(RotateDirectionsFactor(1, p2, z2, model)); + graph.add(RotateDirectionsFactor(1, p3, z3, model)); + + // Check error at ground truth + Values truth; + truth.insert(1, iRc); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + double degree = M_PI / 180; + Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); + initial.insert(1, initialE); + EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1); + + // Optimize + LevenbergMarquardtParams parameters; + //parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + Rot3 actual = result.at(1); + EXPECT(assert_equal(iRc, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 3936d8f45..8c36f0746 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -31,7 +31,7 @@ using namespace std; using namespace gtsam; -static Pose3 camera1(Matrix_(3,3, +static Pose3 camera1((Matrix) (Matrix(3, 3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = Vector_(3, -3.0, +2.0, -1.0); + Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = Vector_(3, -3.0, +2.0, -1.0); + Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -144,10 +144,10 @@ TEST( StereoFactor, Jacobian ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // The expected Jacobians - Matrix H1Expected = Matrix_(3, 6, 0.0, -625.0, 0.0, -100.0, 0.0, 0.0, + Matrix H1Expected = (Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0, 0.0, -625.0, 0.0, -100.0, 0.0, -8.0, 625.0, 0.0, 0.0, 0.0, -100.0, 0.0); - Matrix H2Expected = Matrix_(3, 3, 100.0, 0.0, 0.0, + Matrix H2Expected = (Matrix(3, 3) << 100.0, 0.0, 0.0, 100.0, 0.0, 8.0, 0.0, 100.0, 0.0); @@ -172,10 +172,10 @@ TEST( StereoFactor, JacobianWithTransform ) { factor.evaluateError(pose, point, H1Actual, H2Actual); // The expected Jacobians - Matrix H1Expected = Matrix_(3, 6, -100.0, 0.0, 650.0, 0.0, 100.0, 0.0, + Matrix H1Expected = (Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0, -100.0, -8.0, 649.2, -8.0, 100.0, 0.0, -10.0, -650.0, 0.0, 0.0, 0.0, 100.0); - Matrix H2Expected = Matrix_(3, 3, 0.0, -100.0, 0.0, + Matrix H2Expected = (Matrix(3, 3) << 0.0, -100.0, 0.0, 8.0, -100.0, 0.0, 0.0, 0.0, -100.0);