├── .gitignore ├── .pre-commit-config.yaml ├── README.md ├── examples ├── goats_14_data │ ├── goats_14_6_2002_15_20.pkl │ └── gt_traj_A.tum ├── manhattan │ └── factor_graph.pickle └── solve_goats_example_score.py ├── media ├── 20robot_animation.gif ├── 4robot_animation.gif ├── GoatsTrajs_3col_linewidth2.png ├── MultiTrajsV4.png ├── SingleTrajsV5.png └── title_figure.png ├── score ├── __init__.py ├── solve_score.py └── utils │ ├── __init__.py │ ├── circle_utils.py │ ├── gurobi_utils.py │ ├── matrix_utils.py │ └── plot_utils.py ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # OS generated files # 2 | ###################### 3 | .DS_Store 4 | .DS_Store? 5 | ._* 6 | .Spotlight-V100 7 | .Trashes 8 | ehthumbs.db 9 | [Tt]humbs.db 10 | 11 | # mypy 12 | *.sqlite3 13 | .mypy_cache 14 | 15 | # profiling 16 | *.log 17 | *.svg 18 | 19 | # python installations 20 | *.egg-info 21 | 22 | # Jupyter Notebooks # 23 | ##################### 24 | .ipynb_checkpoints 25 | 26 | # Binaries # 27 | ############ 28 | *.pyc 29 | 30 | # Latex # 31 | ######### 32 | *.aux 33 | *.fdb_latexmk 34 | *.fls 35 | *.log 36 | *.out 37 | *.synctex.gz 38 | *.bbl 39 | *.blg 40 | *.bcf 41 | *.run.xml 42 | *.toc 43 | *.dvi 44 | *.lof 45 | *.lot 46 | main.pdf 47 | 48 | # VSCode # 49 | ############# 50 | .history/ 51 | .vscode/ 52 | .history/* 53 | .vscode/* 54 | .markdownlint.json 55 | *.code-workspace 56 | 57 | # spacemacs # 58 | ############# 59 | auto/ 60 | .#* 61 | 62 | # Python # 63 | ########## 64 | __pycache__/ 65 | test.py 66 | 67 | 68 | # Cached Files from Python # 69 | ########## 70 | cached_planning/*.txt 71 | figures/*.png 72 | trajs/*.txt 73 | *scrap* 74 | rigidity_dicts/*.json 75 | 76 | 77 | # PyTorch # 78 | ########### 79 | *.pt 80 | 81 | # Executables/C++ # 82 | ################### 83 | build/ 84 | 85 | 86 | # Tensorflow # 87 | ############## 88 | *.tfrecord 89 | *.pb 90 | 91 | # Temporary Files # 92 | ################### 93 | *.*~ 94 | *.swp 95 | 96 | # Large Files # 97 | ############### 98 | *.png 99 | *.mp4 100 | *.jpg 101 | <<<<<<< HEAD 102 | *.xlsx 103 | ======= 104 | >>>>>>> fe892fc61eb06385eafaa8af2bef73d512f682ff 105 | 106 | # Blender # 107 | ########### 108 | *.blend 109 | 110 | # MATLAB # 111 | ########## 112 | *.m~ 113 | 114 | # ROS # 115 | ####### 116 | *.bag 117 | 118 | <<<<<<< HEAD 119 | 120 | ======= 121 | >>>>>>> fe892fc61eb06385eafaa8af2bef73d512f682ff 122 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v2.3.0 4 | hooks: 5 | - id: check-yaml 6 | - id: end-of-file-fixer 7 | - id: trailing-whitespace 8 | - repo: https://github.com/psf/black 9 | rev: 22.3.0 10 | hooks: 11 | - id: black 12 | - repo: https://github.com/pre-commit/mirrors-mypy 13 | rev: v0.910 14 | hooks: 15 | - id: mypy 16 | additional_dependencies: [attrs==23.1.0] 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SCORE: Second Order Conic Initialization for RA-SLAM 2 | 3 | Code to solve a second-order cone program to initialize a local-search solver 4 | for the range-aided SLAM problem. The SOCP is a convex relaxation of the 5 | original problem. 6 | 7 | Check out the extended version of our [paper](https://arxiv.org/abs/2210.03177) or our [short video summary](https://www.youtube.com/watch?v=M1K6GTNm0fI&ab_channel=MITMarineRoboticsGroup). 8 | 9 | 10 | 11 | 12 | 13 | ## Results 14 | 15 | We show the key results from our paper, comparing SCORE to a range of other initialization strategies. 16 | 17 | ### Real-World AUV Experiments 18 | 19 | 20 | 21 | - SCORE: our method, using a second-order cone program for initialization 22 | - Odom: initializing with robot odometry 23 | 24 | ### Simulated Multi-Robot Experiments 25 | 26 | 27 | 28 | - SCORE: our method, using a second-order cone program for initialization 29 | - SCORE Init: the estimate returned by SCORE (before refining via local-search) 30 | - Odom-R: initializing with robot odometry, randomizing the first pose of each robot 31 | - Odom-P: initializing with robot odometry, initializing with the true first pose for each robot 32 | - GT-Init: initializing with the ground-truth values (when available) 33 | 34 | 35 | ### Simulated Single-Robot Experiments 36 | 37 | 38 | 39 | - SCORE: our method, using a second-order cone program for initialization 40 | - Odom: initializing with robot odometry 41 | - GT-Init: initializing with the ground-truth values (when available) 42 | 43 | ## Usage 44 | 45 | Feel free to look inside our `/examples` directory! 46 | 47 | ## Dependencies 48 | 49 | ### PyFactorGraph (required) 50 | 51 | [Link to the repo](https://github.com/MarineRoboticsGroup/PyFactorGraph) 52 | 53 | This holds all of the measurements/variables to define our RA-SLAM problem. 54 | This is a custom library developed in the Marine Robotics Group at MIT to 55 | interface with a broader range of SLAM file types (e.g. g2o). You can install 56 | directly from source via: 57 | 58 | ```bash 59 | cd ~//PyFactorGraph 60 | pip install . 61 | ``` 62 | 63 | ### GTSAM (optional) 64 | 65 | We used GTSAM to refine our initial estimates provided by SCORE in the experiments in our paper. 66 | 67 | `pip install gtsam` 68 | -------------------------------------------------------------------------------- /examples/goats_14_data/goats_14_6_2002_15_20.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/examples/goats_14_data/goats_14_6_2002_15_20.pkl -------------------------------------------------------------------------------- /examples/goats_14_data/gt_traj_A.tum: -------------------------------------------------------------------------------- 1 | 0 499.21 334.039 0 0.0 0.0 0.9203136393917676 0.3911812944779946 2 | 1 500.81300000000005 333.603 0 0.0 0.0 0.9201320468717459 0.3916082434265286 3 | 2 504.715 333.293 0 0.0 0.0 0.915962740225581 0.40126332815053617 4 | 3 504.426 332.903 0 0.0 0.0 0.9114548453172743 0.4114001275494012 5 | 4 505.43300000000005 332.712 0 0.0 0.0 0.9118117540821234 0.4106084815462062 6 | 5 506.039 332.381 0 0.0 0.0 0.9083349741159766 0.4182434396350146 7 | 6 506.43300000000005 332.134 0 0.0 0.0 0.905460271377079 0.4244310273268751 8 | 7 506.88599999999997 331.87 0 0.0 0.0 0.9046725870227322 0.4261073929065266 9 | 8 507.833 331.599 0 0.0 0.0 0.9063417016248778 0.42254552405122103 10 | 9 508.458 331.42699999999996 0 0.0 0.0 0.9009412893160283 0.4339410019871049 11 | 10 509.69 330.784 0 0.0 0.0 0.9001620865461861 0.43555506878558603 12 | 11 509.79900000000004 330.412 0 0.0 0.0 0.898491620658471 0.43899066915654866 13 | 12 510.278 330.24699999999996 0 0.0 0.0 0.9006399244914035 0.4345661358321871 14 | 13 510.42900000000003 330.036 0 0.0 0.0 0.8977538039990407 0.4404975679901673 15 | 14 510.76599999999996 329.69 0 0.0 0.0 0.8947501170573066 0.4465671595918538 16 | 15 511.251 329.505 0 0.0 0.0 0.8906040693983551 0.4547794977470838 17 | 16 511.87300000000005 329.171 0 0.0 0.0 0.8902606370212456 0.45545142240476755 18 | 17 511.526 328.485 0 0.0 0.0 0.8928823890282004 0.45028995032455865 19 | 18 512.286 328.276 0 0.0 0.0 0.8927274007257839 0.450597146011141 20 | 19 512.636 328.17699999999996 0 0.0 0.0 0.8911826265205031 0.4536447136118943 21 | 20 513.399 327.728 0 0.0 0.0 0.8929856554614259 0.4500851243266406 22 | 21 511.833 326.128 0 0.0 0.0 0.8911552395619804 0.4536985111296149 23 | 22 513.1569999999999 326.121 0 0.0 0.0 0.8878506928367433 0.4601316629284654 24 | 23 512.953 325.645 0 0.0 0.0 0.8906836728622441 0.4546235749459358 25 | 24 513.889 325.64799999999997 0 0.0 0.0 0.8920218163784704 0.45199234407770045 26 | 25 514.497 325.536 0 0.0 0.0 0.8909990715459345 0.4540051260770992 27 | 26 515.457 325.486 0 0.0 0.0 0.8951648397578219 0.4457352461510679 28 | 27 515.94 325.258 0 0.0 0.0 0.8914699923890155 0.4530797420652668 29 | 28 516.592 324.894 0 0.0 0.0 0.8957049612473114 0.44464887540277487 30 | 29 517.261 324.834 0 0.0 0.0 0.8970134378242984 0.44200327189132155 31 | 30 517.785 324.99 0 0.0 0.0 0.8956808051090108 0.4446975324411799 32 | 31 518.118 324.558 0 0.0 0.0 0.8937938415338536 0.44847805836646754 33 | 32 518.516 324.066 0 0.0 0.0 0.8914699923890155 0.4530797420652668 34 | 33 519.444 323.547 0 0.0 0.0 0.8882226369485582 0.4594132640794667 35 | 34 519.923 323.26099999999997 0 0.0 0.0 0.8899442292091639 0.4560693685146011 36 | 35 519.9019999999999 322.758 0 0.0 0.0 0.8867815934872925 0.462188711947986 37 | 36 520.469 322.60400000000004 0 0.0 0.0 0.8890200635285717 0.4578682415757338 38 | 37 520.939 322.309 0 0.0 0.0 0.8867815934872925 0.462188711947986 39 | 38 521.197 322.04400000000004 0 0.0 0.0 0.8893101206444287 0.4573046132714951 40 | 39 520.979 321.312 0 0.0 0.0 0.8876895212032931 0.46044251969802724 41 | 40 520.4159999999999 320.341 0 0.0 0.0 0.8904997198766458 0.4549837897108154 42 | 41 521.557 320.263 0 0.0 0.0 0.8611049472382527 0.5084272512777083 43 | 42 522.036 319.83799999999997 0 0.0 0.0 0.7957762780657791 0.6055907159689421 44 | 43 520.58 319.11400000000003 0 0.0 0.0 0.6894318791354519 0.7243505256654127 45 | 44 519.869 318.718 0 0.0 0.0 0.621097920384507 0.7837329732083758 46 | 45 516.493 317.73400000000004 0 0.0 0.0 0.429566127966605 0.9030354044575319 47 | 46 513.98 317.507 0 0.0 0.0 0.3183709851499561 0.947966199721618 48 | 47 512.185 319.282 0 0.0 0.0 0.25362095561296794 0.9673036807921104 49 | 48 508.564 323.41700000000003 0 0.0 0.0 0.1263909045344664 0.9919805135439704 50 | 49 507.21 325.92400000000004 0 0.0 0.0 0.07169981907420936 0.9974262558929987 51 | 50 506.43300000000005 329.251 0 0.0 0.0 0.06216662843665482 0.9980657845597247 52 | 51 506.92900000000003 334.04900000000004 0 0.0 0.0 0.12931153483046287 0.9916040172164441 53 | 52 506.57 336.79400000000004 0 0.0 0.0 0.26702754418098545 0.9636888972322302 54 | 53 506.154 338.98800000000006 0 0.0 0.0 0.3288177557462921 0.9443933944633305 55 | 54 505.18300000000005 341.449 0 0.0 0.0 0.33097475604011006 0.9436396085710845 56 | 55 504.50300000000004 343.539 0 0.0 0.0 0.26287438307574507 0.9648300672774177 57 | 56 502.4 346.222 0 0.0 0.0 0.14101982835248622 0.9900067716997876 58 | 57 501.25 349.311 0 0.0 0.0 0.07789221548388628 0.9969617860113856 59 | 58 500.281 352.561 0 0.0 0.0 0.07884948088852839 0.9968865328429357 60 | 59 499.277 356.465 0 0.0 0.0 0.15472996080031198 0.9879568002856876 61 | 60 498.69699999999995 359.89 0 0.0 0.0 0.24428339411777866 0.9697038843679539 62 | 61 497.06699999999995 363.12 0 0.0 0.0 0.3295845750408896 0.9441260550875165 63 | 62 494.494 365.844 0 0.0 0.0 0.36101799952086155 0.9325588474846803 64 | 63 487.791 369.73800000000006 0 0.0 0.0 0.3014135465597495 0.9534935101773235 65 | 64 484.98199999999997 371.583 0 0.0 0.0 0.2467116500798971 0.9690889338522314 66 | 65 481.31699999999995 375.556 0 0.0 0.0 0.19714685510671925 0.980373968198631 67 | 66 437.14099999999996 418.778 0 0.0 0.0 0.2927482404038963 0.9561895563853553 68 | 67 442.166 427.585 0 0.0 0.0 0.29030792100705705 0.9569332845086748 69 | 68 442.37699999999995 433.209 0 0.0 0.0 0.3121421161220447 0.9500354200464591 70 | 69 440.722 437.19699999999995 0 0.0 0.0 0.36446597637978506 0.9312167052096574 71 | 70 440.464 442.048 0 0.0 0.0 0.39760612020961444 0.9175561961928314 72 | 71 438.166 445.079 0 0.0 0.0 0.3430037818696144 0.939334022392004 73 | 72 435.855 448.034 0 0.0 0.0 0.27084828163548114 0.9626220485398759 74 | 73 431.99300000000005 451.13599999999997 0 0.0 0.0 0.18150128786297046 0.9833907069441338 75 | 74 429.76300000000003 455.30199999999996 0 0.0 0.0 0.09747100579300294 0.995238364930583 76 | 75 425.991 460.35 0 0.0 0.0 0.11270395573793307 0.9936286118872695 77 | 76 424.834 466.73400000000004 0 0.0 0.0 0.28652826232433337 0.9580717900498887 78 | 77 423.76 470.381 0 0.0 0.0 0.3379236721258094 0.9411735184422735 79 | 78 422.884 473.92 0 0.0 0.0 0.3222045795091239 0.9466700634029518 80 | 79 419.644 477.014 0 0.0 0.0 0.28294077516837335 0.959137382102856 81 | 80 417.465 480.195 0 0.0 0.0 0.2331041562940168 0.9724517737751598 82 | 81 415.051 483.03 0 0.0 0.0 0.2093889651208767 0.9778324300643788 83 | 82 414.163 486.786 0 0.0 0.0 0.24899812368731575 0.9685039671576964 84 | 83 411.89099999999996 489.972 0 0.0 0.0 0.3213418679808949 0.9469632537129142 85 | 84 407.526 494.844 0 0.0 0.0 0.39880114083513374 0.9170374311164162 86 | 85 404.71 498.095 0 0.0 0.0 0.3527786036860623 0.935706822023497 87 | 86 398.324 503.56 0 0.0 0.0 0.18978859243595175 0.9818249794037531 88 | 87 397.227 508.05400000000003 0 0.0 0.0 0.1751555387201042 0.9845407748060362 89 | 88 396.425 512.008 0 0.0 0.0 0.24061805575097572 0.9706198798946065 90 | 89 393.439 515.98 0 0.0 0.0 0.3876710516463379 0.9217977846119084 91 | 90 391.786 518.752 0 0.0 0.0 0.3927844643112224 0.9196305587548437 92 | 91 389.777 521.933 0 0.0 0.0 0.354359652302097 0.9351092111728645 93 | 92 385.712 526.1080000000001 0 0.0 0.0 0.27916339632380766 0.9602436139610389 94 | 93 382.93199999999996 529.9490000000001 0 0.0 0.0 0.2047061115656844 0.9788234814754178 95 | 94 381.904 533.696 0 0.0 0.0 0.22294175592290053 0.9748317667505577 96 | 95 380.281 537.937 0 0.0 0.0 0.3153043763357693 0.9489905954557777 97 | 96 378.897 539.599 0 0.0 0.0 0.3857116810720668 0.922619368474649 98 | 97 376.329 543.028 0 0.0 0.0 0.4250998967925446 0.9051464399460277 99 | 98 369.281 547.6419999999999 0 0.0 0.0 0.3826996002818755 0.9238728353751358 100 | 99 365.582 550.899 0 0.0 0.0 0.30160483857762144 0.9534330188044501 101 | 100 363.105 554.511 0 0.0 0.0 0.23800147708300226 0.9712647923745147 102 | 101 361.23199999999997 556.73 0 0.0 0.0 0.2540978160869505 0.96717852533017 103 | 102 359.537 561.09 0 0.0 0.0 0.34079942782425426 0.9401360273889416 104 | 103 357.36199999999997 564.631 0 0.0 0.0 0.4133451330540408 0.9105744346183553 105 | 104 346.49800000000005 570.966 0 0.0 0.0 0.3663807292789184 0.9304650241750347 106 | 105 344.158 575.113 0 0.0 0.0 0.31381915162858126 0.9494827750260231 107 | 106 339.13300000000004 580.864 0 0.0 0.0 0.34597477045812 0.9382438159702686 108 | 107 333.415 586.376 0 0.0 0.0 0.47799161736956897 0.878364396890279 109 | 108 330.805 589.099 0 0.0 0.0 0.4794014853659182 0.8775957018063336 110 | 109 327.75300000000004 591.231 0 0.0 0.0 0.44976016104869804 0.8931493702250757 111 | 110 321.231 595.493 0 0.0 0.0 0.3598205771003831 0.9330215175949306 112 | 111 320.539 597.83 0 0.0 0.0 0.35579670397007823 0.9345633769006939 113 | 112 318.589 600.686 0 0.0 0.0 0.4213893432042134 0.9068798274489965 114 | 113 316.562 602.952 0 0.0 0.0 0.4874132147525873 0.873171436823576 115 | 114 313.737 604.7280000000001 0 0.0 0.0 0.5221126314482514 0.8528765444553992 116 | 115 305.221 607.7130000000001 0 0.0 0.0 0.4597781850333953 0.8880337947214605 117 | 116 301.837 610.531 0 0.0 0.0 0.3859510140923284 0.9225192760702094 118 | 117 299.341 613.71 0 0.0 0.0 0.3362945590833911 0.9417568526593834 119 | 118 296.698 615.53 0 0.0 0.0 0.3669075721925199 0.9302574017269579 120 | 119 294.01599999999996 618.807 0 0.0 0.0 0.45845407056351284 0.8887181022032495 121 | 120 290.791 620.847 0 0.0 0.0 0.5359356930458893 0.8442588068355712 122 | 121 286.712 622.868 0 0.0 0.0 0.5642095263372586 0.8256316432830602 123 | 122 282.094 624.753 0 0.0 0.0 0.5563165574135762 0.830970449503174 124 | 123 273.70599999999996 626.566 0 0.0 0.0 0.47799161736956897 0.878364396890279 125 | 124 271.726 630.528 0 0.0 0.0 0.44227917537930256 0.8968774336696202 126 | 125 266.83099999999996 633.106 0 0.0 0.0 0.7679247820440672 0.6405400292897954 127 | 126 261.059 633.2959999999999 0 0.0 0.0 0.8950087369485659 0.4460486080975175 128 | 127 257.267 630.95 0 0.0 0.0 0.9594817030425673 0.2817709380446014 129 | 128 255.167 628.174 0 0.0 0.0 0.9892869016259159 0.14598433570556563 130 | 129 253.51 628.533 0 0.0 0.0 0.9999965612583008 0.002622493388646735 131 | 130 250.997 621.844 0 0.0 0.0 -0.992077211844831 0.12562963718082695 132 | 131 252.11900000000003 616.91 0 0.0 0.0 -0.9904306504013588 0.1380113283232264 133 | 132 253.31900000000002 607.105 0 0.0 0.0 -0.9972012076521605 0.074764640419604 134 | 133 255.90400000000002 601.618 0 0.0 0.0 0.9992922689925061 -0.03761596910633595 135 | 134 256.878 598.456 0 0.0 0.0 0.9995553485825774 -0.0298178657848284 136 | 135 257.91 595.592 0 0.0 0.0 0.9999906910542228 -0.004314835442751917 137 | 136 258.038 590.941 0 0.0 0.0 0.9996499887236292 0.02645562406838955 138 | 137 257.587 586.8430000000001 0 0.0 0.0 0.9984318951610874 0.05597991358549321 139 | 138 257.131 581.469 0 0.0 0.0 0.9944950379858724 0.10478367917513866 140 | 139 256.155 579.231 0 0.0 0.0 0.9919223180029855 0.12684681725445157 141 | 140 253.628 571.486 0 0.0 0.0 0.9992090464799027 0.039765329531183384 142 | 141 252.766 569.303 0 0.0 0.0 0.9999380946704947 -0.011126851609542613 143 | 142 251.44299999999998 564.344 0 0.0 0.0 0.9985548558732542 -0.05374197439566681 144 | 143 251.085 559.3530000000001 0 0.0 0.0 -0.9976981331032331 0.0678117630085183 145 | 144 251.44400000000002 554.481 0 0.0 0.0 -0.9955130587918292 0.09462425574310254 146 | 145 251.791 550.0269999999999 0 0.0 0.0 -0.9902557129838221 0.13926098844580334 147 | 146 252.753 536.624 0 0.0 0.0 -0.9859494943677485 0.1670436905602276 148 | 147 254.43599999999998 534.246 0 0.0 0.0 -0.9908902515443928 0.13467185821206284 149 | 148 255.416 531.097 0 0.0 0.0 -0.9949788508632653 0.100085395212369 150 | 149 256.66900000000004 529.107 0 0.0 0.0 -0.9978132607350563 0.0660961171421938 151 | 150 257.525 524.659 0 0.0 0.0 0.9998133207069613 0.019321587329174623 152 | 151 258.235 519.811 0 0.0 0.0 0.9908576980352023 0.1349111642688624 153 | 152 257.225 516.0790000000001 0 0.0 0.0 0.9889223454059747 0.14843380597675881 154 | 153 254.65900000000002 509.04699999999997 0 0.0 0.0 0.9971752299050636 0.07511032461508634 155 | 154 253.088 505.167 0 0.0 0.0 0.9997266565465578 0.023379738883079603 156 | 155 251.93099999999998 500.839 0 0.0 0.0 0.9994276284625059 -0.0338292102451594 157 | 156 250.899 496.949 0 0.0 0.0 -0.9943933495043358 0.10574434482064683 158 | 157 250.037 489.908 0 0.0 0.0 -0.9854134358922823 0.1701774378787238 159 | 158 252.206 478.07 0 0.0 0.0 -0.9903578166087226 0.13853301080249367 160 | 159 253.81599999999997 473.65 0 0.0 0.0 -0.996055684636009 0.08873033925490971 161 | 160 254.578 466.94199999999995 0 0.0 0.0 0.9999425522148662 0.010718781181628915 162 | 161 255.96400000000003 463.158 0 0.0 0.0 0.9999242700867618 0.012306668576699601 163 | 162 256.14099999999996 458.93699999999995 0 0.0 0.0 0.9999588271225225 0.009074362773721808 164 | 163 256.478 455.05 0 0.0 0.0 0.9999870965051296 0.00508004165735664 165 | 164 255.523 449.845 0 0.0 0.0 0.9999747571662465 0.007105281859741637 166 | 165 254.747 446.464 0 0.0 0.0 0.9999933018801314 0.003660081265837023 167 | 166 253.37 438.70599999999996 0 0.0 0.0 0.9993005253880757 -0.03739598854310382 168 | 167 253.428 431.509 0 0.0 0.0 -0.9963666232994594 0.08516778718995353 169 | 168 254.64700000000002 426.52099999999996 0 0.0 0.0 -0.9958252610898167 0.09128006011938361 170 | 169 254.61900000000003 423.736 0 0.0 0.0 -0.9967994180205898 0.07994323131956538 171 | 170 256.08799999999997 418.30800000000005 0 0.0 0.0 0.9996808062849679 -0.02526431367435889 172 | 171 256.08299999999997 416.57 0 0.0 0.0 0.999829852591581 0.018446296828834727 173 | 172 256.907 410.592 0 0.0 0.0 0.9959098783015436 0.0903521682163995 174 | 173 256.914 407.43699999999995 0 0.0 0.0 0.996756007948069 0.08048267279004338 175 | 174 255.511 403.311 0 0.0 0.0 0.9985172307283549 0.054436568027174996 176 | 175 255.153 400.477 0 0.0 0.0 0.999312595291587 0.03707205000526448 177 | 176 254.41400000000002 395.26199999999994 0 0.0 0.0 0.9999744531521715 -0.007147939774206417 178 | 177 251.824 386.416 0 0.0 0.0 -0.9934542302003957 0.11423087365917851 179 | 178 252.18099999999998 382.406 0 0.0 0.0 -0.9866122847545421 0.1630834129254792 180 | 179 252.945 378.9 0 0.0 0.0 -0.9871765839794792 0.15963205205285816 181 | 180 255.773 376.338 0 0.0 0.0 -0.9912898748702171 0.13169807887660798 182 | 181 256.664 372.395 0 0.0 0.0 -0.9965109140942041 0.08346255502399667 183 | 182 257.56 364.394 0 0.0 0.0 0.9982471535025799 0.05918294115703274 184 | 183 257.529 360.55300000000005 0 0.0 0.0 0.9943004596588043 0.10661423883464359 185 | 184 255.355 353.06800000000004 0 0.0 0.0 0.9922671297566195 0.12412068000361628 186 | 185 251.71900000000002 342.32800000000003 0 0.0 0.0 0.9999642496475507 -0.008455733369198115 187 | 186 250.457 334.694 0 0.0 0.0 -0.9923407916136017 0.12353037399639964 188 | 187 250.299 331.769 0 0.0 0.0 -0.9899455561325415 0.14144891619744948 189 | 188 250.736 327.727 0 0.0 0.0 -0.9890585510436852 0.14752349849215887 190 | 189 252.123 319.23900000000003 0 0.0 0.0 -0.992746715303457 0.12022462000021822 191 | 190 252.986 311.472 0 0.0 0.0 -0.9962010750179061 0.08708282341063743 192 | 191 253.11700000000002 308.19599999999997 0 0.0 0.0 -0.9969888966187075 0.0775444389947613 193 | 192 253.77900000000002 304.439 0 0.0 0.0 -0.9979942025226578 0.06330538469328024 194 | 193 254.37 298.317 0 0.0 0.0 0.9990881909317911 -0.04269410662656943 195 | 194 254.144 294.79200000000003 0 0.0 0.0 0.9999699627586945 -0.007750714829947132 196 | 195 257.306 291.219 0 0.0 0.0 0.9989007611106611 0.04687504082709657 197 | 196 257.785 286.751 0 0.0 0.0 0.9911949847468629 0.132410355383053 198 | 197 257.59 282.816 0 0.0 0.0 0.9763682341356459 0.21611356128397052 199 | 198 256.578 279.093 0 0.0 0.0 0.996791370426457 0.08004351218772053 200 | 199 252.75799999999998 269.618 0 0.0 0.0 -0.9113871995419519 0.4115499635658814 201 | 200 258.251 263.685 0 0.0 0.0 -0.7529438677068083 0.6580847453654524 202 | 201 270.644 263.878 0 0.0 0.0 -0.4581013723811079 0.8888999564757248 203 | 202 273.29900000000004 266.082 0 0.0 0.0 -0.4931563236312129 0.8699407108892803 204 | 203 278.20099999999996 270.48900000000003 0 0.0 0.0 -0.6869475384974275 0.7267070106668335 205 | 204 281.639 273.475 0 0.0 0.0 -0.770122327996294 0.6378962297439675 206 | 205 284.28700000000003 273.94599999999997 0 0.0 0.0 -0.8071058386117655 0.5904067794993538 207 | 206 289.778 273.066 0 0.0 0.0 -0.8222451353569753 0.5691334969774573 208 | 207 294.015 270.644 0 0.0 0.0 -0.8080996700731077 0.5890457734571521 209 | 208 294.27299999999997 268.0 0 0.0 0.0 -0.7925691645203127 0.6097820261794977 210 | 209 300.68 266.95799999999997 0 0.0 0.0 -0.7553205885403891 0.6553554825642341 211 | 210 306.256 266.497 0 0.0 0.0 -0.7233304922889963 0.6905019905293238 212 | 211 310.08 265.844 0 0.0 0.0 -0.6893030591122392 0.7244731138548268 213 | 212 314.126 265.282 0 0.0 0.0 -0.5840152396589432 0.8117426931276358 214 | 213 318.942 265.709 0 0.0 0.0 -0.370727888398984 0.9287415317316386 215 | 214 324.899 272.209 0 0.0 0.0 -0.08599000481775561 0.9962959997267089 216 | 215 324.281 280.538 0 0.0 0.0 -0.03105612174726064 0.9995176423165423 217 | 216 323.901 285.151 0 0.0 0.0 -0.05888578479970141 0.9982647265873533 218 | 217 323.113 290.019 0 0.0 0.0 -0.1357526615607022 0.9907427591858471 219 | 218 322.73900000000003 295.025 0 0.0 0.0 -0.19341663790797492 0.9811167128228809 220 | 219 322.931 296.863 0 0.0 0.0 -0.22054657932274682 0.9753764434048197 221 | 220 324.142 301.264 0 0.0 0.0 -0.2501227474256964 0.968214135003317 222 | 221 326.265 306.738 0 0.0 0.0 -0.2259820949323193 0.9741314555900554 223 | 222 328.805 313.168 0 0.0 0.0 -0.10652688388397437 0.9943098224446796 224 | 223 330.083 317.29 0 0.0 0.0 -0.020543375866415584 0.9997889625856105 225 | 224 329.531 324.98900000000003 0 0.0 0.0 0.022069558876239518 0.9997564376241886 226 | 225 328.35 328.212 0 0.0 0.0 -0.016735471372653778 0.9998599521922733 227 | 226 328.847 331.211 0 0.0 0.0 -0.05622331426449843 0.9984182184501219 228 | 227 328.231 339.009 0 0.0 0.0 -0.14961547763198083 0.9887442585688951 229 | 228 330.33 348.037 0 0.0 0.0 -0.12617393158479134 0.9920081345374323 230 | 229 331.95599999999996 349.293 0 0.0 0.0 -0.10464261793597175 0.9945098906051695 231 | 230 333.452 353.575 0 0.0 0.0 -0.027179934997253977 0.9996305573228267 232 | 231 334.88300000000004 358.218 0 0.0 0.0 0.022560163425399286 0.9997454871247078 233 | 232 334.959 361.43800000000005 0 0.0 0.0 0.05596585057426645 0.9984326835443134 234 | 233 334.467 364.94 0 0.0 0.0 0.06488672913185428 0.9978926356991364 235 | 234 332.88599999999997 369.296 0 0.0 0.0 0.05388734427363248 0.9985470214902926 236 | 235 331.538 377.1 0 0.0 0.0 -0.05849944769477068 0.9982874408803342 237 | 236 331.99300000000005 386.73800000000006 0 0.0 0.0 -0.10342728923066292 0.9946370171285587 238 | 237 333.709 391.089 0 0.0 0.0 -0.05493379206988763 0.9984899991931929 239 | 238 335.30199999999996 394.461 0 0.0 0.0 -0.01721748584877303 0.9998517681041762 240 | 239 335.82599999999996 399.592 0 0.0 0.0 0.017349250936981725 0.9998494904193959 241 | 240 336.649 401.725 0 0.0 0.0 0.0315841673823608 0.9995010957326476 242 | 241 336.039 406.994 0 0.0 0.0 0.08748132698452545 0.9961661595482084 243 | 242 336.03 409.736 0 0.0 0.0 0.10735682155827352 0.9942205554427573 244 | 243 334.384 413.39300000000003 0 0.0 0.0 0.09118901561297338 0.9958336022807911 245 | 244 333.524 416.605 0 0.0 0.0 0.035392119669370406 0.9993735026832105 246 | 245 332.19800000000004 420.819 0 0.0 0.0 -0.03956654080189945 0.9992169378313058 247 | 246 331.666 424.189 0 0.0 0.0 -0.09620261691663108 0.9953617716681669 248 | 247 332.38300000000004 428.75699999999995 0 0.0 0.0 -0.1656771857833191 0.9861800393999666 249 | 248 334.194 432.583 0 0.0 0.0 -0.1974330149839564 0.9803163798459887 250 | 249 336.3 435.284 0 0.0 0.0 -0.16895249959683387 0.9856241945488057 251 | 250 338.379 438.466 0 0.0 0.0 -0.08518583077055268 0.9963650807991671 252 | 251 339.89300000000003 442.191 0 0.0 0.0 0.009266672122608552 0.9999570634721133 253 | 252 339.267 446.98800000000006 0 0.0 0.0 0.0816780587652967 0.9966587654339537 254 | 253 330.88300000000004 463.64099999999996 0 0.0 0.0 -0.032838925061058524 0.999460657055011 255 | 254 333.48199999999997 470.42400000000004 0 0.0 0.0 -0.055879493851520065 0.9984375204122179 256 | 255 332.91 475.876 0 0.0 0.0 -0.06501326887472982 0.9978843995524843 257 | 256 334.394 480.10900000000004 0 0.0 0.0 -0.04285686004223937 0.9990812226977944 258 | 257 333.71 485.519 0 0.0 0.0 -0.014675518258664026 0.9998923087832208 259 | 258 333.38699999999994 488.923 0 0.0 0.0 0.018639619703284743 0.9998262671971151 260 | 259 335.36300000000006 492.458 0 0.0 0.0 0.06012795109450425 0.9981906779254036 261 | 260 334.50199999999995 498.836 0 0.0 0.0 0.0665200565899706 0.9977850881183117 262 | 261 332.82800000000003 506.11 0 0.0 0.0 -0.0035527480076832465 0.9999936889708824 263 | 262 332.311 510.70599999999996 0 0.0 0.0 -0.06360098636407298 0.9979754077799298 264 | 263 332.971 514.526 0 0.0 0.0 -0.11941317465059481 0.9928446473239741 265 | 264 333.556 518.361 0 0.0 0.0 -0.13678179422047024 0.9906012016799843 266 | 265 335.436 523.644 0 0.0 0.0 -0.12314924653967828 0.9923881614956466 267 | 266 338.463 526.7919999999999 0 0.0 0.0 -0.0699696122260326 0.9975491232841311 268 | 267 337.735 530.855 0 0.0 0.0 0.053029102912131094 0.9985929672515897 269 | 268 338.035 534.362 0 0.0 0.0 0.1027543864920083 0.9947067588272691 270 | 269 336.23699999999997 538.174 0 0.0 0.0 0.09215134431412816 0.9957450124108577 271 | 270 335.17900000000003 541.529 0 0.0 0.0 0.0466255818612755 0.9989124361604963 272 | 271 335.67900000000003 543.207 0 0.0 0.0 -0.026220792028905388 0.9996561759251912 273 | 272 334.254 547.736 0 0.0 0.0 -0.1108324697436214 0.9938391035024378 274 | 273 336.37199999999996 553.877 0 0.0 0.0 -0.10895263776969918 0.9940469419112081 275 | 274 337.756 557.2280000000001 0 0.0 0.0 -0.06719391722359008 0.9977399347967131 276 | 275 339.538 561.278 0 0.0 0.0 0.016192711604534407 0.999868889450458 277 | 276 338.68300000000005 569.635 0 0.0 0.0 0.1576745948571794 0.9874911250925875 278 | 277 334.374 577.004 0 0.0 0.0 0.16488259948264744 0.9863131999460643 279 | 278 329.98400000000004 584.427 0 0.0 0.0 0.05379689017754656 0.9985518988050771 280 | 279 327.842 590.066 0 0.0 0.0 -0.11379446959673808 0.9935043123656772 281 | 280 328.916 599.64 0 0.0 0.0 -0.18459437779091267 0.9828147921597363 282 | 281 331.45599999999996 605.21 0 0.0 0.0 -0.16044801428746613 0.9870442921729546 283 | 282 334.35400000000004 607.43 0 0.0 0.0 -0.11099947428787164 0.993820465027671 284 | 283 337.347 611.119 0 0.0 0.0 -0.03022906962647291 0.9995429972490018 285 | 284 338.83099999999996 619.007 0 0.0 0.0 0.09109729124828275 0.9958419972702626 286 | 285 336.01800000000003 626.414 0 0.0 0.0 0.36494443517411457 0.9310293009542968 287 | 286 323.945 631.859 0 0.0 0.0 0.8837406650556007 0.46797696196189464 288 | 287 322.598 629.52 0 0.0 0.0 0.9597556989656577 0.28083624819980385 289 | 288 320.246 627.177 0 0.0 0.0 0.9943778397263353 0.10589009330993406 290 | 289 320.626 622.461 0 0.0 0.0 0.9999997861775706 -0.0006539455735842432 291 | 290 319.647 612.053 0 0.0 0.0 -0.9904669686088273 0.1377504413598744 292 | 291 321.85400000000004 605.518 0 0.0 0.0 -0.9921565969073024 0.12500114884800312 293 | 292 324.21 598.626 0 0.0 0.0 -0.9970346312996583 0.07695416810774032 294 | 293 325.82599999999996 590.54 0 0.0 0.0 0.9995106082213644 -0.03128168878047009 295 | 294 326.371 582.362 0 0.0 0.0 0.9999748177057101 -0.00709675661354182 296 | 295 327.103 578.5319999999999 0 0.0 0.0 0.9999479716244245 -0.01020068841790064 297 | 296 327.161 574.715 0 0.0 0.0 0.9999380946704947 -0.011126851609542613 298 | 297 326.829 572.191 0 0.0 0.0 0.9999440153405067 -0.010581407501107516 299 | 298 326.841 568.3340000000001 0 0.0 0.0 0.9999909349933601 -0.004257925681059939 300 | 299 327.068 562.5930000000001 0 0.0 0.0 0.9999998203898927 0.0005993497997734245 301 | 300 326.525 557.054 0 0.0 0.0 0.999999998441601 -5.5828290029395094e-05 302 | 301 326.238 554.194 0 0.0 0.0 0.999999616240291 -0.0008760817716748446 303 | 302 325.252 550.356 0 0.0 0.0 0.9999940888987515 -0.0034383379059844984 304 | 303 323.282 542.242 0 0.0 0.0 0.9997167865493112 -0.02379803965706204 305 | 304 323.936 540.317 0 0.0 0.0 0.9987234008012765 -0.05051305466839944 306 | 305 324.225 536.232 0 0.0 0.0 -0.9969767586615322 0.07770033905167283 307 | 306 325.183 533.419 0 0.0 0.0 -0.996421007360198 0.08452914344347856 308 | 307 325.95799999999997 529.053 0 0.0 0.0 -0.9962706101862763 0.08628366751051313 309 | 308 326.606 525.052 0 0.0 0.0 -0.9974324275386297 0.07161391271531056 310 | 309 326.99 519.414 0 0.0 0.0 0.999876695355353 -0.015703314467296142 311 | 310 326.777 515.587 0 0.0 0.0 0.9994241264248955 0.03393251419854456 312 | 311 326.564 510.978 0 0.0 0.0 0.9981641936428297 0.06056601794207303 313 | 312 325.598 504.491 0 0.0 0.0 0.9992563166538904 0.03855922233785256 314 | 313 324.505 500.098 0 0.0 0.0 0.9998495100404489 0.01734812012508476 315 | 314 322.777 491.038 0 0.0 0.0 0.998469678760404 -0.05530190409104915 316 | 315 322.76599999999996 487.14599999999996 0 0.0 0.0 -0.9961265088313199 0.08793166894541661 317 | 316 322.83 483.603 0 0.0 0.0 -0.9956379031875454 0.09330147767483615 318 | 317 323.48900000000003 478.944 0 0.0 0.0 -0.9954216235747163 0.09558133353262915 319 | 318 324.697 471.57800000000003 0 0.0 0.0 -0.9975052251337028 0.07059267547671683 320 | 319 325.572 468.56699999999995 0 0.0 0.0 0.9985403206176368 -0.05401137010692377 321 | 320 325.981 458.98900000000003 0 0.0 0.0 0.9996572291388439 0.02618060790831606 322 | 321 325.633 454.89 0 0.0 0.0 0.9987371571366506 0.05024033195154532 323 | 322 325.387 451.52 0 0.0 0.0 0.9990883761631206 0.04268977179300332 324 | 323 325.447 449.765 0 0.0 0.0 0.9997467592878061 0.022503717326911938 325 | 324 324.046 444.645 0 0.0 0.0 0.9999812835344968 0.006118217117789235 326 | 325 322.909 436.50699999999995 0 0.0 0.0 0.9998402100127474 -0.017876085747869534 327 | 326 321.52 428.98400000000004 0 0.0 0.0 0.9982888458448603 -0.05847546717843916 328 | 327 322.203 425.661 0 0.0 0.0 -0.9971609518527513 0.07529964209818613 329 | 328 322.439 418.37699999999995 0 0.0 0.0 -0.9946820126618834 0.10299365847907771 330 | 329 322.95799999999997 414.587 0 0.0 0.0 -0.9952160154719093 0.09769893831673088 331 | 330 323.724 410.517 0 0.0 0.0 -0.9967434091700292 0.08063855328630089 332 | 331 323.97900000000004 406.184 0 0.0 0.0 0.9983574202580755 -0.05729276931376582 333 | 332 323.861 401.717 0 0.0 0.0 0.9997128641536108 -0.02396224625914882 334 | 333 324.422 397.846 0 0.0 0.0 0.9999920367516358 0.003990793569586226 335 | 334 323.64799999999997 390.11400000000003 0 0.0 0.0 0.9997029962375995 0.024370459855452282 336 | 335 322.469 381.84 0 0.0 0.0 0.9994203427771863 -0.034043772457403206 337 | 336 322.835 369.699 0 0.0 0.0 -0.9949253928572821 0.10061541953290652 338 | 337 323.772 365.741 0 0.0 0.0 -0.9948879211892313 0.10098526759765407 339 | 338 323.55400000000003 361.07599999999996 0 0.0 0.0 -0.9965776121744025 0.08266234277327258 340 | 339 324.628 353.909 0 0.0 0.0 0.9986176048755684 -0.05256309763116389 341 | 340 325.012 345.82199999999995 0 0.0 0.0 0.9995054662848147 -0.03144555400649115 342 | 341 324.395 337.381 0 0.0 0.0 0.9999642496475507 -0.008455733369198115 343 | 342 326.168 327.865 0 0.0 0.0 0.9998956834611118 0.014443759754165415 344 | 343 324.98 323.736 0 0.0 0.0 0.9997708949397648 0.021404617056650574 345 | 344 324.863 320.26 0 0.0 0.0 0.9999892045383467 0.004646590875526523 346 | 345 324.432 316.262 0 0.0 0.0 0.9997754157664539 -0.02119240498419561 347 | 346 324.08299999999997 313.04 0 0.0 0.0 0.9991912695181193 -0.040209537659230785 348 | 347 323.42 305.27 0 0.0 0.0 0.9985257137229497 -0.05428074275536264 349 | 348 323.472 301.29900000000004 0 0.0 0.0 0.9996146285360523 -0.027759582430758194 350 | 349 323.813 297.122 0 0.0 0.0 0.9999920367516358 0.003990793569586226 351 | 350 323.089 289.439 0 0.0 0.0 0.9994426755727784 0.03338170522795135 352 | 351 322.574 285.483 0 0.0 0.0 0.9993802620221399 0.03520073693204344 353 | 352 322.264 281.71299999999997 0 0.0 0.0 -0.9963893713241221 0.08490124093510563 354 | 353 321.857 274.219 0 0.0 0.0 -0.917178264388751 0.39847714028892334 355 | 354 325.431 271.8 0 0.0 0.0 -0.8690483170844837 0.4947272203675746 356 | 355 329.032 268.157 0 0.0 0.0 -0.8062915596690642 0.5915183182340408 357 | 356 336.829 265.014 0 0.0 0.0 -0.6291803745270778 0.7772593237201895 358 | 357 341.725 267.442 0 0.0 0.0 -0.5648276108314546 0.8252089250865086 359 | 358 345.824 268.38599999999997 0 0.0 0.0 -0.5977093864800263 0.8017128471676568 360 | 359 349.555 272.301 0 0.0 0.0 -0.7112822061567765 0.7029065536789003 361 | 360 353.44 274.21 0 0.0 0.0 -0.7947286400290452 0.6069648990819686 362 | 361 356.819 274.918 0 0.0 0.0 -0.8210974927704497 0.5707879705863477 363 | 362 360.11 273.33099999999996 0 0.0 0.0 -0.8184273429006235 0.5746100280996019 364 | 363 363.836 269.871 0 0.0 0.0 -0.7972945405149939 0.6035904370224769 365 | 364 367.746 271.207 0 0.0 0.0 -0.7358572345831353 0.6771367146386765 366 | 365 371.668 269.408 0 0.0 0.0 -0.6933879898934904 0.7205644283972564 367 | 366 375.68300000000005 269.832 0 0.0 0.0 -0.7183291809314378 0.6957033763195128 368 | 367 379.486 270.936 0 0.0 0.0 -0.7710590084998171 0.6367636966813348 369 | 368 381.869 271.832 0 0.0 0.0 -0.749573409714262 0.66192122148284 370 | 369 388.68800000000005 270.39099999999996 0 0.0 0.0 -0.41198625503930003 0.9111900601184655 371 | 370 394.276 275.981 0 0.0 0.0 -0.21462154987302237 0.9766972869472412 372 | 371 394.69 279.66700000000003 0 0.0 0.0 -0.13719325358103077 0.9905443004590209 373 | 372 393.62300000000005 294.472 0 0.0 0.0 -0.1249315372035654 0.9921653647512365 374 | 373 393.16900000000004 299.404 0 0.0 0.0 -0.174558130752132 0.9846468701967835 375 | 374 394.30699999999996 302.733 0 0.0 0.0 -0.22226928158289067 0.9749853160251316 376 | 375 396.346 306.835 0 0.0 0.0 -0.22035087958268212 0.9754206732826295 377 | 376 399.755 315.279 0 0.0 0.0 -0.04942193649835485 0.9987779894414737 378 | 377 400.906 317.66900000000004 0 0.0 0.0 0.008335613936964232 0.9999652581666495 379 | 378 401.853 321.369 0 0.0 0.0 0.05953956195119121 0.9982259466486835 380 | 379 400.85400000000004 325.616 0 0.0 0.0 0.03579522844395316 0.9993591454630538 381 | 380 399.715 329.586 0 0.0 0.0 -0.018443275577685565 0.9998299083273943 382 | 381 398.621 331.836 0 0.0 0.0 -0.052093532493127426 0.9986422101395411 383 | 382 397.32099999999997 338.67900000000003 0 0.0 0.0 -0.14802150978719106 0.9889841417537091 384 | 383 399.673 343.705 0 0.0 0.0 -0.1637739005908756 0.9864979014094506 385 | 384 401.88699999999994 348.62800000000004 0 0.0 0.0 -0.1426157631503266 0.9897781287243369 386 | 385 404.106 353.81199999999995 0 0.0 0.0 -0.04782599811374682 0.9988556822206218 387 | 386 404.30199999999996 362.038 0 0.0 0.0 0.07324670036435989 0.9973138527493407 388 | 387 401.89099999999996 368.51099999999997 0 0.0 0.0 0.03552632588595501 0.9993687408404592 389 | 388 401.21 372.57300000000004 0 0.0 0.0 -0.020237297832838293 0.9997952049176997 390 | 389 399.86300000000006 377.21 0 0.0 0.0 -0.09430779919494708 0.9955430874708565 391 | 390 401.035 379.465 0 0.0 0.0 -0.13443455321546757 0.9909224747182585 392 | 391 401.86199999999997 383.455 0 0.0 0.0 -0.1584172566907902 0.9873722564375426 393 | 392 404.786 391.11400000000003 0 0.0 0.0 -0.14085117270176767 0.9900307809096325 394 | 393 406.695 393.952 0 0.0 0.0 -0.07801783625910157 0.9969519633490111 395 | 394 405.11699999999996 400.782 0 0.0 0.0 0.049780139315108274 0.9987602003132525 396 | 395 404.39099999999996 407.05199999999996 0 0.0 0.0 0.08044614550736981 0.9967589566555232 397 | 396 404.80400000000003 409.89599999999996 0 0.0 0.0 0.03960985085662838 0.9992152219192398 398 | 397 403.63800000000003 415.256 0 0.0 0.0 -0.021461344318129308 0.9997696788261078 399 | 398 404.11400000000003 417.786 0 0.0 0.0 -0.02709263436204069 0.9996329272104459 400 | 399 404.44800000000004 420.68300000000005 0 0.0 0.0 -0.02246641096700399 0.9997475983358308 401 | 400 403.76199999999994 423.55300000000005 0 0.0 0.0 -0.012964699448681224 0.9999159547523009 402 | 401 404.465 426.366 0 0.0 0.0 -0.003817077766766531 0.9999927149321252 403 | 402 403.744 430.624 0 0.0 0.0 -0.0006858313686677032 0.9999997648176392 404 | 403 403.449 437.25199999999995 0 0.0 0.0 0.016059141698596614 0.9998710436690845 405 | 404 402.68699999999995 445.07300000000004 0 0.0 0.0 0.06388906037033187 0.997957007072447 406 | 405 401.62300000000005 452.07 0 0.0 0.0 0.03866663576139123 0.9992521660116108 407 | 406 402.31699999999995 454.82800000000003 0 0.0 0.0 0.021668316589583084 0.9997652144659633 408 | 407 402.09 459.61400000000003 0 0.0 0.0 -0.045753481686674626 0.998952761102119 409 | 408 401.926 464.309 0 0.0 0.0 -0.0745727055848756 0.9972155792915348 410 | 409 401.403 466.793 0 0.0 0.0 -0.08026868928242124 0.9967732628441044 411 | 410 402.87 473.463 0 0.0 0.0 -0.0825592882798475 0.996586154789803 412 | 411 403.36800000000005 474.29400000000004 0 0.0 0.0 -0.0746152873493442 0.9972123940734866 413 | 412 405.50699999999995 482.526 0 0.0 0.0 -0.014412509737187515 0.999896134387505 414 | 413 406.163 488.19800000000004 0 0.0 0.0 0.0379484644489671 0.9992796976052127 415 | 414 405.36400000000003 492.539 0 0.0 0.0 0.04185602254542704 0.9991236526960398 416 | 415 403.839 496.07599999999996 0 0.0 0.0 0.031270923871945466 0.9995109450727365 417 | 416 402.912 498.663 0 0.0 0.0 -0.00645999798160271 0.9999791339953438 418 | 417 404.514 502.08099999999996 0 0.0 0.0 -0.08090523886781585 0.9967217978572264 419 | 418 405.92199999999997 507.42400000000004 0 0.0 0.0 -0.11729259511115547 0.9930974006269931 420 | 419 405.603 511.07599999999996 0 0.0 0.0 -0.07958945650340783 0.9968277275504992 421 | 420 407.718 513.744 0 0.0 0.0 -0.03405606326836294 0.9994199240332671 422 | 421 408.76099999999997 519.195 0 0.0 0.0 0.02389810849351785 0.9997143994213707 423 | 422 407.166 526.7909999999999 0 0.0 0.0 0.07315557091098246 0.997320541473346 424 | 423 405.75800000000004 533.935 0 0.0 0.0 0.07101776786060228 0.997475050639412 425 | 424 406.036 537.5840000000001 0 0.0 0.0 0.07547748805940901 0.997147506037217 426 | 425 405.65 540.486 0 0.0 0.0 0.07006328456994414 0.9975425485438058 427 | 426 405.845 545.094 0 0.0 0.0 0.06574893853693485 0.997836197520047 428 | 427 403.26 553.1030000000001 0 0.0 0.0 0.02189147872815097 0.9997603528641726 429 | 428 402.35900000000004 556.681 0 0.0 0.0 -0.014236847805845818 0.9998986509464612 430 | 429 401.68699999999995 560.8380000000001 0 0.0 0.0 -0.12202923165843367 0.9925265067598207 431 | 430 402.92 564.205 0 0.0 0.0 -0.1631660600880488 0.9865986199236969 432 | 431 404.836 567.94 0 0.0 0.0 -0.16830603178507247 0.9857347917491612 433 | 432 405.73699999999997 571.019 0 0.0 0.0 -0.09220017000561642 0.9957404926239243 434 | 433 407.62800000000004 573.244 0 0.0 0.0 -0.019274985089167612 0.9998142202178425 435 | 434 407.07199999999995 583.7869999999999 0 0.0 0.0 0.14632988439684863 0.989235848992749 436 | 435 405.852 587.387 0 0.0 0.0 0.15730053566415325 0.9875507791905034 437 | 436 402.949 593.74 0 0.0 0.0 0.11533503506210599 0.993326648030356 438 | 437 399.499 600.778 0 0.0 0.0 0.0061652573939473656 0.999980994620031 439 | 438 397.885 607.535 0 0.0 0.0 -0.13080461322810077 0.991408166780084 440 | 439 399.869 615.359 0 0.0 0.0 -0.2518144228143898 0.9677755403308433 441 | 440 402.788 618.732 0 0.0 0.0 -0.20140047204151307 0.9795089840636765 442 | 441 404.164 619.731 0 0.0 0.0 -0.18891137390033705 0.9819941409250297 443 | 442 411.81 633.177 0 0.0 0.0 0.26139504181500306 0.965231905872642 444 | 443 411.70099999999996 636.38 0 0.0 0.0 0.5002589091929834 0.8658758708804896 445 | 444 409.039 638.47 0 0.0 0.0 0.6618891514955336 0.7496017283414724 446 | 445 407.506 638.876 0 0.0 0.0 0.7275383552352765 0.6860670096000453 447 | 446 402.82300000000004 638.5219999999999 0 0.0 0.0 0.844371803551757 0.5357576479778456 448 | 447 399.535 637.7959999999999 0 0.0 0.0 0.917350390918432 0.3980807208114958 449 | 448 397.333 636.0369999999999 0 0.0 0.0 0.9884583519148751 0.15149285966582382 450 | 449 395.23199999999997 632.813 0 0.0 0.0 0.9993802620221399 0.03520073693204344 451 | 450 392.708 623.064 0 0.0 0.0 -0.993682087967819 0.11223149313769168 452 | 451 391.94300000000004 616.273 0 0.0 0.0 -0.9927526835637165 0.12017532723916127 453 | 452 393.791 608.155 0 0.0 0.0 -0.9916405008818544 0.12903145744656602 454 | 453 394.775 599.485 0 0.0 0.0 -0.9961265088313199 0.08793166894541661 455 | 454 395.342 593.8919999999999 0 0.0 0.0 0.9986007086986258 -0.05288312194455062 456 | 455 396.81300000000005 590.567 0 0.0 0.0 0.9997613916799011 -0.021843985585681378 457 | 456 397.333 585.935 0 0.0 0.0 0.9999627193222541 0.008634811268514475 458 | 457 397.88199999999995 581.489 0 0.0 0.0 0.9996499887236292 0.02645562406838955 459 | 458 397.06800000000004 574.201 0 0.0 0.0 0.9995588683432015 0.029699641689055555 460 | 459 396.611 566.068 0 0.0 0.0 0.9998174984999393 0.019104179996112854 461 | 460 395.815 557.632 0 0.0 0.0 0.9998644019450387 0.016467474702887924 462 | 461 395.733 553.846 0 0.0 0.0 0.9999998794123576 0.000491095988881541 463 | 462 394.11400000000003 545.8240000000001 0 0.0 0.0 -0.9956379031875454 0.09330147767483615 464 | 463 394.709 539.9830000000001 0 0.0 0.0 -0.9944103273663928 0.1055845671822525 465 | 464 395.069 536.246 0 0.0 0.0 -0.9963623502956417 0.08521776172456721 466 | 465 395.38 534.1419999999999 0 0.0 0.0 -0.9973120997207938 0.0732705653758813 467 | 466 396.36300000000006 528.763 0 0.0 0.0 0.9997542840716946 -0.022166900556757786 468 | 467 396.92800000000005 525.525 0 0.0 0.0 0.9992815806897617 0.037898845551959416 469 | 468 396.661 521.756 0 0.0 0.0 0.9895397480631354 0.1442604831655103 470 | 469 393.63 514.759 0 0.0 0.0 0.9934231030383469 0.1145012591619058 471 | 470 391.89 509.343 0 0.0 0.0 0.9999812835344968 0.006118217117789235 472 | 471 391.236 505.55400000000003 0 0.0 0.0 -0.9979906517659768 0.06336133669455613 473 | 472 390.925 498.791 0 0.0 0.0 -0.9895029448891383 0.14451270551658402 474 | 473 391.00300000000004 493.43800000000005 0 0.0 0.0 -0.977572442786769 0.21059942807118243 475 | 474 393.601 486.13 0 0.0 0.0 -0.9848773794738649 0.17325284240292507 476 | 475 395.556 484.625 0 0.0 0.0 -0.9925094195250156 0.1221681306811096 477 | 476 396.603 480.709 0 0.0 0.0 0.9987449071346941 -0.05008603071227737 478 | 477 398.514 475.139 0 0.0 0.0 0.9998513894635521 0.01723946019468775 479 | 478 398.63699999999994 469.605 0 0.0 0.0 0.9971708784357488 0.07516807300760828 480 | 479 398.398 464.845 0 0.0 0.0 0.990842680636727 0.13502141396321232 481 | 480 399.37699999999995 460.167 0 0.0 0.0 0.9828650863904972 0.18432640059036715 482 | 481 396.86400000000003 456.63300000000004 0 0.0 0.0 0.9839429229437302 0.17848340087792072 483 | 482 392.48400000000004 449.45099999999996 0 0.0 0.0 0.9995799037328816 0.028983030437881464 484 | 483 390.12199999999996 441.621 0 0.0 0.0 -0.9948232930058756 0.10161995715874844 485 | 484 390.241 433.63800000000003 0 0.0 0.0 -0.9829674601087646 0.18377968431609706 486 | 485 391.316 429.838 0 0.0 0.0 -0.9851460745993954 0.17171840816115894 487 | 486 392.101 425.476 0 0.0 0.0 -0.9908687868527624 0.13482969717734639 488 | 487 392.837 423.555 0 0.0 0.0 -0.9933324757734839 0.11528483236627877 489 | 488 393.99300000000005 419.68199999999996 0 0.0 0.0 -0.9950054770189051 0.0998203421271493 490 | 489 394.73800000000006 416.592 0 0.0 0.0 -0.9973394799551788 0.07289692533113858 491 | 490 395.289 412.13800000000003 0 0.0 0.0 0.999451272277967 -0.033123320213301374 492 | 491 395.506 407.25300000000004 0 0.0 0.0 0.9996881544648096 0.024971860618363022 493 | 492 394.63199999999995 398.968 0 0.0 0.0 0.9948006427422236 0.10184145128413512 494 | 493 392.467 392.42400000000004 0 0.0 0.0 0.9999636499561344 0.008526357159157467 495 | 494 391.149 384.95 0 0.0 0.0 -0.9972012076521605 0.074764640419604 496 | 495 387.256 377.7 0 0.0 0.0 -0.9920175952956254 0.12609952666003446 497 | 496 389.94599999999997 371.225 0 0.0 0.0 -0.9870177742848498 0.16061106202806 498 | 497 390.24699999999996 366.13699999999994 0 0.0 0.0 -0.9877217409489591 0.1562234376038285 499 | 498 391.626 358.97 0 0.0 0.0 -0.9929927395956237 0.11817537438222007 500 | 499 393.599 356.126 0 0.0 0.0 -0.9937180355575972 0.11191275980669003 501 | 500 395.42400000000004 347.911 0 0.0 0.0 0.9997719190873996 -0.021356727377996106 502 | 501 396.62 345.168 0 0.0 0.0 0.9969322161401489 0.07826976697225714 503 | 502 396.58 340.4 0 0.0 0.0 0.9858588189929185 0.16757800874186332 504 | 503 394.379 334.452 0 0.0 0.0 0.9929400346201877 0.11861740027694329 505 | 504 392.92400000000004 331.711 0 0.0 0.0 0.9998319567558362 0.01833189160440567 506 | 505 395.11400000000003 324.313 0 0.0 0.0 -0.9967864662199037 0.0801045614234084 507 | 506 393.962 321.155 0 0.0 0.0 -0.9941494607852044 0.10801319188176656 508 | 507 393.60699999999997 316.638 0 0.0 0.0 -0.9975202129851495 0.07038057037323625 509 | 508 393.499 306.107 0 0.0 0.0 0.9998826814500137 -0.015317419375681839 510 | 509 394.106 301.658 0 0.0 0.0 0.9999077819258626 -0.013580413988590983 511 | 510 393.82599999999996 297.89 0 0.0 0.0 0.9999434124503003 -0.010638228106632277 512 | 511 393.30400000000003 294.124 0 0.0 0.0 0.9999744531521715 -0.007147939774206417 513 | 512 392.931 289.952 0 0.0 0.0 0.9998956834611118 0.014443759754165415 514 | 513 392.478 286.349 0 0.0 0.0 0.998956960772792 0.04566169645979761 515 | 514 389.80800000000005 277.586 0 0.0 0.0 -0.9657690990310656 0.2594032524019772 516 | 515 391.7 272.98900000000003 0 0.0 0.0 -0.9294358050014236 0.36898385382202775 517 | 516 392.726 270.473 0 0.0 0.0 -0.8834665772423261 0.4684941908879223 518 | 517 395.535 267.522 0 0.0 0.0 -0.8146609074704467 0.579937587882893 519 | 518 398.63199999999995 266.526 0 0.0 0.0 -0.7736258816830893 0.6336426399715084 520 | 519 404.34 266.44 0 0.0 0.0 -0.5988564696248124 0.8008563721345457 521 | 520 408.36800000000005 267.41700000000003 0 0.0 0.0 -0.5241709458723484 0.851613069124287 522 | 521 411.676 269.941 0 0.0 0.0 -0.6022281737706883 0.7983240111112916 523 | 522 413.69300000000004 271.654 0 0.0 0.0 -0.6721688995525746 0.740397846076203 524 | 523 418.47 274.823 0 0.0 0.0 -0.7814401485726107 0.6239802033709212 525 | 524 430.463 271.654 0 0.0 0.0 -0.7825233500427718 0.622621238505271 526 | 525 437.469 268.366 0 0.0 0.0 -0.7181873815355515 0.6958497574930296 527 | 526 444.25199999999995 268.453 0 0.0 0.0 -0.757120333976906 0.653275439518966 528 | 527 448.01 269.325 0 0.0 0.0 -0.7589164567208493 0.6511880002873758 529 | 528 451.85 269.606 0 0.0 0.0 -0.6617987199354142 0.7496815685955253 530 | 529 459.395 269.504 0 0.0 0.0 -0.32888658544330523 0.9443694266098641 531 | 530 463.181 273.655 0 0.0 0.0 -0.2640243747140685 0.9645160079318773 532 | 531 463.478 278.92900000000003 0 0.0 0.0 -0.20674122129480124 0.9783956599543633 533 | 532 463.664 284.248 0 0.0 0.0 -0.10162379062334913 0.9948229014147904 534 | 533 464.17 289.084 0 0.0 0.0 -0.02643867298815626 0.999650437188233 535 | 534 464.037 291.103 0 0.0 0.0 -0.015815458072380516 0.9998749278214555 536 | 535 465.651 295.02099999999996 0 0.0 0.0 -0.041169012474851885 0.9991521968208075 537 | 536 464.75300000000004 299.691 0 0.0 0.0 -0.10359497064649359 0.9946195664960308 538 | 537 463.824 305.868 0 0.0 0.0 -0.16276057248488887 0.9866655948417332 539 | 538 463.74800000000005 308.142 0 0.0 0.0 -0.19242090168525497 0.9813124867210411 540 | 539 465.034 312.623 0 0.0 0.0 -0.21068608773715392 0.9775537695871324 541 | 540 465.845 317.293 0 0.0 0.0 -0.20610913564793307 0.9785290104041178 542 | 541 468.22 321.555 0 0.0 0.0 -0.1667292935642623 0.9860027092597474 543 | 542 470.82199999999995 325.339 0 0.0 0.0 -0.11978714856994828 0.9927995966142821 544 | 543 472.074 334.288 0 0.0 0.0 -0.01208656934982054 0.999926954752872 545 | 544 471.56800000000004 341.04 0 0.0 0.0 0.018016624854272693 0.999837687441747 546 | 545 471.13800000000003 345.841 0 0.0 0.0 0.0009475665819504341 0.9999995510586857 547 | 546 469.75699999999995 349.96 0 0.0 0.0 -0.043548927332890466 0.9990512954439099 548 | 547 469.346 353.217 0 0.0 0.0 -0.08886528856168427 0.9960436539072214 549 | 548 469.97900000000004 356.824 0 0.0 0.0 -0.1277877709615806 0.9918015353853162 550 | 549 470.43 360.914 0 0.0 0.0 -0.14785794231308483 0.9890086091106287 551 | 550 471.855 364.798 0 0.0 0.0 -0.127456843723924 0.991844117282517 552 | 551 472.202 369.288 0 0.0 0.0 -0.07929222705597842 0.9968514145691438 553 | 552 474.379 373.56 0 0.0 0.0 -0.014061628988337491 0.9999011304074991 554 | 553 473.58 381.13300000000004 0 0.0 0.0 0.10261673038866995 0.9947209692392832 555 | 554 471.226 387.958 0 0.0 0.0 0.11916985860197918 0.9928738816188007 556 | 555 467.291 394.64300000000003 0 0.0 0.0 0.05782002816784291 0.9983270227448868 557 | 556 466.82099999999997 398.68800000000005 0 0.0 0.0 -0.04700571338703132 0.9988946205225936 558 | 557 469.792 404.101 0 0.0 0.0 -0.14519785141664418 0.9894026399520016 559 | 558 470.708 407.671 0 0.0 0.0 -0.1615439580771061 0.9868655174889749 560 | 559 472.457 414.892 0 0.0 0.0 -0.026526000946334526 0.9996481237284423 561 | 560 473.726 419.269 0 0.0 0.0 0.04576982733251838 0.9989520123138806 562 | 561 473.01599999999996 422.63300000000004 0 0.0 0.0 0.08186089354479531 0.9966437648969905 563 | 562 472.60699999999997 426.022 0 0.0 0.0 0.07183644001529682 0.997416425514403 564 | 563 471.755 429.959 0 0.0 0.0 0.024389097618672955 0.9997025417179587 565 | 564 470.914 432.05300000000005 0 0.0 0.0 -0.00584331167853911 0.9999829277085821 566 | 565 471.78 437.23400000000004 0 0.0 0.0 -0.05695379036519183 0.998376815517587 567 | 566 470.63 442.495 0 0.0 0.0 -0.11712616889656012 0.9931170427294129 568 | 567 472.102 444.925 0 0.0 0.0 -0.1265050967200563 0.9919659573311219 569 | 568 473.583 449.13699999999994 0 0.0 0.0 -0.11045665572016936 0.9938809421691895 570 | 569 473.675 453.651 0 0.0 0.0 -0.07448758362774639 0.9972219411371269 571 | 570 476.48199999999997 457.194 0 0.0 0.0 -0.027005330077651254 0.9996352895668486 572 | 571 477.457 463.207 0 0.0 0.0 0.0371860500655457 0.9993083596570794 573 | 572 477.59 467.765 0 0.0 0.0 0.08926629151802429 0.9960077957519304 574 | 573 477.44300000000004 472.241 0 0.0 0.0 0.12009492823098115 0.9927624127721574 575 | 574 473.405 481.20599999999996 0 0.0 0.0 0.10353631785837707 0.9946256737508484 576 | 575 472.216 484.74199999999996 0 0.0 0.0 0.06429725898483937 0.9979307904293947 577 | 576 470.30699999999996 488.10400000000004 0 0.0 0.0 0.012194097020933287 0.9999256492349039 578 | 577 470.27099999999996 490.56199999999995 0 0.0 0.0 -0.04060623547902903 0.9991752266946082 579 | 578 470.316 498.615 0 0.0 0.0 -0.09775917070726337 0.9952101007036797 580 | 579 470.681 527.2919999999999 0 0.0 0.0 -0.061630488133397075 0.9980990346315536 581 | 580 472.38699999999994 530.5269999999999 0 0.0 0.0 -0.06368658802347932 0.997969948698821 582 | 581 473.596 534.172 0 0.0 0.0 -0.036921767384247156 0.9993181590931006 583 | 582 474.19 538.717 0 0.0 0.0 0.021445640723841915 0.999770015800606 584 | 583 473.535 542.4169999999999 0 0.0 0.0 0.08995289700156224 0.9959460207867825 585 | 584 471.06199999999995 548.94 0 0.0 0.0 0.10441059803768764 0.9945342764416983 586 | 585 467.689 554.306 0 0.0 0.0 -0.021330490916289205 0.9997724791957768 587 | 586 466.029 561.73 0 0.0 0.0 -0.17363186356659463 0.9848106294889345 588 | 587 467.501 565.2330000000001 0 0.0 0.0 -0.2074517203996383 0.9782452574396823 589 | 588 469.004 568.174 0 0.0 0.0 -0.1963604939942174 0.9805317722533763 590 | 589 476.082 578.438 0 0.0 0.0 -0.05493379206988763 0.9984899991931929 591 | 590 476.695 584.5269999999999 0 0.0 0.0 0.053480797358981105 0.9985688781019803 592 | 591 478.148 589.3480000000001 0 0.0 0.0 0.08377900806101418 0.9964843590384711 593 | 592 478.006 593.101 0 0.0 0.0 0.08172376632198797 0.9966550185586531 594 | 593 477.714 598.547 0 0.0 0.0 0.0857895227143494 0.9963132829549369 595 | 594 478.371 600.2769999999999 0 0.0 0.0 0.10036430411459107 0.9949507557962826 596 | 595 475.767 608.438 0 0.0 0.0 0.16614741361662624 0.9861009263500901 597 | 596 474.19599999999997 611.599 0 0.0 0.0 0.174263023708759 0.9846991411430602 598 | 597 473.635 614.3919999999999 0 0.0 0.0 0.16094908525430685 0.9869627105194004 599 | 598 472.57099999999997 617.85 0 0.0 0.0 0.2177251684685885 0.976010118295566 600 | 599 471.411 622.012 0 0.0 0.0 0.4355481796827061 0.9001654198951886 601 | 600 464.875 625.408 0 0.0 0.0 0.8084311649462586 0.5885907334843415 602 | 601 458.24199999999996 624.683 0 0.0 0.0 0.9736399274110702 0.22809053411083507 603 | 602 456.791 622.216 0 0.0 0.0 0.9970142011541261 0.07721840905509374 604 | 603 454.50199999999995 618.448 0 0.0 0.0 -0.9973317293469511 0.0730028878731514 605 | 604 454.145 615.128 0 0.0 0.0 -0.9837023030871171 0.17980483558876115 606 | 605 454.463 611.906 0 0.0 0.0 -0.9695471836656653 0.24490459090424743 607 | 606 455.583 608.578 0 0.0 0.0 -0.954567851833593 0.29799365135150024 608 | 607 457.461 606.129 0 0.0 0.0 -0.9576768950773112 0.28784538320959835 609 | 608 460.19 600.428 0 0.0 0.0 -0.9833647945245951 0.1816416276341987 610 | 609 462.36800000000005 595.997 0 0.0 0.0 -0.986722168733757 0.16241724578797403 611 | 610 462.926 591.729 0 0.0 0.0 -0.9903144470790491 0.13884270202829274 612 | 611 464.99199999999996 589.0459999999999 0 0.0 0.0 -0.9934599109253041 0.11418145814573775 613 | 612 466.07800000000003 584.609 0 0.0 0.0 -0.997893979807207 0.06486605479396457 614 | 613 467.24 581.0459999999999 0 0.0 0.0 0.9998460372721389 -0.017547129429067478 615 | 614 468.111 577.391 0 0.0 0.0 0.9999315392033523 0.011701149790288535 616 | 615 467.884 573.289 0 0.0 0.0 0.9995239588078354 0.03085215987759252 617 | 616 467.695 569.698 0 0.0 0.0 0.9991823056694511 0.04043167121304303 618 | 617 467.34 563.327 0 0.0 0.0 0.9983438133966083 0.05752938599288395 619 | 618 467.147 562.308 0 0.0 0.0 0.9990116725471165 0.044448600817268674 620 | 619 466.037 560.856 0 0.0 0.0 0.9992509230755359 -0.03869874329599648 621 | 620 466.165 560.134 0 0.0 0.0 -0.9925161540996165 0.12211340569858159 622 | 621 466.04400000000004 559.175 0 0.0 0.0 -0.9899455561325415 0.14144891619744948 623 | 622 468.244 560.726 0 0.0 0.0 -0.9866628488814797 0.16277721780729137 624 | 623 468.289 560.064 0 0.0 0.0 -0.9834217605879938 0.18133295564240534 625 | 624 468.129 559.857 0 0.0 0.0 -0.9798643020558738 0.19966459264615566 626 | 625 467.6 560.436 0 0.0 0.0 -0.9787276104087927 0.2051639944617338 627 | 626 468.059 560.133 0 0.0 0.0 -0.9800486344065007 0.19875782801679331 628 | 627 468.504 560.3919999999999 0 0.0 0.0 -0.981737017337372 0.19024307816454342 629 | 628 470.00300000000004 561.441 0 0.0 0.0 -0.9851637831284411 0.17161678359664548 630 | 629 470.13300000000004 561.339 0 0.0 0.0 -0.9882156026593942 0.1530683594363322 631 | 630 471.81300000000005 560.7909999999999 0 0.0 0.0 -0.9891359053878944 0.1470039478125346 632 | 631 471.278 561.16 0 0.0 0.0 -0.9906040770658877 0.1367609684831197 633 | 632 471.616 561.59 0 0.0 0.0 -0.9920640591768064 0.1257334581153239 634 | 633 471.551 561.438 0 0.0 0.0 -0.9918907242340588 0.1270936315416094 635 | 634 471.49699999999996 560.842 0 0.0 0.0 -0.9949471432428686 0.10040011032292115 636 | 635 471.709 560.513 0 0.0 0.0 -0.9958496024766065 0.09101411564798477 637 | 636 471.7 560.278 0 0.0 0.0 -0.997176920055256 0.07508788257178003 638 | 637 473.069 559.683 0 0.0 0.0 -0.9977954514035394 0.06636442690483418 639 | 638 472.98199999999997 559.732 0 0.0 0.0 0.998961884564731 -0.045553849309153446 640 | 639 472.842 559.982 0 0.0 0.0 0.999451272277967 -0.033123320213301374 641 | 640 472.935 560.002 0 0.0 0.0 0.9999164374556456 -0.012927416834382777 642 | 641 472.917 560.031 0 0.0 0.0 0.9998891961858087 -0.014886079097506446 643 | 642 473.087 559.684 0 0.0 0.0 0.9997755278620684 0.02118711608791189 644 | 643 473.13199999999995 559.384 0 0.0 0.0 0.9997926528194657 0.020362990158992258 645 | 644 473.406 559.385 0 0.0 0.0 0.9989993569514763 0.0447245437152431 646 | 645 473.832 559.422 0 0.0 0.0 0.9965604029563665 0.08286955568509108 647 | 646 473.89099999999996 559.513 0 0.0 0.0 0.9951255350564631 0.09861627390338779 648 | 647 474.832 558.906 0 0.0 0.0 0.9935249204288547 0.11361440263821282 649 | 648 475.18300000000005 558.9780000000001 0 0.0 0.0 0.9920427690099666 0.12590132824969788 650 | 649 475.213 558.955 0 0.0 0.0 0.9897577330677738 0.14275724091107053 651 | 650 475.348 558.739 0 0.0 0.0 0.9873851603812805 0.15833680891957133 652 | 651 475.342 558.592 0 0.0 0.0 0.9823398701257535 0.18710526331805216 653 | 652 475.58 558.617 0 0.0 0.0 0.9783990702813997 0.2067250813822374 654 | 653 475.184 558.497 0 0.0 0.0 0.9759744557848229 0.21788497345048555 655 | 654 475.684 558.322 0 0.0 0.0 0.9734186579310323 0.2290330028440178 656 | 655 475.89300000000003 558.297 0 0.0 0.0 0.9680552862491957 0.25073683966459304 657 | 656 476.241 558.216 0 0.0 0.0 0.9627384736475679 0.27043415346207883 658 | 657 477.111 558.295 0 0.0 0.0 0.9589952979116024 0.28342197971123706 659 | 658 477.397 558.321 0 0.0 0.0 0.9595136894510131 0.28166199558354466 660 | 659 477.51199999999994 558.169 0 0.0 0.0 0.9537710639249486 0.3005341205578354 661 | 660 477.477 557.932 0 0.0 0.0 0.9519413856783241 0.3062802609258916 662 | 661 477.87199999999996 557.455 0 0.0 0.0 0.9465373632067127 0.32259420337892586 663 | 662 478.298 557.754 0 0.0 0.0 0.9447993149470452 0.3276495909895116 664 | 663 478.324 557.866 0 0.0 0.0 0.9415480080318348 0.33687883366469323 665 | 664 478.096 557.799 0 0.0 0.0 0.9369416985212774 0.34948569866600215 666 | 665 479.13699999999994 557.216 0 0.0 0.0 0.9334305733386771 0.3587580866776784 667 | 666 479.38800000000003 556.775 0 0.0 0.0 0.9336375593256919 0.3582190779625857 668 | 667 479.014 556.906 0 0.0 0.0 0.9286112058190127 0.371054212248506 669 | 668 479.221 556.496 0 0.0 0.0 0.9245722254043894 0.38100682410000847 670 | 669 479.37300000000005 556.369 0 0.0 0.0 0.9246158159165793 0.3809010277708874 671 | 670 479.55800000000005 556.406 0 0.0 0.0 0.922379296025153 0.3862854310793293 672 | 671 479.827 556.352 0 0.0 0.0 0.9200187442697815 0.39187435510920393 673 | 672 480.32199999999995 556.153 0 0.0 0.0 0.9164506610255082 0.40014770511138653 674 | 673 480.216 556.564 0 0.0 0.0 0.9171415185275443 0.398561707885981 675 | 674 480.43800000000005 556.226 0 0.0 0.0 0.9112388748716946 0.4118782743998134 676 | 675 480.74699999999996 555.976 0 0.0 0.0 0.9087709201755805 0.4172953566039631 677 | 676 481.394 555.818 0 0.0 0.0 0.9119775779109138 0.4102400484932487 678 | 677 482.035 555.8380000000001 0 0.0 0.0 0.9058516568599451 0.42359506107141087 679 | 678 482.059 555.479 0 0.0 0.0 0.9086248766021987 0.4176132584337322 680 | -------------------------------------------------------------------------------- /examples/manhattan/factor_graph.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/examples/manhattan/factor_graph.pickle -------------------------------------------------------------------------------- /examples/solve_goats_example_score.py: -------------------------------------------------------------------------------- 1 | import os 2 | from os.path import join 3 | import logging, coloredlogs 4 | 5 | logger = logging.getLogger(__name__) 6 | field_styles = { 7 | "filename": {"color": "green"}, 8 | "filename": {"color": "green"}, 9 | "levelname": {"bold": True, "color": "black"}, 10 | "name": {"color": "blue"}, 11 | } 12 | coloredlogs.install( 13 | level="INFO", 14 | fmt="[%(filename)s:%(lineno)d] %(name)s %(levelname)s - %(message)s", 15 | field_styles=field_styles, 16 | ) 17 | 18 | from py_factor_graph.parsing.parse_pickle_file import parse_pickle_file 19 | from py_factor_graph.utils.plot_utils import visualize_solution 20 | from score.solve_score import solve_score 21 | from score.utils.solver_utils import ScoreSolverParams 22 | from score.utils.gurobi_utils import QCQP_RELAXATION 23 | 24 | 25 | if __name__ == "__main__": 26 | 27 | # Set up the solver 28 | solver_params = ScoreSolverParams( 29 | solver="gurobi", 30 | verbose=True, 31 | save_results=True, 32 | init_technique="none", 33 | custom_init_file=None, 34 | ) 35 | 36 | # extract the factor graph data 37 | cur_file_dir = os.path.dirname(os.path.realpath(__file__)) 38 | data_dir = join(cur_file_dir, "goats_14_data") 39 | goats_file_path = join(data_dir, "goats_14_6_2002_15_20.pkl") 40 | goats_pyfg = parse_pickle_file(goats_file_path) 41 | 42 | score_result = solve_score( 43 | goats_pyfg, solver_params, QCQP_RELAXATION 44 | ) # the solution to the convex relaxation - not the refined result! 45 | visualize_solution(score_result) 46 | -------------------------------------------------------------------------------- /media/20robot_animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/media/20robot_animation.gif -------------------------------------------------------------------------------- /media/4robot_animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/media/4robot_animation.gif -------------------------------------------------------------------------------- /media/GoatsTrajs_3col_linewidth2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/media/GoatsTrajs_3col_linewidth2.png -------------------------------------------------------------------------------- /media/MultiTrajsV4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/media/MultiTrajsV4.png -------------------------------------------------------------------------------- /media/SingleTrajsV5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/media/SingleTrajsV5.png -------------------------------------------------------------------------------- /media/title_figure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/media/title_figure.png -------------------------------------------------------------------------------- /score/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/score/__init__.py -------------------------------------------------------------------------------- /score/solve_score.py: -------------------------------------------------------------------------------- 1 | from os.path import join 2 | import logging, coloredlogs 3 | from typing import List 4 | 5 | logger = logging.getLogger(__name__) 6 | field_styles = { 7 | "filename": {"color": "green"}, 8 | "levelname": {"bold": True, "color": "black"}, 9 | "name": {"color": "blue"}, 10 | } 11 | coloredlogs.install( 12 | level="WARNING", 13 | fmt="[%(filename)s:%(lineno)d] %(name)s %(levelname)s - %(message)s", 14 | field_styles=field_styles, 15 | ) 16 | 17 | from py_factor_graph.factor_graph import FactorGraphData 18 | from py_factor_graph.utils.solver_utils import ( 19 | SolverResults, 20 | ) 21 | from py_factor_graph.utils.matrix_utils import get_matrix_determinant 22 | 23 | 24 | import score.utils.gurobi_utils as gu 25 | from gurobipy import GRB, GurobiError 26 | 27 | 28 | def _check_factor_graph(data: FactorGraphData): 29 | unconnected_variables = data.unconnected_variable_names 30 | assert ( 31 | len(unconnected_variables) == 0 32 | ), f"Found {unconnected_variables} unconnected variables. " 33 | 34 | 35 | def _check_solution_quality(result, rotations): 36 | # get list of the determinants of the rotation matrices 37 | det_list = [ 38 | get_matrix_determinant(result.GetSolution(rotations[key])) 39 | for key in rotations.keys() 40 | ] 41 | 42 | import matplotlib.pyplot as plt 43 | 44 | logger.warning( 45 | "Plotting the rotation matrix determinants - be sure to close the plot to continue" 46 | ) 47 | x_idxs = [i for i in range(len(det_list))] 48 | plt.plot(x_idxs, det_list) 49 | plt.ylim([-0.1, 1.1]) 50 | plt.title("Determinants of Unrounded Rotation Matrices") 51 | plt.show(block=True) # type: ignore 52 | 53 | 54 | def solve_score( 55 | data: FactorGraphData, 56 | relaxation_type: str = gu.QCQP_RELAXATION, 57 | ) -> SolverResults: 58 | """ 59 | Takes the data describing the problem and returns the MLE solution to the 60 | poses and landmark positions 61 | 62 | args: 63 | data (FactorGraphData): the data describing the problem 64 | results_filepath (str): where to save the results 65 | 66 | returns: 67 | SolverResults: the results of the solver 68 | """ 69 | 70 | _check_factor_graph(data) 71 | 72 | variables = gu.VariableCollection(data.dimension) 73 | model = gu.get_model() 74 | gu.initialize_model(variables, model, data, relaxation_type) 75 | try: 76 | model.optimize() 77 | except GurobiError as e: 78 | # set the nonconvex parameter to 2 and try again 79 | logger.warning( 80 | f"GurobiError: {e}. " 81 | "If the problem is just slightly non-convex, this may be because of numerical issues. " 82 | "Resolving with nonconvex parameter set to 2." 83 | ) 84 | model.setParam(GRB.Param.NonConvex, 2) 85 | model.optimize() 86 | return gu.extract_solver_results(model, variables, data) 87 | 88 | 89 | def solve_problem_with_intermediate_iterates( 90 | data: FactorGraphData, relaxation_type: str 91 | ) -> List[SolverResults]: 92 | logger.warning( 93 | """Solving the problem with intermediate iterates - this is for 94 | debugging or visualization only as it is much slower than a single 95 | solve. Use solve_score() for solving the problem""" 96 | ) 97 | iterates = [] 98 | model_vars = gu.VariableCollection(dim=data.dimension) 99 | model = gu.get_model() 100 | gu.initialize_model(model_vars, model, data, relaxation_type) 101 | curr_iter = 0 102 | finished_solving = False 103 | while not finished_solving: 104 | model.Params.BarIterLimit = curr_iter 105 | model.optimize() 106 | 107 | # check if solver done 108 | if model.status != GRB.Status.ITERATION_LIMIT: 109 | finished_solving = True 110 | 111 | curr_solver_results = gu.extract_solver_results(model, model_vars, data) 112 | iterates.append(curr_solver_results) 113 | 114 | curr_iter += 1 115 | 116 | return iterates 117 | -------------------------------------------------------------------------------- /score/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/score/41626b49702d27a8fca03982533ff52f6306278d/score/utils/__init__.py -------------------------------------------------------------------------------- /score/utils/circle_utils.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple, Optional, List, Set 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import matplotlib.patches as patches 5 | from matplotlib.colors import to_rgba 6 | import attr 7 | from scipy.spatial import ConvexHull 8 | 9 | 10 | @attr.s(frozen=True) 11 | class Point: 12 | x: float = attr.ib() 13 | y: float = attr.ib() 14 | 15 | @property 16 | def theta(self): 17 | angle = np.arctan2(self.y, self.x) + (2 * np.pi) 18 | return angle % (2 * np.pi) 19 | 20 | @property 21 | def bearing(self): 22 | angle = np.arctan2(self.y, self.x) + (2 * np.pi) 23 | return angle % (2 * np.pi) 24 | 25 | @property 26 | def distance(self): 27 | return np.sqrt(self.x**2 + self.y**2) 28 | 29 | def is_close(self, other: "Point", tol: float = 0.01) -> bool: 30 | return abs(self.x - other.x) < tol and abs(self.y - other.y) < tol 31 | 32 | def angle_to_point(self, other: "Point") -> float: 33 | angle = np.arctan2(other.y - self.y, other.x - self.x) + (2 * np.pi) 34 | return angle % (2 * np.pi) 35 | 36 | def draw_point(self, ax): 37 | ax.plot(self.x, self.y, "k.") 38 | 39 | def __add__(self, other): 40 | return Point(self.x + other.x, self.y + other.y) 41 | 42 | def __sub__(self, other): 43 | return Point(self.x - other.x, self.y - other.y) 44 | 45 | def __neg__(self): 46 | return Point(-self.x, -self.y) 47 | 48 | def __str__(self): 49 | return f"Point({self.x:.2f}, {self.y:.2f})" 50 | 51 | 52 | @attr.s 53 | class Arc: 54 | """Represents an arc (section of perimeter of circle)""" 55 | 56 | center: Point = attr.ib() 57 | radius: float = attr.ib() 58 | thetas: Optional[Tuple[float, float]] = attr.ib() 59 | 60 | @radius.validator 61 | def check_radius(self, attribute, value): 62 | assert value > 0, "Radius must be greater than 0" 63 | 64 | @thetas.validator 65 | def check_thetas(self, attribute, value): 66 | if value is None: 67 | return 68 | 69 | assert value[0] <= value[1], "Thetas must be increasing" 70 | assert all(isinstance(f, float) for f in value), "Thetas must be floats" 71 | assert all(0 <= x for x in value), "Thetas must be positive" 72 | 73 | def thetas_intersection(self, other: "Arc") -> Optional[Tuple[float, float]]: 74 | """ 75 | Returns the intersection of the two arcs. This is not a set intersection 76 | but rather the intersection of the thetas. 77 | 78 | Args: 79 | other (Arc): The other arc to find the intersection of 80 | 81 | Returns: 82 | Optional[Tuple[float, float]]: The thetas of the intersection 83 | """ 84 | assert self.center == other.center 85 | assert self.radius == other.radius 86 | 87 | if self.is_empty or other.is_empty: 88 | return None 89 | 90 | two_pi = 2 * np.pi 91 | 92 | # normalize everything by 2pi 93 | self_thetas = [x % two_pi for x in self.thetas] # type: ignore 94 | other_thetas = [x % two_pi for x in other.thetas] # type: ignore 95 | 96 | # we think about the relative arrangement of the arcs instead of trying 97 | # to reason about "self" or "other". Without losing generality we 98 | # arrange them so that whichever arc starts with the least theta is the 99 | # "origin" or reference arc. By definition the start end point of this 100 | # arc cannot be in the intersection of the two arcs 101 | base_range = min([self_thetas, other_thetas], key=lambda x: x[0]) 102 | other_range = max([self_thetas, other_thetas], key=lambda x: x[0]) 103 | 104 | base_start, base_end = base_range 105 | other_start, other_end = other_range 106 | 107 | # print(base_range) 108 | # print(other_range) 109 | 110 | # some quick rearranging so that the ends are always greater than the 111 | # starts 112 | if base_end < base_start: 113 | base_end += two_pi 114 | if other_end < other_start: 115 | other_end += two_pi 116 | 117 | # quick sanity check 118 | assert base_start <= base_end 119 | assert other_start <= other_end 120 | 121 | # these distances from the "origin point" (other_start) are all we need 122 | # to reason about what is going on 123 | dist_to_other_start = (other_start - base_start) % two_pi 124 | dist_to_base_end = (base_end - base_start) % two_pi 125 | dist_to_other_end = (other_end - base_start) % two_pi 126 | 127 | # print("dist to other start", dist_to_other_start) 128 | # print("dist to base end", dist_to_base_end) 129 | # print("dist to other end", dist_to_other_end) 130 | # print() 131 | 132 | # if the distance to base end is closer than other start then either 133 | # base is fully encapsulated by other or there is no intersection 134 | if dist_to_base_end < dist_to_other_start: 135 | 136 | if dist_to_other_end < dist_to_base_end: 137 | int_end = other_end % two_pi 138 | if int_end < base_start: 139 | int_end += two_pi 140 | pts = (base_start, int_end) 141 | return pts 142 | 143 | # if other start is closer to base end than to other end then base 144 | # is fully encapsulated 145 | dist_other_start_to_base_end = (base_end - other_start) % two_pi 146 | dist_other_start_to_other_end = (other_end - other_start) % two_pi 147 | if dist_other_start_to_base_end < dist_other_start_to_other_end: 148 | return (base_start, base_end) 149 | 150 | return None 151 | 152 | # if the other end is closer than the other start then the intersection 153 | # is the base start and the other end 154 | if dist_to_other_end < dist_to_other_start: 155 | # do some wraparound quick checking to make sure we take the right 156 | # arc 157 | int_end = other_end % two_pi 158 | if int_end < base_start: 159 | int_end += two_pi 160 | 161 | return (base_start, int_end) 162 | 163 | # otherwise whichever end is closer to the relative start is the end of 164 | # the intersection 165 | if dist_to_base_end < dist_to_other_end: 166 | return (other_start, base_end) 167 | else: 168 | # if we have made it here then the other arc must be encapsulated by 169 | # the base arc and thus the length must be <= the base arc's length 170 | assert other_start - other_end <= base_end - base_start 171 | return (other_start, other_end) 172 | 173 | def __str__(self) -> str: 174 | status = "Arc: " 175 | status += f"center: {self.center}, " 176 | status += f"radius: {self.radius}, " 177 | status += f"thetas: {self.thetas}, " 178 | status += f"endpoints: {self.end_points}" 179 | return status 180 | 181 | @property 182 | def end_points(self): 183 | if self.is_empty: 184 | return [] 185 | 186 | points = [ 187 | Point( 188 | self.radius * np.cos(theta) + self.x, 189 | self.radius * np.sin(theta) + self.y, 190 | ) 191 | for theta in self.thetas 192 | ] 193 | return points 194 | 195 | @property 196 | def x(self): 197 | return self.center.x 198 | 199 | @property 200 | def y(self): 201 | return self.center.y 202 | 203 | @property 204 | def arc_length_radians(self): 205 | """Returns the length of the arc, in radians""" 206 | if self.is_empty: 207 | return 0 208 | 209 | diff = self.thetas[1] - self.thetas[0] 210 | 211 | assert diff >= 0, f"Thetas must be increasing: {self.thetas}" 212 | return diff 213 | 214 | def set_empty(self): 215 | self.thetas = None 216 | 217 | def set_thetas(self, thetas: Optional[Tuple[float, float]]): 218 | if thetas is None: 219 | self.set_empty() 220 | else: 221 | assert thetas[0] <= thetas[1], "Thetas must be increasing" 222 | self.thetas = thetas 223 | 224 | @property 225 | def is_empty(self) -> bool: 226 | return self.thetas is None 227 | 228 | def update_with_arc_intersection(self, other: "Arc"): 229 | """ 230 | Modifies this object in place to represent the intersection of this arc 231 | with another arc. Each arc must be a part of the same circle. 232 | 233 | Args: 234 | other (Arc): The other arc to find the intersection of 235 | """ 236 | assert self.center == other.center 237 | assert self.radius == other.radius 238 | assert self.arc_length_radians <= 2 * np.pi 239 | assert other.arc_length_radians <= 2 * np.pi 240 | 241 | start_len = self.arc_length_radians 242 | 243 | # if thetas is None this arc is already empty 244 | if self.is_empty: 245 | return 246 | 247 | # if this arc is a full circle then the intersection is just the other 248 | # arc 249 | if abs(self.arc_length_radians - 2 * np.pi) < 1e-3: 250 | self.set_thetas(other.thetas) 251 | return 252 | 253 | # if the other arc is a full circle then the intersection is just this 254 | # arc 255 | if abs(other.arc_length_radians - 2 * np.pi) < 1e-3: 256 | return 257 | 258 | intersection = self.thetas_intersection(other) 259 | if intersection is None: 260 | # print(f"No intersection between arcs - setting empty") 261 | # print(self) 262 | # print(other) 263 | self.set_empty() 264 | else: 265 | self.set_thetas(intersection) 266 | 267 | new_len = self.arc_length_radians 268 | assert new_len <= start_len, f"New arc length must be <= old length" 269 | 270 | def draw_arc_patch( 271 | self, 272 | ax: plt.Axes, 273 | resolution: int = 50, 274 | color: str = "blue", 275 | ) -> Optional[patches.Polygon]: 276 | """Draws an arc as a generic patches.Polygon 277 | 278 | Args: 279 | arc (Arc): the arc to draw 280 | ax (plt.Axes): the axes to draw the arc on 281 | resolution (int, optional): the resolution of the arc. Defaults to 282 | 50. 283 | color (str, optional): the color of the arc. Defaults to "black". 284 | 285 | Returns: 286 | patches.Polygon: the arc 287 | """ 288 | if self.is_empty: 289 | return None 290 | 291 | radius = self.radius 292 | theta1, theta2 = self.thetas # type: ignore 293 | if theta2 < theta1: 294 | theta2 += 2 * np.pi 295 | # generate the points 296 | theta = np.linspace((theta1), (theta2), resolution) 297 | points = np.vstack( 298 | (radius * np.cos(theta) + self.x, radius * np.sin(theta) + self.y) 299 | ) 300 | # build the polygon and add it to the axes 301 | coloring = to_rgba(color, 0.5) 302 | poly = patches.Polygon(points.T, closed=True, color=coloring, linewidth=0) 303 | ax.add_patch(poly) 304 | return poly 305 | 306 | 307 | @attr.s 308 | class Circle: 309 | """A circle object 310 | 311 | Attributes: 312 | x (float): x coordinate of the center of the circle 313 | y (float): y coordinate of the center of the circle 314 | radius (float): radius of the circle 315 | """ 316 | 317 | center: Point = attr.ib() 318 | radius: float = attr.ib() 319 | 320 | @radius.validator 321 | def check_radius(self, attribute, value): 322 | assert value > 0, "Radius must be greater than 0" 323 | 324 | @property 325 | def x(self): 326 | return self.center.x 327 | 328 | @property 329 | def y(self): 330 | return self.center.y 331 | 332 | def angle_to_point(self, point: Point) -> float: 333 | """returns the angle from the center of the circle to a given point 334 | 335 | Args: 336 | point (Point): [description] 337 | 338 | Returns: 339 | float: [description] 340 | """ 341 | return self.center.angle_to_point(point) 342 | 343 | def is_inside_circle(self, point: Point) -> bool: 344 | """ 345 | Returns true if the given point is inside the circle 346 | 347 | Args: 348 | point (Point): The point to check 349 | 350 | Returns: 351 | bool: True if the point is inside the circle 352 | """ 353 | return (point - self.center).distance <= self.radius 354 | 355 | def completely_contains(self, other: "Circle") -> bool: 356 | """ 357 | Returns true if the given circle is completely inside "self" 358 | 359 | Args: 360 | other (Circle): The other circle to check 361 | 362 | Returns: 363 | bool: True if other is completely inside "self" 364 | """ 365 | 366 | # the other circle is not inside this circle 367 | other_is_inside_self = self.is_inside_circle(other.center) 368 | if not other_is_inside_self: 369 | return False 370 | 371 | # the other circle is inside this circle, but not completely 372 | circ_dist = (other.center - self.center).distance 373 | if circ_dist + other.radius > self.radius: 374 | return False 375 | 376 | return True 377 | 378 | def get_intersection_point_angles( 379 | self, other: "Circle" 380 | ) -> Optional[Tuple[float, float]]: 381 | """[summary] 382 | 383 | Args: 384 | other (Circle): [description] 385 | 386 | Returns: 387 | Optional[Tuple[float, float]]: [description] 388 | """ 389 | d = np.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2) 390 | 391 | if d > self.radius + other.radius: 392 | print("Circles too far away") 393 | return None 394 | 395 | min_rad = min(self.radius, other.radius) 396 | max_rad = max(self.radius, other.radius) 397 | if d + min_rad < max_rad: 398 | print("Circles nested inside eachother") 399 | return None 400 | 401 | l = (self.radius**2 - other.radius**2 + d**2) / (2 * d) 402 | h = np.sqrt(self.radius**2 - l**2) 403 | 404 | x_int_base = self.x + l * (other.x - self.x) / d 405 | x_int_1 = x_int_base + h * (other.y - self.y) / d 406 | x_int_2 = x_int_base - h * (other.y - self.y) / d 407 | 408 | y_int_base = self.y + l * (other.y - self.y) / d 409 | y_int_1 = y_int_base - h * (other.x - self.x) / d 410 | y_int_2 = y_int_base + h * (other.x - self.x) / d 411 | 412 | int_pt_1 = Point(x_int_1, y_int_1) 413 | int_pt_2 = Point(x_int_2, y_int_2) 414 | pt_list = [int_pt_1, int_pt_2] 415 | 416 | centered_pts = [pt - self.center for pt in pt_list] 417 | 418 | assert all(abs(pt.distance - self.radius) < 1e-3 for pt in centered_pts) 419 | 420 | pt_thetas = [pt.theta for pt in centered_pts] 421 | pt_thetas.sort() 422 | assert pt_thetas[0] < pt_thetas[1] 423 | return (float(pt_thetas[0]), float(pt_thetas[1])) 424 | 425 | def get_circle_intersection_arc(self, other: "Circle") -> Optional[Arc]: 426 | """ 427 | Returns the intersection of two circles 428 | 429 | Args: 430 | other (Circle): The other circle to find the intersection of 431 | 432 | Returns: 433 | Optional[Arc]: The arc on this circle representing the boundary of 434 | the intersection of the circles 435 | """ 436 | 437 | # if the other circle is completely inside this circle none of this 438 | # circle makes up the boundary 439 | other_is_completely_inside_self = self.completely_contains(other) 440 | if other_is_completely_inside_self: 441 | print("other is completely inside self") 442 | return None 443 | 444 | # if this circle is completely inside the other circle then the entire 445 | # circle makes up the boundary 446 | self_is_completely_inside_other = other.completely_contains(self) 447 | if self_is_completely_inside_other: 448 | print("self is completely inside other") 449 | return Arc(self.center, self.radius, (0.0, 2 * np.pi)) 450 | 451 | # if there is no intersection of the circles and neither is completely 452 | # inside the other then return None 453 | intersect_is_null = circles_have_no_overlap(self, other) 454 | if intersect_is_null: 455 | print("Circles have no intersection") 456 | return None 457 | 458 | # we've now checked the reasons why there might be no intersection, so 459 | # we get the intersection points 460 | intersect_point_thetas = self.get_intersection_point_angles(other) 461 | assert ( 462 | intersect_point_thetas is not None 463 | ), "Intersection points should not be None - have already weeded out possible causes of that" 464 | 465 | # these are the angles to the intersection points - sorted least to 466 | # greatest 467 | angle_to_pt_1, angle_to_pt_2 = intersect_point_thetas 468 | 469 | # make two arc objects and we will choose between the larger or smaller 470 | # one as described below 471 | arc1 = Arc(self.center, self.radius, (angle_to_pt_1, angle_to_pt_2)) 472 | arc2 = Arc( 473 | self.center, self.radius, (angle_to_pt_2, angle_to_pt_1 + (2 * np.pi)) 474 | ) 475 | larger_arc = max(arc1, arc2, key=lambda arc: arc.arc_length_radians) 476 | smaller_arc = min(arc1, arc2, key=lambda arc: arc.arc_length_radians) 477 | 478 | # from here based on the relative angles between the centers of the 479 | # circles and the intersection points, we can determine whether to use 480 | # the larger or smaller arc. We get both the relative angles to avoid 481 | # having to think about 2pi wraparound issues 482 | angle_to_other_center = self.angle_to_point(other.center) 483 | rel_angle_1 = abs(angle_to_other_center - angle_to_pt_1) 484 | rel_angle_2 = abs(angle_to_other_center - angle_to_pt_2) 485 | relative_angle = min(rel_angle_1, rel_angle_2) 486 | if (relative_angle) > np.pi / 2: 487 | return larger_arc 488 | else: 489 | return smaller_arc 490 | 491 | def draw_ray(self, theta: float, ax: plt.Axes) -> None: 492 | """ 493 | Draws a ray from the center of this circle to a point on the circle 494 | that is the given angle away from the center 495 | 496 | Args: 497 | theta (float): The angle of the ray to draw 498 | """ 499 | dist = 10 500 | ray_end_x = self.x + dist * np.cos(theta) 501 | ray_end_y = self.y + dist * np.sin(theta) 502 | ax.plot([self.x, ray_end_x], [self.y, ray_end_y], color="black") 503 | 504 | def draw_circle_patch( 505 | self, 506 | ax: plt.Axes, 507 | color: str = "black", 508 | ) -> patches.Circle: 509 | """Draws a circle as a generic patches.Circle 510 | 511 | Args: 512 | circle (Circle): the circle to draw 513 | ax (plt.Axes): the axes to draw the circle on 514 | resolution (int, optional): the resolution of the circle. Defaults to 515 | 50. 516 | color (str, optional): the color of the circle. Defaults to "black". 517 | 518 | Returns: 519 | patches.Circle: the circle 520 | """ 521 | circle = patches.Circle((self.x, self.y), self.radius, fill=False, color=color) 522 | ax.add_patch(circle) 523 | return circle 524 | 525 | 526 | @attr.s 527 | class CircleIntersection: 528 | """ 529 | Represents the intersection of generic number of circles 530 | 531 | Attributes: 532 | circles (List[Circle]): The circles that make up the intersection 533 | arcs (List[Arc]): The arcs that make up the boundary of the intersection (these are what are actually drawn on the plot) 534 | intersect_is_null (bool): this is a flag for when we've decided that the intersection must be empty from here onwards 535 | """ 536 | 537 | circles: List[Circle] = attr.ib(default=attr.Factory(list)) 538 | intersection_arcs: List[Arc] = attr.ib(default=attr.Factory(list)) 539 | intersect_is_null: bool = attr.ib(default=False) 540 | 541 | def add_circle(self, new_circle: Circle) -> None: 542 | """ 543 | Adds a circle to the intersection 544 | 545 | Args: 546 | new_circle (Circle): the circle to add 547 | """ 548 | self.circles.append(new_circle) 549 | 550 | # if the intersection has been decided to be null then we won't add any 551 | # more arcs 552 | if self.intersect_is_null: 553 | empty_arc = Arc(new_circle.center, new_circle.radius, None) 554 | self.intersection_arcs.append(empty_arc) 555 | return 556 | 557 | # this should only be entered if this is the first circle added to the 558 | # intersection so we can just add it and exit 559 | if len(self.circles) == 1: 560 | first_arc = Arc(new_circle.center, new_circle.radius, (0.0, 2 * np.pi)) 561 | self.intersection_arcs.append(first_arc) 562 | return 563 | 564 | # from here onwards this arc will be iteratively trimmed as we consider 565 | # all of the existing arcs to see if they intersect with the new arc 566 | new_intersection_arc = Arc( 567 | new_circle.center, new_circle.radius, (0.0, 2 * np.pi) 568 | ) 569 | 570 | assert len(self.circles[:-1]) == len( 571 | self.intersection_arcs 572 | ), "The number of circles and arcs should be the same" 573 | for existing_circ, existing_arc in zip( 574 | self.circles[:-1], self.intersection_arcs 575 | ): 576 | 577 | # if any of the existing circles have no intersection with the new 578 | # circle then the intersection is null from here onwards 579 | if circles_have_no_overlap(new_circle, existing_circ): 580 | print("No intersection in circles - setting null") 581 | empty_arc = Arc(new_circle.center, new_circle.radius, None) 582 | self.intersection_arcs.append(empty_arc) 583 | for arc in self.intersection_arcs: 584 | arc.set_empty() 585 | self.intersect_is_null = True 586 | return 587 | 588 | # if any of the existing circles are completely inside of the new 589 | # then we won't add it to the intersection list and we exit because 590 | # the intersection is already a subset of this circle 591 | if new_circle.completely_contains(existing_circ): 592 | empty_arc = Arc(new_circle.center, new_circle.radius, None) 593 | self.intersection_arcs.append(empty_arc) 594 | return 595 | 596 | # Note: if the existing arc is already empty then this existing 597 | # circle has no impact on that arc but it can still impact this new 598 | # arc (this was causing a bug for me!) 599 | 600 | # for every circle this circle is completely inside of we can 601 | # eliminate that circle and its arc 602 | if existing_circ.completely_contains(new_circle): 603 | # print("Completely contains - setting empty") 604 | existing_arc.set_empty() 605 | continue 606 | 607 | # if we've made it this far then they must have an intersection 608 | new_intersect = new_circle.get_circle_intersection_arc(existing_circ) 609 | assert new_intersect is not None, "new_intersect should not be None" 610 | new_intersection_arc.update_with_arc_intersection(new_intersect) 611 | 612 | exist_intersect = existing_circ.get_circle_intersection_arc(new_circle) 613 | assert exist_intersect is not None, "exist_intersect should not be None" 614 | existing_arc.update_with_arc_intersection(exist_intersect) 615 | 616 | self.intersection_arcs.append(new_intersection_arc) 617 | 618 | assert all(x is not None for x in self.circles), "Circles should not be None" 619 | assert all( 620 | x is not None for x in self.intersection_arcs 621 | ), f"Intersection arcs should not be None: {self.intersection_arcs}" 622 | 623 | def draw_circles(self, ax: plt.Axes, color: str = "red") -> None: 624 | """ 625 | Draws all of the circles in the intersection 626 | 627 | Args: 628 | ax (plt.Axes): the axes to draw the circles on 629 | color (str, optional): the color of the circles. Defaults to "red". 630 | """ 631 | for circle in self.circles[-3:]: 632 | circle.draw_circle_patch(ax, color=color) 633 | 634 | def draw_intersection(self, ax: plt.Axes, color: str = "blue") -> None: 635 | """ 636 | Draws the intersection of all of the circles 637 | 638 | Args: 639 | ax (plt.Axes): the axes to draw the intersection on 640 | color (str, optional): the color of the intersection. Defaults to 641 | "blue". 642 | """ 643 | if self.intersect_is_null: 644 | return 645 | 646 | fill_points: Set[Point] = set() 647 | for arc in self.intersection_arcs: 648 | if not arc.is_empty: 649 | 650 | # get the endpoints for filling in the intersection not captured 651 | # by the arcs 652 | endpt1, endpt2 = arc.end_points 653 | if all(not pt.is_close(endpt1) for pt in fill_points): 654 | fill_points.add(endpt1) 655 | if all(not pt.is_close(endpt2) for pt in fill_points): 656 | fill_points.add(endpt2) 657 | 658 | # draw the arc 659 | arc.draw_arc_patch(ax, color=color) 660 | 661 | # if the intersection not captured by the arcs is more than a line 662 | # segment then we make the polygon and fill it in 663 | if len(fill_points) > 2: 664 | xy_pts = [] 665 | for pt in fill_points: 666 | xy_pts.append([pt.x, pt.y]) 667 | 668 | hull = ConvexHull(np.asarray(xy_pts)) 669 | hull_pts = np.asarray(xy_pts)[hull.vertices] 670 | 671 | coloring = to_rgba(color, 0.5) 672 | fill_poly = patches.Polygon(hull_pts, True, fc=coloring, linewidth=0) 673 | ax.add_patch(fill_poly) 674 | 675 | 676 | def circles_have_no_overlap(c1: Circle, c2: Circle) -> bool: 677 | """ 678 | Returns true if the given circle has no overlap with "self" 679 | 680 | Args: 681 | c1: The first circle 682 | c2: The second circle 683 | 684 | Returns: 685 | bool: True if the circles have no overlap 686 | 687 | """ 688 | circle_dist = (c1.center - c2.center).distance 689 | circles_have_no_overlap = circle_dist > c1.radius + c2.radius 690 | return circles_have_no_overlap 691 | 692 | 693 | def get_random_circle( 694 | max_x: float, 695 | max_y: float, 696 | max_radius: float, 697 | ) -> Circle: 698 | """ 699 | Returns a random circle with the given radius 700 | 701 | Args: 702 | max_x (float): The maximum x value of the circle 703 | max_y (float): The maximum y value of the circle 704 | max_radius (float): The radius of the circle 705 | 706 | Returns: 707 | Circle: A random circle 708 | """ 709 | x = np.random.uniform(-max_x, max_x) 710 | y = np.random.uniform(-max_y, max_y) 711 | center = Point(x, y) 712 | rad = np.random.uniform(0, max_radius) 713 | return Circle(center, rad) 714 | 715 | 716 | if __name__ == "__main__": 717 | np.random.seed(0) 718 | 719 | t0 = False 720 | t1 = False 721 | t2 = True 722 | 723 | if t0: 724 | a1_angles = (4.889303064250705, 7.622526837716786) 725 | a2_angles = (7.371161260586642, 10.104385034052722) 726 | 727 | a1 = Arc(Point(0, 0), 1, a1_angles) 728 | a2 = Arc(Point(0, 0), 1, a2_angles) 729 | 730 | fig, ax = plt.subplots() 731 | a1.draw_arc_patch(ax, color="red") 732 | a2.draw_arc_patch(ax, color="blue") 733 | 734 | a1.update_with_arc_intersection(a2) 735 | a1.draw_arc_patch(ax, color="green") 736 | 737 | ax.set_xlim(-3, 3) 738 | ax.set_ylim(-3, 3) 739 | ax.set_aspect("equal") 740 | plt.show(block=True) 741 | plt.show() 742 | 743 | elif t1: 744 | 745 | # fig, ax = plt.subplots() 746 | n_step = 100 747 | for i in range(n_step): 748 | print() 749 | fig, ax = plt.subplots() 750 | 751 | a1_start = np.random.uniform(0, 2 * np.pi) 752 | a1_end = a1_start + np.random.uniform(0, np.pi) 753 | a1_angles = (a1_start, a1_end) 754 | a1 = Arc(Point(0, 0), 1, a1_angles) 755 | 756 | offset = i * np.pi / (n_step / 6) + np.pi / 4 757 | a2_angles = (a1_start + offset, a1_end + offset) 758 | a2 = Arc(Point(0, 0), 1, a2_angles) 759 | 760 | a1.draw_arc_patch(ax, color="red") 761 | a2.draw_arc_patch(ax, color="blue") 762 | 763 | a1.update_with_arc_intersection(a2) 764 | a1.draw_arc_patch(ax, color="green") 765 | 766 | ax.set_xlim(-3, 3) 767 | ax.set_ylim(-3, 3) 768 | ax.set_aspect("equal") 769 | plt.show(block=True) 770 | # plt.pause(0.2) 771 | # ax.patches = [] 772 | 773 | elif t2: 774 | num_circles = 5 775 | 776 | fig, ax = plt.subplots() 777 | for _ in range(100): 778 | 779 | c1 = Circle(Point(0, 0), 1) 780 | intersect_list = CircleIntersection() 781 | intersect_list.add_circle(c1) 782 | max_x = 0.5 783 | max_y = 0.5 784 | max_radius = 1 785 | 786 | for _ in range(num_circles - 1): 787 | rand_circ = get_random_circle(max_x, max_y, max_radius) 788 | rand_circ.radius = 1 789 | intersect_list.add_circle(rand_circ) 790 | 791 | # draw what we've found 792 | intersect_list.draw_intersection(ax) 793 | # intersect_list.draw_circles(ax) 794 | # for circ in intersect_list.circles: 795 | # print(circ) 796 | # for arc in intersect_list.intersection_arcs: 797 | # print(arc) 798 | # print() 799 | 800 | ax.set_xlim(-3, 3) 801 | ax.set_ylim(-3, 3) 802 | ax.set_aspect("equal") 803 | plt.show(block=False) 804 | plt.pause(0.3) 805 | # ax.patches = [] 806 | ax.clear() 807 | -------------------------------------------------------------------------------- /score/utils/gurobi_utils.py: -------------------------------------------------------------------------------- 1 | import gurobipy as gp 2 | from gurobipy import GRB 3 | import numpy as np 4 | from typing import List, Dict, Tuple, MutableMapping, Union 5 | from score.utils.matrix_utils import get_random_transformation_matrix 6 | from py_factor_graph.factor_graph import FactorGraphData 7 | from py_factor_graph.measurements import ( 8 | FGRangeMeasurement, 9 | POSE_MEASUREMENT_TYPES, 10 | PoseMeasurement2D, 11 | PoseMeasurement3D, 12 | ) 13 | from py_factor_graph.priors import LandmarkPrior2D, LandmarkPrior3D 14 | from py_factor_graph.utils.solver_utils import ( 15 | SolverResults, 16 | VariableValues, 17 | save_to_tum, 18 | ) 19 | from py_factor_graph.utils.plot_utils import visualize_solution 20 | from py_factor_graph.utils.matrix_utils import ( 21 | round_to_special_orthogonal, 22 | get_random_rotation_matrix, 23 | ) 24 | from attrs import define, field, validators 25 | 26 | SOCP_RELAXATION = "SOCP" 27 | QCQP_RELAXATION = "QCQP" 28 | ACCEPTABLE_RELAXATIONS = [SOCP_RELAXATION, QCQP_RELAXATION] 29 | 30 | RANDOM_INIT = "random" 31 | ZERO_INIT = "zero" 32 | ODOM_INIT = "odom" 33 | GT_INIT = "gt" 34 | ACCEPTABLE_INIT = [RANDOM_INIT, ZERO_INIT, ODOM_INIT, GT_INIT] 35 | 36 | 37 | def is_dimension(instance, attribute, value) -> None: 38 | """ 39 | Return validator for dimension. 40 | 41 | Args: 42 | value (int): value to validate 43 | 44 | Returns: 45 | None 46 | """ 47 | if not isinstance(value, int): 48 | raise ValueError(f"{value} is not an int") 49 | if not value in [2, 3]: 50 | raise ValueError(f"Value {value} is not 2 or 3") 51 | 52 | 53 | @define 54 | class VariableCollection: 55 | dim: int = field(validator=is_dimension) 56 | pose_vars: Dict[str, gp.MVar] = field(factory=dict) 57 | landmark_vars: Dict[str, gp.MVar] = field(factory=dict) 58 | distance_vars: MutableMapping[Tuple[str, str], Union[gp.MVar, gp.Var]] = field( 59 | factory=dict 60 | ) 61 | 62 | def _check_is_new_variable(self, var_name: Union[str, Tuple[str, str]]): 63 | if isinstance(var_name, tuple): 64 | if var_name in self.distance_vars: 65 | raise ValueError( 66 | f"Variable name {var_name} already exists in distance_vars" 67 | ) 68 | elif isinstance(var_name, str): 69 | if var_name in self.pose_vars: 70 | raise ValueError( 71 | f"Variable name {var_name} already exists in pose_vars" 72 | ) 73 | if var_name in self.landmark_vars: 74 | raise ValueError( 75 | f"Variable name {var_name} already exists in landmark_vars" 76 | ) 77 | else: 78 | raise ValueError( 79 | f"Variable name {var_name} is not a valid type: {type(var_name)}" 80 | ) 81 | 82 | def add_pose_variable(self, pose_var: gp.MVar, name: str): 83 | self._check_is_new_variable(name) 84 | self.pose_vars[name] = pose_var 85 | 86 | def add_landmark_variable(self, landmark_var: gp.MVar, name: str): 87 | self._check_is_new_variable(name) 88 | self.landmark_vars[name] = landmark_var 89 | 90 | def add_distance_variable(self, distance_var: gp.MVar, dist_key: Tuple[str, str]): 91 | self._check_is_new_variable(dist_key) 92 | self.distance_vars[dist_key] = distance_var 93 | 94 | def set_distance_variables(self, dist_vars: gp.tupledict): 95 | assert ( 96 | self.distance_vars == {} 97 | ), f"Trying to set distance variables when it is non-empty, this is not the intended usage: {self.distance_vars}" 98 | self.distance_vars = dist_vars 99 | 100 | def get_pose_var(self, pose_name: str) -> gp.MVar: 101 | return self.pose_vars[pose_name] 102 | 103 | def get_translation_var(self, var_name: str) -> gp.MVar: 104 | if var_name in self.pose_vars: 105 | return self.pose_vars[var_name][:, -1] 106 | elif var_name in self.landmark_vars: 107 | return self.landmark_vars[var_name] 108 | else: 109 | raise ValueError(f"Variable name {var_name} not found") 110 | 111 | def get_distance_var(self, dist_key: Tuple[str, str]) -> gp.MVar: 112 | return self.distance_vars[dist_key] 113 | 114 | def get_variable_values(self) -> VariableValues: 115 | def _clean_pose_est(pose_est: np.ndarray) -> np.ndarray: 116 | # each pose needs to be homogenized and rotation matrix needs to be 117 | # orthogonalized 118 | rot_est = pose_est[: self.dim, : self.dim] 119 | rounded_rot = round_to_special_orthogonal(rot_est) 120 | 121 | # make a new (homogeneous) pose estimate with the rounded rotation 122 | new_pose_est = np.eye(self.dim + 1) 123 | new_pose_est[: self.dim, : self.dim] = rounded_rot 124 | new_pose_est[: self.dim, -1] = pose_est[: self.dim, -1] 125 | return new_pose_est 126 | 127 | def _clean_dist_est(dist_est: Union[float, np.ndarray]) -> np.ndarray: 128 | if isinstance(dist_est, float): 129 | return np.array([dist_est]) 130 | else: 131 | return dist_est 132 | 133 | pose_vals = {k: _clean_pose_est(v.X) for k, v in self.pose_vars.items()} 134 | landmark_vals = {k: v.X for k, v in self.landmark_vars.items()} 135 | dist_vals = {k: _clean_dist_est(v.X) for k, v in self.distance_vars.items()} 136 | return VariableValues(self.dim, pose_vals, landmark_vals, dist_vals) 137 | 138 | 139 | def _check_valid_relaxation(relaxation: str): 140 | if relaxation not in ACCEPTABLE_RELAXATIONS: 141 | raise ValueError( 142 | f"Relaxation {relaxation} is not supported. " 143 | f"Acceptable relaxations are {ACCEPTABLE_RELAXATIONS}" 144 | ) 145 | 146 | 147 | def vec_norm_sq(vec: Union[gp.MLinExpr, gp.MVar]) -> gp.MQuadExpr: 148 | """ 149 | Returns the squared L2 norm of a vector. 150 | 151 | Args: 152 | vec (Union[gp.MLinExpr, gp.Mvar]): The vector to get the norm of. 153 | 154 | Returns: 155 | gp.MQuadExpr: The norm of the vector. 156 | """ 157 | return gp.quicksum(vec * vec) 158 | 159 | 160 | def mat_frob_norm_sq(mat: Union[gp.MLinExpr, gp.MVar]) -> gp.MQuadExpr: 161 | """ 162 | Returns the squared Frobenius norm of a matrix. 163 | 164 | Args: 165 | mat (Union[gp.MLinExpr, gp.Mvar]): The matrix to get the norm of. 166 | 167 | Returns: 168 | gp.MQuadExpr: The norm of the matrix. 169 | """ 170 | return gp.quicksum(gp.quicksum((mat * mat))) 171 | 172 | 173 | def initialize_model( 174 | variables: VariableCollection, 175 | model: gp.Model, 176 | fg: FactorGraphData, 177 | relaxation_type: str, 178 | ): 179 | _check_valid_relaxation(relaxation_type) 180 | add_all_variables(variables, model, fg, relaxation_type) 181 | first_pose = fg.pose_variables[0][0] 182 | pose_var = variables.get_pose_var(first_pose.name) 183 | pin_pose(pose_var, model) 184 | add_distance_constraints(variables, model, relaxation_type) 185 | cost = get_full_cost_objective(variables, fg, relaxation_type) 186 | model.setObjective(cost) 187 | model.update() 188 | 189 | 190 | def extract_solver_results( 191 | model: gp.Model, vars: VariableCollection, data: FactorGraphData 192 | ) -> SolverResults: 193 | solver_vals = vars.get_variable_values() 194 | solve_time = model.Runtime 195 | solved = model.status == GRB.Status.OPTIMAL 196 | pose_chain_names = data.get_pose_chain_names() 197 | solve_results = SolverResults( 198 | variables=solver_vals, 199 | total_time=solve_time, 200 | solved=solved, 201 | pose_chain_names=pose_chain_names, 202 | ) 203 | return solve_results 204 | 205 | 206 | def get_model() -> gp.Model: 207 | model = gp.Model() 208 | model.Params.OutputFlag = 0 209 | # model.Params.Aggregate = 0 210 | # model.Params.AggFill = 0 211 | # model.Params.Presolve = 0 212 | model.Params.BarQCPConvTol = 1e-1 213 | # model.Params.ScaleFlag = 2 214 | 215 | return model 216 | 217 | 218 | ##### set up variables ##### 219 | 220 | 221 | def add_all_variables( 222 | variables: VariableCollection, 223 | mod: gp.Model, 224 | fg: FactorGraphData, 225 | relaxation_type: str, 226 | ) -> None: 227 | _check_valid_relaxation(relaxation_type) 228 | add_pose_variables(variables, mod, fg) 229 | add_landmark_variables(variables, mod, fg) 230 | add_distance_variables(variables, mod, fg, relaxation_type) 231 | 232 | 233 | def add_pose_variables( 234 | vars_collection: VariableCollection, mod: gp.Model, fg: FactorGraphData 235 | ): 236 | dim = fg.dimension 237 | for pose_chain in fg.pose_variables: 238 | for pose in pose_chain: 239 | pose_name = pose.name 240 | pose_var = mod.addMVar( 241 | shape=(dim, dim + 1), 242 | lb=-gp.GRB.INFINITY, 243 | ub=gp.GRB.INFINITY, 244 | name=pose_name, 245 | ) 246 | vars_collection.add_pose_variable(pose_var, pose_name) 247 | 248 | 249 | def add_landmark_variables( 250 | vars_collection: VariableCollection, mod: gp.Model, fg: FactorGraphData 251 | ): 252 | dim = fg.dimension 253 | for landmark_var in fg.landmark_variables: 254 | landmark_name = landmark_var.name 255 | landmark_var = mod.addMVar( 256 | shape=(dim,), lb=-gp.GRB.INFINITY, ub=gp.GRB.INFINITY, name=landmark_name 257 | ) 258 | vars_collection.add_landmark_variable(landmark_var, landmark_name) 259 | 260 | 261 | def add_distance_variables( 262 | vars_collection: VariableCollection, 263 | mod: gp.Model, 264 | fg: FactorGraphData, 265 | relaxation_type: str, 266 | ): 267 | """ 268 | Adds variables to the model that represent the distances between the robot's 269 | landmarks and the landmarks. 270 | 271 | Args: 272 | 273 | 274 | Returns: 275 | Dict[Tuple[str, str], np.ndarray]: The dict of variables representing 276 | the distances between the robot's landmarks and the landmarks. 277 | """ 278 | assert relaxation_type in ACCEPTABLE_RELAXATIONS 279 | 280 | distances: Dict[Tuple[str, str], np.ndarray] = {} 281 | num_range_measures = len(fg.range_measurements) 282 | if num_range_measures == 0: 283 | return distances 284 | 285 | # if we are using the SOCP relaxation then we get all keys and use a single 286 | # call to instantiate all variables. Otherwise, as the distance variables 287 | # are matrices, we will have to instantiate them one by one. 288 | dist_keys = [(meas.first_key, meas.second_key) for meas in fg.range_measurements] 289 | if relaxation_type == "SOCP": 290 | dist_vars = mod.addVars( 291 | dist_keys, 292 | lb=0, 293 | ) 294 | vars_collection.set_distance_variables(dist_vars) 295 | elif relaxation_type == "QCQP": 296 | for dist_key in dist_keys: 297 | name = f"dist_{dist_key[0]}_{dist_key[1]}" 298 | vars_collection.add_distance_variable( 299 | mod.addMVar( 300 | shape=(fg.dimension,), 301 | lb=-gp.GRB.INFINITY, 302 | ub=gp.GRB.INFINITY, 303 | name=name, 304 | ), 305 | dist_key, 306 | ) 307 | else: 308 | raise ValueError(f"Relaxation type {relaxation_type} not supported") 309 | 310 | return distances 311 | 312 | 313 | ##### set up constraints ##### 314 | 315 | 316 | def pin_pose( 317 | pose_var: gp.MVar, 318 | mod: gp.Model, 319 | ): 320 | """ 321 | Pins the pose variable to the identity matrix. 322 | 323 | Args: 324 | pose_var (gp.MVar): The pose variable to pin. 325 | mod (gp.Model): The model to add the constraint to. 326 | """ 327 | dim = pose_var.shape[0] 328 | for i in range(dim): 329 | for j in range(dim + 1): 330 | if i == j: 331 | mod.addConstr(pose_var[i, j] == 1) 332 | else: 333 | mod.addConstr(pose_var[i, j] == 0) 334 | 335 | 336 | def add_distance_constraints( 337 | variables: VariableCollection, 338 | mod: gp.Model, 339 | relaxation_type: str, 340 | ): 341 | if relaxation_type == "QCQP": 342 | # constrain distance variables to be within unit ball 343 | for dist_var in variables.distance_vars.values(): 344 | mod.addConstr(vec_norm_sq(dist_var) <= 1) 345 | if relaxation_type == "SOCP": 346 | # create second-order cone constraints 347 | # ||t_i - t_j||^2 <= d_ij^2 348 | for dist_key, dist_var in variables.distance_vars.items(): 349 | trans_i = variables.get_translation_var(dist_key[0]) 350 | trans_j = variables.get_translation_var(dist_key[1]) 351 | diff = trans_i - trans_j 352 | mod.addConstr(vec_norm_sq(diff) <= dist_var**2) 353 | 354 | 355 | ##### set up objective ##### 356 | 357 | 358 | def get_full_cost_objective( 359 | variables: VariableCollection, data: FactorGraphData, relaxation_type: str 360 | ) -> gp.MQuadExpr: 361 | """Get the full cost objective function for the factor graph. 362 | 363 | Args: 364 | variables (VariableCollection): the variables in the model 365 | data (FactorGraphData): the factor graph data 366 | relaxation_type (str): the relaxation type to use 367 | 368 | Returns: 369 | gp.MQuadExpr: the full cost objective function 370 | """ 371 | _check_valid_relaxation(relaxation_type) 372 | cost = 0 373 | cost += get_all_odom_costs(variables, data) 374 | cost += get_all_loop_closure_costs(variables, data) 375 | cost += get_all_range_costs(variables, data, relaxation_type) 376 | cost += get_all_landmark_prior_costs(variables, data) 377 | return cost 378 | 379 | 380 | def get_all_odom_costs( 381 | variables: VariableCollection, 382 | data: FactorGraphData, 383 | ): 384 | """Add the cost associated with the odometry measurements as: 385 | 386 | translation component of cost 387 | k_ij * ||t_i - t_j - R_i @ t_ij^meas||^2 388 | 389 | rotation component of cost 390 | tau_ij * || R_j - (R_i @ R_ij^\top) ||_\frob^2 391 | 392 | Args: 393 | variables (VariableCollection): the variables in the model 394 | data (FactorGraphData): the factor graph data 395 | 396 | """ 397 | cost = 0 398 | for odom_chain in data.odom_measurements: 399 | for odom_measure in odom_chain: 400 | pose_i = variables.get_pose_var(odom_measure.base_pose) 401 | pose_j = variables.get_pose_var(odom_measure.to_pose) 402 | cost += get_relative_pose_cost_expression(pose_i, pose_j, odom_measure) 403 | 404 | return cost 405 | 406 | 407 | def get_all_loop_closure_costs( 408 | variables: VariableCollection, 409 | data: FactorGraphData, 410 | ): 411 | """Add the cost associated with the loop closure measurements as: 412 | 413 | translation component of cost 414 | k_ij * ||t_i - t_j - R_i @ t_ij^meas||^2 415 | 416 | rotation component of cost 417 | tau_ij * || R_j - (R_i @ R_ij^\top) ||_\frob^2 418 | 419 | Args: 420 | variables (VariableCollection): the variables in the model 421 | data (FactorGraphData): the factor graph data 422 | 423 | """ 424 | cost = 0.0 425 | for loop_measure in data.loop_closure_measurements: 426 | pose_i = variables.get_pose_var(loop_measure.base_pose) 427 | pose_j = variables.get_pose_var(loop_measure.to_pose) 428 | cost += get_relative_pose_cost_expression(pose_i, pose_j, loop_measure) 429 | 430 | return cost 431 | 432 | 433 | def get_all_landmark_prior_costs( 434 | var_collection: VariableCollection, 435 | data: FactorGraphData, 436 | ) -> gp.MQuadExpr: 437 | cost = 0.0 438 | for landmark_prior in data.landmark_priors: 439 | # translation component of cost 440 | # k_ij * ||t_i - t_ij||^2 441 | t_i = var_collection.get_translation_var(landmark_prior.name) 442 | translation_term = t_i - landmark_prior.translation_vector 443 | unweighted_cost = vec_norm_sq(translation_term) 444 | weight = landmark_prior.translation_precision 445 | cost += weight * unweighted_cost 446 | return cost 447 | 448 | 449 | def get_all_range_costs( 450 | variables: VariableCollection, data: FactorGraphData, relaxation: str 451 | ) -> gp.MQuadExpr: 452 | """Add the cost associated with the range measurements as: 453 | 454 | k_ij * ||d_ij - d_ij^meas||^2 455 | 456 | Args: 457 | variables (VariableCollection): the variables in the model 458 | data (FactorGraphData): the factor graph data 459 | relaxation (str): the relaxation type 460 | 461 | """ 462 | cost = 0.0 463 | for range_measure in data.range_measurements: 464 | dist_key = (range_measure.first_key, range_measure.second_key) 465 | cost += get_single_range_cost( 466 | variables.get_translation_var(dist_key[0]), 467 | variables.get_translation_var(dist_key[1]), 468 | variables.get_distance_var(dist_key), 469 | range_measure, 470 | relaxation, 471 | ) 472 | return cost 473 | 474 | 475 | def get_single_range_cost( 476 | t_i: gp.MVar, 477 | t_j: gp.MVar, 478 | d_ij: gp.MVar, 479 | measure: FGRangeMeasurement, 480 | relaxation: str, 481 | ) -> gp.QuadExpr: 482 | _check_valid_relaxation(relaxation) 483 | 484 | # SOCP cost = k_ij * ||d_ij - d_ij^meas||^2 485 | # QCQP cost = k_ij * ||t_i - t_j - (d_ij^meas) * d_ij||^2 486 | if relaxation == "SOCP": 487 | unweighted_cost = measure.dist**2 - 2 * measure.dist * d_ij + d_ij**2 488 | elif relaxation == "QCQP": 489 | intermed = t_i - t_j - d_ij * measure.dist 490 | # print(f"t_i: {t_i}") 491 | # print(f"t_j: {t_j}") 492 | # print(f"d_ij: {d_ij}") 493 | # print(f"ti - tj: {t_i - t_j}") 494 | # print(f"measure.dist: {measure.dist}") 495 | # print(f"intermed: {intermed}") 496 | unweighted_cost = vec_norm_sq(intermed) 497 | else: 498 | raise ValueError(f"Relaxation {relaxation} is not supported.") 499 | 500 | cost = measure.precision * unweighted_cost 501 | return cost 502 | 503 | 504 | def get_relative_pose_cost_expression( 505 | pose_i: gp.MVar, pose_j: gp.MVar, measure: POSE_MEASUREMENT_TYPES 506 | ) -> gp.QuadExpr: 507 | t_i = pose_i[:, -1] 508 | t_j = pose_j[:, -1] 509 | R_i = pose_i[:, :-1] 510 | R_j = pose_j[:, :-1] 511 | 512 | # translation component of cost 513 | # k_ij * ||t_i - t_j - R_i @ t_{ij}^{meas}||^2 514 | k_ij = measure.translation_precision 515 | trans_measure = measure.translation_vector 516 | term = t_j - t_i - (R_i @ trans_measure) 517 | trans_obj = k_ij * vec_norm_sq(term) 518 | 519 | # rotation component of cost 520 | # tau_ij * || R_j - (R_i @ R_{ij}^{meas}) ||_\frob 521 | tau_ij = measure.rotation_precision 522 | rot_measure = measure.rotation_matrix 523 | diff_rot_matrix = R_j - (R_i @ rot_measure) 524 | rot_obj = tau_ij * mat_frob_norm_sq(diff_rot_matrix) 525 | 526 | return rot_obj + trans_obj 527 | -------------------------------------------------------------------------------- /score/utils/matrix_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.linalg as la # type: ignore 3 | import scipy.spatial 4 | from typing import List, Tuple, Optional 5 | 6 | import logging 7 | 8 | logger = logging.getLogger(__name__) 9 | 10 | 11 | def apply_transformation_matrix_perturbation( 12 | transformation_matrix, 13 | perturb_magnitude: Optional[float], 14 | perturb_rotation: Optional[float], 15 | ) -> np.ndarray: 16 | """Applies a random SE(2) perturbation to a transformation matrix 17 | 18 | Args: 19 | transformation_matrix ([type]): [description] 20 | perturb_magnitude (Optional[float]): [description] 21 | perturb_rotation (Optional[float]): [description] 22 | 23 | Returns: 24 | np.ndarray: [description] 25 | """ 26 | _check_transformation_matrix(transformation_matrix) 27 | 28 | # get the x/y perturbation 29 | perturb_direction = np.random.uniform(0, 2 * np.pi) 30 | perturb_x = np.cos(perturb_direction) * perturb_magnitude 31 | perturb_y = np.sin(perturb_direction) * perturb_magnitude 32 | 33 | # get the rotation perturbation 34 | perturb_theta = np.random.choice([-1, 1]) * perturb_rotation 35 | 36 | # compose the perturbation into a transformation matrix 37 | rand_trans = np.eye(3) 38 | rand_trans[:2, :2] = get_rotation_matrix_from_theta(perturb_theta) 39 | rand_trans[:2, 2] = perturb_x, perturb_y 40 | _check_transformation_matrix(rand_trans) 41 | 42 | # perturb curr pose 43 | return transformation_matrix @ rand_trans 44 | 45 | 46 | def get_matrix_determinant(mat: np.ndarray) -> float: 47 | """returns the determinant of the matrix 48 | 49 | Args: 50 | mat (np.ndarray): [description] 51 | 52 | Returns: 53 | float: [description] 54 | """ 55 | _check_square(mat) 56 | return float(np.linalg.det(mat)) 57 | 58 | 59 | def round_to_special_orthogonal(mat: np.ndarray) -> np.ndarray: 60 | """ 61 | Rounds a matrix to special orthogonal form. 62 | 63 | Args: 64 | mat (np.ndarray): the matrix to round 65 | 66 | Returns: 67 | np.ndarray: the rounded matrix 68 | """ 69 | _check_square(mat) 70 | dim = mat.shape[0] 71 | try: 72 | S, D, Vh = la.svd(mat) 73 | R_so = S @ Vh 74 | if np.linalg.det(R_so) < 0: 75 | R_so = S @ np.diag([1] * (dim - 1) + [-1]) @ Vh 76 | _check_rotation_matrix(R_so, assert_test=True) 77 | except ValueError: 78 | raise ValueError(f"Could not round matrix to special orthogonal form: {mat}") 79 | return R_so 80 | 81 | 82 | def get_theta_from_rotation_matrix_so_projection(mat: np.ndarray) -> float: 83 | """ 84 | Returns theta from the projection of the matrix M onto the special 85 | orthogonal group 86 | 87 | Args: 88 | mat (np.ndarray): the candidate rotation matrix 89 | 90 | Returns: 91 | float: theta 92 | 93 | """ 94 | R_so = round_to_special_orthogonal(mat) 95 | return get_theta_from_rotation_matrix(R_so) 96 | 97 | 98 | def get_theta_from_rotation_matrix(mat: np.ndarray) -> float: 99 | """ 100 | Returns theta from a matrix M 101 | 102 | Args: 103 | mat (np.ndarray): the candidate rotation matrix 104 | 105 | Returns: 106 | float: theta 107 | """ 108 | _check_rotation_matrix(mat) 109 | mat_dim = mat.shape[0] 110 | assert mat_dim == 2, f"Rotation matrix must be 2x2, got {mat_dim}x{mat_dim}" 111 | return float(np.arctan2(mat[1, 0], mat[0, 0])) 112 | 113 | 114 | def get_quat_from_rotation_matrix(mat: np.ndarray) -> np.ndarray: 115 | """Returns the quaternion from a rotation matrix 116 | 117 | Args: 118 | mat (np.ndarray): the rotation matrix 119 | 120 | Returns: 121 | np.ndarray: the quaternion 122 | """ 123 | _check_rotation_matrix(mat) 124 | mat_dim = mat.shape[0] 125 | 126 | if mat_dim == 2: 127 | rot_matrix = np.eye(3) 128 | rot_matrix[:2, :2] = mat 129 | else: 130 | rot_matrix = mat 131 | 132 | rot = scipy.spatial.transform.Rotation.from_matrix(rot_matrix) 133 | assert isinstance(rot, scipy.spatial.transform.Rotation) 134 | quat = rot.as_quat() 135 | assert isinstance(quat, np.ndarray) 136 | return quat 137 | 138 | 139 | def get_random_vector(dim: int, bounds: Optional[List[float]] = None) -> np.ndarray: 140 | """Returns a random vector of size dim 141 | 142 | Args: 143 | dim (int): the dimension of the vector 144 | 145 | Returns: 146 | np.ndarray: the random vector 147 | """ 148 | if bounds is None: 149 | return np.random.rand(dim) 150 | else: 151 | if dim == 2: 152 | x_min, x_max, y_min, y_max = bounds 153 | return np.array( 154 | [np.random.uniform(x_min, x_max), np.random.uniform(y_min, y_max)] 155 | ) 156 | else: 157 | raise NotImplementedError("Only 2D vectors are supported") 158 | 159 | 160 | def get_rotation_matrix_from_theta(theta: float) -> np.ndarray: 161 | """Returns the rotation matrix from theta 162 | 163 | Args: 164 | theta (float): the angle of rotation 165 | 166 | Returns: 167 | np.ndarray: the rotation matrix 168 | """ 169 | return np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) 170 | 171 | 172 | def get_rotation_matrix_from_transformation_matrix(T: np.ndarray) -> np.ndarray: 173 | """Returns the rotation matrix from the transformation matrix 174 | 175 | Args: 176 | T (np.ndarray): the transformation matrix 177 | 178 | Returns: 179 | np.ndarray: the rotation matrix 180 | """ 181 | _check_transformation_matrix(T) 182 | mat_dim = T.shape[0] 183 | return T[: mat_dim - 1, : mat_dim - 1] 184 | 185 | 186 | def get_theta_from_transformation_matrix(T: np.ndarray) -> float: 187 | """Returns the angle theta from a transformation matrix 188 | 189 | Args: 190 | T (np.ndarray): the transformation matrix 191 | 192 | Returns: 193 | float: the angle theta 194 | """ 195 | _check_transformation_matrix(T) 196 | mat_dim = T.shape[0] 197 | assert mat_dim == 3, f"Transformation matrix must be 3x3, got {mat_dim}x{mat_dim}" 198 | return get_theta_from_rotation_matrix( 199 | get_rotation_matrix_from_transformation_matrix(T) 200 | ) 201 | 202 | 203 | def get_quat_from_transformation_matrix(T: np.ndarray) -> np.ndarray: 204 | """Returns the quaternion from a transformation matrix 205 | 206 | Args: 207 | T (np.ndarray): the transformation matrix 208 | 209 | Returns: 210 | np.ndarray: the quaternion 211 | """ 212 | _check_transformation_matrix(T) 213 | return get_quat_from_rotation_matrix( 214 | get_rotation_matrix_from_transformation_matrix(T) 215 | ) 216 | 217 | 218 | def get_translation_from_transformation_matrix(T: np.ndarray) -> np.ndarray: 219 | """Returns the translation from a transformation matrix 220 | 221 | Args: 222 | T (np.ndarray): the transformation matrix 223 | 224 | Returns: 225 | np.ndarray: the translation 226 | """ 227 | _check_transformation_matrix(T) 228 | mat_dim = T.shape[0] 229 | return T[: mat_dim - 1, mat_dim - 1] 230 | 231 | 232 | def get_random_rotation_matrix(dim: int = 2) -> np.ndarray: 233 | """Returns a random rotation matrix of size dim x dim""" 234 | if dim == 2: 235 | theta = 2 * np.pi * np.random.rand() 236 | return get_rotation_matrix_from_theta(theta) 237 | else: 238 | rand_rot = scipy.spatial.transform.Rotation.random() 239 | assert isinstance(rand_rot, scipy.spatial.transform.Rotation) 240 | rot_mat = rand_rot.as_matrix() 241 | assert isinstance(rot_mat, np.ndarray) 242 | return rot_mat 243 | 244 | 245 | def get_random_transformation_matrix(dim: int = 2) -> np.ndarray: 246 | R = get_random_rotation_matrix(dim) 247 | t = get_random_vector(dim) 248 | return make_transformation_matrix(R, t) 249 | 250 | 251 | def make_transformation_matrix(R: np.ndarray, t: np.ndarray) -> np.ndarray: 252 | """ 253 | Returns the transformation matrix from a rotation matrix and translation vector 254 | 255 | Args: 256 | R (np.ndarray): the rotation matrix 257 | t (np.ndarray): the translation vector 258 | 259 | Returns: 260 | np.ndarray: the transformation matrix 261 | """ 262 | _check_rotation_matrix(R) 263 | dim = R.shape[0] 264 | assert t.shape in [(dim,), (dim, 1)], f"Translation vector must be of size {dim}" 265 | T = np.eye(dim + 1) 266 | T[:dim, :dim] = R 267 | T[:dim, dim] = t 268 | _check_transformation_matrix(T) 269 | return T 270 | 271 | 272 | def make_transformation_matrix_from_theta( 273 | theta: float, 274 | translation: np.ndarray, 275 | ) -> np.ndarray: 276 | """ 277 | Returns the transformation matrix from theta and translation 278 | 279 | Args: 280 | theta (float): the angle of rotation 281 | translation (np.ndarray): the translation 282 | 283 | Returns: 284 | np.ndarray: the transformation matrix 285 | """ 286 | R = get_rotation_matrix_from_theta(theta) 287 | return make_transformation_matrix(R, translation) 288 | 289 | 290 | #### test functions #### 291 | 292 | 293 | def _check_rotation_matrix(R: np.ndarray, assert_test: bool = False): 294 | """ 295 | Checks that R is a rotation matrix. 296 | 297 | Args: 298 | R (np.ndarray): the candidate rotation matrix 299 | assert_test (bool): if false just print if not rotation matrix, otherwise raise error 300 | """ 301 | d = R.shape[0] 302 | is_orthogonal = np.allclose(R @ R.T, np.eye(d), rtol=1e-3, atol=1e-3) 303 | if not is_orthogonal: 304 | # print(f"R not orthogonal: {R @ R.T}") 305 | if assert_test: 306 | raise ValueError(f"R is not orthogonal {R @ R.T}") 307 | else: 308 | logger.warning(f"R is not orthogonal {R @ R.T}") 309 | 310 | has_correct_det = abs(np.linalg.det(R) - 1) < 1e-3 311 | if not has_correct_det: 312 | # print(f"R det != 1: {np.linalg.det(R)}") 313 | if assert_test: 314 | logger.warning(f"R has incorrect determinant {np.linalg.det(R)}") 315 | print(la.svd(R)) 316 | print(la.eigvals(R)) 317 | print(R) 318 | raise ValueError(f"R det incorrect {np.linalg.det(R)}") 319 | 320 | 321 | def _check_square(mat: np.ndarray): 322 | assert mat.shape[0] == mat.shape[1], "matrix must be square" 323 | 324 | 325 | def _check_symmetric(mat): 326 | assert np.allclose(mat, mat.T) 327 | 328 | 329 | def _check_psd(mat: np.ndarray): 330 | """Checks that a matrix is positive semi-definite""" 331 | assert isinstance(mat, np.ndarray) 332 | assert ( 333 | np.min(la.eigvals(mat)) + 1e-1 >= 0.0 334 | ), f"min eigenvalue is {np.min(la.eigvals(mat))}" 335 | 336 | 337 | def _check_is_laplacian(L: np.ndarray): 338 | """Checks that a matrix is a Laplacian based on well-known properties 339 | 340 | Must be: 341 | - symmetric 342 | - ones vector in null space of L 343 | - no negative eigenvalues 344 | 345 | Args: 346 | L (np.ndarray): the candidate Laplacian 347 | """ 348 | assert isinstance(L, np.ndarray) 349 | _check_symmetric(L) 350 | _check_psd(L) 351 | ones = np.ones(L.shape[0]) 352 | zeros = np.zeros(L.shape[0]) 353 | assert np.allclose(L @ ones, zeros), f"L @ ones != zeros: {L @ ones}" 354 | 355 | 356 | def _check_transformation_matrix( 357 | T: np.ndarray, assert_test: bool = True, dim: Optional[int] = None 358 | ): 359 | """Checks that the matrix passed in is a homogeneous transformation matrix. 360 | If assert_test is True, then this is in the form of assertions, otherwise we 361 | just print out error messages but continue 362 | 363 | Args: 364 | T (np.ndarray): the matrix to test 365 | assert_test (bool, optional): Whether this is a 'hard' test and is 366 | asserted or just a 'soft' test and only prints message if test fails. Defaults to True. 367 | """ 368 | _check_square(T) 369 | matrix_dim = T.shape[0] 370 | if dim is not None: 371 | assert ( 372 | matrix_dim == dim + 1 373 | ), f"matrix dimension {matrix_dim} != dim + 1 {dim + 1}" 374 | 375 | assert matrix_dim in [ 376 | 3, 377 | 4, 378 | ], f"Was {T.shape} but must be 3x3 or 4x4 for a transformation matrix" 379 | 380 | # check that is rotation matrix in upper left block 381 | R = T[:-1, :-1] 382 | _check_rotation_matrix(R, assert_test=assert_test) 383 | 384 | # check that the bottom row is [0, 0, 1] 385 | bottom = T[-1, :] 386 | bottom_expected = np.array([0] * (matrix_dim - 1) + [1]) 387 | assert np.allclose( 388 | bottom.flatten(), bottom_expected 389 | ), f"Transformation matrix bottom row is {bottom} but should be {bottom_expected}" 390 | 391 | 392 | #### print functions #### 393 | 394 | 395 | def _print_eigvals( 396 | M: np.ndarray, name: str = None, print_eigvec: bool = False, symmetric: bool = True 397 | ): 398 | """print the eigenvalues of a matrix""" 399 | 400 | if name is not None: 401 | print(name) 402 | 403 | if print_eigvec: 404 | # get the eigenvalues of the matrix 405 | if symmetric: 406 | eigvals, eigvecs = la.eigh(M) 407 | else: 408 | eigvals, eigvecs = la.eig(M) 409 | 410 | # sort the eigenvalues and eigenvectors 411 | idx = eigvals.argsort()[::1] 412 | eigvals = eigvals[idx] 413 | eigvecs = eigvecs[:, idx] 414 | 415 | print(f"eigenvectors: {eigvecs}") 416 | else: 417 | if symmetric: 418 | eigvals = la.eigvalsh(M) 419 | else: 420 | eigvals = la.eigvals(M) 421 | print(f"eigenvalues\n{eigvals}") 422 | 423 | print("\n\n\n") 424 | 425 | 426 | def _matprint_block(mat, fmt="g"): 427 | col_maxes = [max([len(("{:" + fmt + "}").format(x)) for x in col]) for col in mat.T] 428 | num_col = mat.shape[1] 429 | row_spacer = "" 430 | for _ in range(num_col): 431 | row_spacer += "__ __ __ " 432 | for j, x in enumerate(mat): 433 | if j % 2 == 0: 434 | print(row_spacer) 435 | print("") 436 | for i, y in enumerate(x): 437 | if i % 2 == 1: 438 | print(("{:" + str(col_maxes[i]) + fmt + "}").format(y), end=" | ") 439 | else: 440 | print(("{:" + str(col_maxes[i]) + fmt + "}").format(y), end=" ") 441 | print("") 442 | 443 | print(row_spacer) 444 | print("\n\n\n") 445 | -------------------------------------------------------------------------------- /score/utils/plot_utils.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, Tuple, List, Optional 2 | import numpy as np 3 | import math 4 | import matplotlib.pyplot as plt 5 | import matplotlib.patches as mpatches 6 | import matplotlib.lines as mlines 7 | from matplotlib.colors import to_rgba 8 | from py_factor_graph.factor_graph import FactorGraphData 9 | from py_factor_graph.variables import PoseVariable, LandmarkVariable 10 | from py_factor_graph.utils.solver_utils import SolverResults, VariableValues 11 | from py_factor_graph.utils.matrix_utils import ( 12 | _check_transformation_matrix, 13 | get_theta_from_rotation_matrix, 14 | get_translation_from_transformation_matrix, 15 | ) 16 | from score.utils.circle_utils import Arc, Circle, CircleIntersection, Point 17 | 18 | colors = ["red", "green", "blue", "orange", "purple", "black", "cyan"] 19 | 20 | 21 | def plot_error( 22 | data: FactorGraphData, 23 | solved_results: SolverResults, 24 | initial_values: Optional[VariableValues] = None, 25 | color_dist_circles: bool = False, 26 | ) -> None: 27 | """ 28 | Plots the error for the given data 29 | 30 | Args: 31 | data (FactorGraphData): the groundtruth data 32 | solved_results (Dict[str, Dict[str, np.ndarray]]): the solved values of the variables 33 | initial_values (VariableValues): the initial values of the variables 34 | before solving 35 | color_dist_circles (bool, optional): whether to display the circles 36 | indicating the distance measurements. Defaults to False. 37 | 38 | """ 39 | 40 | def draw_all_information( 41 | ax: plt.Axes, 42 | gt_data: FactorGraphData, 43 | solution_data: SolverResults, 44 | init_vals: Optional[VariableValues] = None, 45 | use_arrows: bool = True, 46 | ): 47 | """Draws the pose estimates and groundtruth 48 | 49 | Args: 50 | ax (plt.Axes): the axes to draw on 51 | gt_data (FactorGraphData): the groundtruth data 52 | solution_results (SolverResults): the solved values of the variables 53 | """ 54 | assert gt_data.num_poses > 0 55 | max_pose_chain_length = max( 56 | [len(pose_chain) for pose_chain in gt_data.pose_variables] 57 | ) 58 | num_landmarks = len(gt_data.landmark_variables) 59 | 60 | true_poses_dict = gt_data.pose_variables_dict 61 | loop_closures = gt_data.loop_closure_measurements 62 | loop_closure_dict = { 63 | x.base_pose: true_poses_dict[x.to_pose] for x in loop_closures 64 | } 65 | 66 | for landmark in gt_data.landmark_variables: 67 | draw_landmark_variable(ax, landmark) 68 | draw_landmark_solution(ax, solution_data.landmarks[landmark.name]) 69 | 70 | pose_var_plot_obj: List[mpatches.FancyArrow] = [] 71 | pose_sol_plot_obj: List[mpatches.FancyArrow] = [] 72 | pose_init_val_plot_obj: List[mpatches.FancyArrow] = [] 73 | range_circles: List[CircleIntersection] = [ 74 | CircleIntersection() for _ in range(num_landmarks) 75 | ] 76 | pose_to_range_measures_dict = gt_data.pose_to_range_measures_dict 77 | 78 | # draw range measurements 79 | range_measure_plot_lines: List[mlines.Line2D] = [] 80 | 81 | cnt = 0 82 | num_frames_skip = 2 83 | for pose_idx in range(max_pose_chain_length): 84 | if cnt % num_frames_skip == 0: 85 | cnt = 0 86 | else: 87 | cnt += 1 88 | continue 89 | 90 | for pose_chain in gt_data.pose_variables: 91 | 92 | if len(pose_chain) == 0: 93 | continue 94 | 95 | # if past end of pose chain just grab last pose, otherwise use 96 | # next in chain 97 | if len(pose_chain) <= pose_idx: 98 | pose = pose_chain[-1] 99 | else: 100 | pose = pose_chain[pose_idx] 101 | 102 | # draw inferred solution 103 | soln_arrow = draw_pose_solution( 104 | ax, 105 | solution_data.poses[pose.name], 106 | ) 107 | pose_sol_plot_obj.append(soln_arrow) 108 | 109 | if init_vals is not None: 110 | # draw the initial point used 111 | init_arrow = draw_pose_solution( 112 | ax, 113 | init_vals.poses[pose.name], 114 | color="green", 115 | alpha=0.5, 116 | ) 117 | pose_init_val_plot_obj.append(init_arrow) 118 | 119 | if pose.name in pose_to_range_measures_dict: 120 | cur_range_measures = pose_to_range_measures_dict[pose.name] 121 | 122 | for range_measure in cur_range_measures: 123 | range_var1, range_var2 = range_measure.association 124 | x1, y1 = solution_data.translations[range_var1] 125 | if "L" == range_var2[0]: 126 | x2, y2 = solution_data.landmarks[range_var2] 127 | else: 128 | x2, y2 = solution_data.translations[range_var2] 129 | 130 | new_line = draw_line(ax, x1, y1, x2, y2, color="red") 131 | range_measure_plot_lines.append(new_line) 132 | 133 | # draw arc to inferred landmarks 134 | for landmark_idx, landmark in enumerate(gt_data.landmark_variables): 135 | soln_pose_center = solution_data.translations[pose.name] 136 | soln_landmark_center = solution_data.landmarks[landmark.name] 137 | range_key = (pose.name, landmark.name) 138 | if color_dist_circles and range_key in pose_to_range_measures_dict: 139 | arc_radius = pose_to_range_measures_dict[range_key] 140 | dist_circle = Circle( 141 | Point(soln_pose_center[0], soln_pose_center[1]), arc_radius 142 | ) 143 | range_circles[landmark_idx].add_circle(dist_circle) 144 | range_circles[landmark_idx].draw_intersection( 145 | ax, color=colors[landmark_idx] 146 | ) 147 | # range_circles[landmark_idx].draw_circles( 148 | # ax, color=colors[landmark_idx] 149 | # ) 150 | 151 | # draw groundtruth solution 152 | var_arrow = draw_pose_variable(ax, pose) 153 | pose_var_plot_obj.append(var_arrow) 154 | 155 | # if loop closure draw it 156 | if pose.name in loop_closure_dict: 157 | loop_line, loop_pose = draw_loop_closure_measurement( 158 | ax, 159 | solution_data.translations[pose.name], 160 | loop_closure_dict[pose.name], 161 | ) 162 | else: 163 | loop_line = None 164 | loop_pose = None 165 | 166 | plt.pause(0.001) 167 | ax.patches.clear() 168 | # print(ax.patches) 169 | # ax.patches = [] 170 | # if loop_line and loop_pose: 171 | # loop_line.remove() 172 | # loop_pose.remove() 173 | 174 | while len(range_measure_plot_lines) > 0: 175 | range_measure_plot_lines.pop().remove() 176 | 177 | # if pose_idx > 5: 178 | # # ax.remove(pose_sol_plot_obj[0]) 179 | # pose_sol_plot_obj[0].remove() 180 | # pose_sol_plot_obj.pop(0) 181 | # pose_var_plot_obj[0].remove() 182 | # pose_var_plot_obj.pop(0) 183 | # if init_vals is not None: 184 | # pose_init_val_plot_obj[0].remove() 185 | # pose_init_val_plot_obj.pop(0) 186 | 187 | plt.close() 188 | 189 | # set up plot 190 | fig, ax = plt.subplots(figsize=(10, 10)) 191 | ax.set_xlim(data.x_min - 1, data.x_max + 1) 192 | ax.set_ylim(data.y_min - 1, data.y_max + 1) 193 | 194 | # draw all poses to view static image result 195 | draw_all_information(ax, data, solved_results) 196 | 197 | 198 | def draw_arrow( 199 | ax: plt.Axes, 200 | x: float, 201 | y: float, 202 | theta: float, 203 | color: str = "black", 204 | ) -> mpatches.FancyArrow: 205 | """Draws an arrow on the given axes 206 | 207 | Args: 208 | ax (plt.Axes): the axes to draw the arrow on 209 | x (float): the x position of the arrow 210 | y (float): the y position of the arrow 211 | theta (float): the angle of the arrow 212 | quiver_length (float, optional): the length of the arrow. Defaults to 0.1. 213 | quiver_width (float, optional): the width of the arrow. Defaults to 0.01. 214 | color (str, optional): color of the arrow. Defaults to "black". 215 | 216 | Returns: 217 | mpatches.FancyArrow: the arrow 218 | """ 219 | plot_x_range = ax.get_xlim()[1] - ax.get_xlim()[0] 220 | plot_y_range = ax.get_ylim()[1] - ax.get_ylim()[0] 221 | 222 | quiver_length: float = max(plot_x_range, plot_y_range) / 20 223 | quiver_width: float = max(plot_x_range, plot_y_range) / 100 224 | dx = quiver_length * math.cos(theta) 225 | dy = quiver_length * math.sin(theta) 226 | return ax.arrow( 227 | x, 228 | y, 229 | dx, 230 | dy, 231 | head_width=quiver_length, 232 | head_length=quiver_length, 233 | width=quiver_width, 234 | color=color, 235 | ) 236 | 237 | 238 | def draw_line( 239 | ax: plt.Axes, 240 | x_start: float, 241 | y_start: float, 242 | x_end: float, 243 | y_end: float, 244 | color: str = "black", 245 | ) -> mlines.Line2D: 246 | """Draws a line on the given axes between the two points 247 | 248 | Args: 249 | ax (plt.Axes): the axes to draw the arrow on 250 | x_start (float): the x position of the start of the line 251 | y_start (float): the y position of the start of the line 252 | x_end (float): the x position of the end of the line 253 | y_end (float): the y position of the end of the line 254 | color (str, optional): color of the arrow. Defaults to "black". 255 | 256 | Returns: 257 | mpatches.FancyArrow: the arrow 258 | """ 259 | line = mlines.Line2D([x_start, x_end], [y_start, y_end], color=color) 260 | ax.add_line(line) 261 | return line 262 | 263 | 264 | def draw_pose_variable(ax: plt.Axes, pose: PoseVariable): 265 | true_x = pose.true_x 266 | true_y = pose.true_y 267 | true_theta = pose.true_theta 268 | return draw_arrow(ax, true_x, true_y, true_theta, color="blue") 269 | 270 | 271 | def draw_pose_solution( 272 | ax: plt.Axes, 273 | pose: np.ndarray, 274 | color: str = "red", 275 | alpha: float = 1.0, 276 | ): 277 | _check_transformation_matrix(pose) 278 | x, y = get_translation_from_transformation_matrix(pose) 279 | theta = get_theta_from_rotation_matrix(pose[0:2, 0:2]) 280 | 281 | coloring = to_rgba(color, alpha) 282 | return draw_arrow(ax, x, y, theta, color=coloring) 283 | 284 | 285 | def draw_loop_closure_measurement( 286 | ax: plt.Axes, base_loc: np.ndarray, to_pose: PoseVariable 287 | ) -> Tuple[mlines.Line2D, mpatches.FancyArrow]: 288 | assert base_loc.size == 2 289 | 290 | x_start = base_loc[0] 291 | y_start = base_loc[1] 292 | x_end = to_pose.true_x 293 | y_end = to_pose.true_y 294 | 295 | line = draw_line(ax, x_start, y_start, x_end, y_end, color="green") 296 | arrow = draw_pose_variable(ax, to_pose) 297 | 298 | return line, arrow 299 | 300 | 301 | def draw_landmark_variable(ax: plt.Axes, landmark: LandmarkVariable): 302 | true_x = landmark.true_x 303 | true_y = landmark.true_y 304 | ax.scatter(true_x, true_y, color="green", marker=(5, 2)) 305 | 306 | 307 | def draw_landmark_solution(ax: plt.Axes, translation: np.ndarray): 308 | x = translation[0] 309 | y = translation[1] 310 | ax.scatter(x, y, color="red", marker=(4, 2)) 311 | 312 | 313 | def draw_arc_patch( 314 | arc: Arc, 315 | ax: plt.Axes, 316 | resolution: int = 50, 317 | color: str = "black", 318 | ) -> mpatches.Polygon: 319 | """Draws an arc as a generic mpatches.Polygon 320 | 321 | Args: 322 | arc (Arc): the arc to draw 323 | ax (plt.Axes): the axes to draw the arc on 324 | resolution (int, optional): the resolution of the arc. Defaults to 325 | 50. 326 | color (str, optional): the color of the arc. Defaults to "black". 327 | 328 | Returns: 329 | mpatches.Polygon: the arc 330 | """ 331 | center = arc.center 332 | radius = arc.radius 333 | 334 | assert arc.thetas is not None 335 | theta1, theta2 = arc.thetas 336 | 337 | # generate the points 338 | theta = np.linspace((theta1), (theta2), resolution) 339 | points = np.vstack( 340 | (radius * np.cos(theta) + center.x, radius * np.sin(theta) + center.y) 341 | ) 342 | # build the polygon and add it to the axes 343 | poly = mpatches.Polygon(points.T, closed=True) 344 | ax.add_patch(poly) 345 | return poly 346 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [options] 2 | install_requires = 3 | attrs 4 | coloredlogs 5 | numpy 6 | gurobipy 7 | 8 | packages = find: 9 | 10 | [metadata] 11 | name = score 12 | version = 0.1.0 13 | description = A second-order conic relaxation of the range-only SLAM problem 14 | author = Alan Papalia 15 | author_email = apapalia@mit.edu 16 | url = https://github.com:MarineRoboticsGroup/score.git 17 | classifiers = 18 | Typing :: Typed 19 | 20 | [options.packages.find] 21 | exclude = examples* 22 | 23 | [options.package_data] 24 | score = py.typed 25 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | sys.path.insert(0, os.path.join(os.path.dirname(__file__), "score")) 5 | 6 | from setuptools import setup 7 | 8 | setup() 9 | --------------------------------------------------------------------------------